MATLAB 代码小结

MATLAB 代码小结

最近用MATLAB写了不少代码，在此小结一下：

1：宏定义真的方便，特别是将参数全部放在一块儿，可以很方便的修改参数来看预期结果。

2：已知一粒子，在其周围九宫格内撒粒子代码

pix_exp = 5;
angle_exp = 5;  
for count_x = -(pix_exp-1)/2 : 1 : (pix_exp-1)/2
    for count_y = -(pix_exp-1)/2 : 1 : (pix_exp-1)/2
        for count_angle = -(angle_exp-1)/2 : 1 : (angle_exp-1)/2
            particle_point = particle_potential + [count_x;count_y;count_angle*angle_resol];
            %如果该点在地图范围内，则撒粒子，否则不撒
            if particle_point(1) < map_x && particle_point(1) > 0 && particle_point(2) < map_y && particle_point(2) > 0 
                P(:,(P_num)) = particle_point;
                P_num = P_num + 1;
            end
        end
    end
end

3:已知一个点，在图像中，以这个点为圆心在图像中画圆

% 计算圆上点的坐标并取整。其中feature_point_x，y为圆心，Dis_corner_feature为半径，count_angle为自定义的角度数组，0到2pi
circle_x = round(feature_point_x(i) + Dis_corner_feature*cos(count_angle(k)));
circle_y = round(feature_point_y(i) + Dis_corner_feature*sin(count_angle(k)));
if circle_x < map_x && circle_x > 0 && circle_y < map_y && circle_y > 0
    count_zero_point(circle_y,circle_x) = count_zero_point(circle_y,circle_x) + 1;
end

4：同3，很多问题都可以通过离散化解决。比如求解三角函数交点时，比如霍夫变换算法，都是通过构建矩阵，离散化角度来实现的。

5：关于蒙特卡罗算法，这个算法虽然很无脑，但对于激光雷达单帧数据和已知全局地图绝对是最有用的，同时此算法还可用于地图的构建，动态定位等，可以说一个算法解决了机器人领域好几个热点问题。缺陷就在于计算量巨大。

6：已知点的坐标和角度，在图中画出带箭头的点：其中mypose(1)是x坐标，mypose(2)是y坐标，mypose(3)是角度信息。其实就是一个圆代表该点，三条线来画出箭头。

px = myPose(1)+20*cos(myPose(3));
py = myPose(2)+20*sin(myPose(3));
plot(myPose(1),myPose(2),'og');
plot([myPose(1) px],[myPose(2) py],'-r');
plot([px px+10*cos(myPose(3) - 2*pi/3)],[py py+10*sin(myPose(3) - 2*pi/3)],'-r');
plot([px px+10*cos(myPose(3) + 2*pi/3)],[py py+10*sin(myPose(3) + 2*pi/3)],'-r');

7：最小二乘法，输入x,y坐标数组，输出最小二乘拟合的直线y=a*x+b,并且输出最大和最小的x的值，并指出其在数组中的位置。

%least squares 
function least_squares_result = least_squares_point(x,y)
point_num = length(x);

x_min = x(1);x_max = x(1);
x2 = 0;x1 = 0;x1y1 = 0;y1 = 0;point_min = 0;point_max = 0;
for j = 1 : 1 : point_num
    if x(j) < x_min
        x_min = x(j);
        point_min = j;
    end
    if x(j) > x_max
        x_max = x(j);
        point_max = j;
    end       
    x2 = x2 + x(j)^2;   % 求Σ(xi^2)
    x1 = x1 + x(j);
    x1y1 = x1y1 + x(j)*y(j);
    y1 = y1 + y(j);
end
%解出直线y=ax+b斜率a=(n*x1y1-x1*y1)/(n*x2-x1*x1)
a=(point_num*x1y1-x1*y1)/(point_num*x2-x1*x1); 
%解出直线截距b=(y1-a*x1)/n
b=(y1-a*x1)/point_num;                    
%     py=a*px+b;
%     plot(px,py,'-g');
result = [a,b,x_min,x_max,point_min,point_max];
least_squares_result = result;