目标定位基于matlab扩展卡尔曼算法SLAM（运动轨迹+误差 ）含Matlab源码 1637期

Posted 2022-01-15 紫极神光

一、获取代码方式

获取代码方式1：
完整代码已上传我的资源： 【目标定位】基于matlab扩展卡尔曼算法SLAM（运动轨迹+误差 ）【含Matlab源码 1637期】

获取代码方式2：
通过订阅紫极神光博客付费专栏，凭支付凭证，私信博主，可获得此代码。

备注：订阅紫极神光博客付费专栏，可免费获得1份代码（有效期为订阅日起，三天内有效）；

二、EKF-SLAM步骤简介

1 运动更新
运动更新时，固定点坐标不变，所以只需要根据运动方程更新(x,y,α)及其协方差与互协方差即可。

2 观测到曾经观测过的固定点
这时候依次对观测到的特征点信息进行EKF更新

Landmark observations are processed in the EKF usually one-by-one

3 观测到新的固定点
这时候观测到新的固定点，需要进行状态增广。根据逆观测方程，使用观测信息推测出新加的增广状态均值与方差，然后加入到总体的状态与协方差矩阵中。

三、部分源代码

    % I. 初始化
%
disp('EKF-2D-SLAM sample program start!!')
% 运动噪声
q = [0.01;0.01];
Q = diag(q.^2);
% 测量噪声
m = [.15; 1*pi/180];
M = diag(m.^2);

% R: 机器人初始位置
% u: 控制量
R = [0;-2.2;0];
u = [0.1;0.05];

% 设置外界路标点环境
% 环形摆放的landmarks
% W: 设置所有路标点位置
jiaodu_perLandMark =6;  %取1,3,6,15,30,60...(360的倍数均可)
r1=2;
r2=3;
r3=3.5;
W = landmarks(r1,r2,r3,jiaodu_perLandMark);

% 传感器探测半径
sensor_r = 2.5;

% Id容器用来判别当前探测到的路标点曾经是否被观测过；若没有观测过，那么此时需要将其加入Id容器。
% 这里使用W中每个点的索引作为路标点的id；Id初始化为一个足够大的零数组即可。
% Id(类型)==1，表示曾经观测过；Id(类型)==0，表示曾经没有观测过。
% 如果用c++实现，建议使用map结构。
Id = zeros(1,size(W,2));

% y_news表示当前新探测到的路标点，y_news(:,i)记录观测量和路标点类型
% 同理y_olds
y_olds = zeros(3,size(W,2));
y_news = zeros(3,size(W,2));

%   状态量及协方差初始化
x = zeros(numel(R)+numel(W), 1);
P = zeros(numel(x),numel(x));

% id_to_x_map：id------>>>id对应的状态变量在x中的位置
id_to_x_map = zeros(1,size(W,2));

% x和P初始化
r = [1 2 3];
x(r) = R;
%x(r) = [8;-2.5;0];
P(r,r) = 0;

% 每次状态增广在x中的位置
s = [4 5];

%主循环次数
% 125/每圈
 loop =250;
 
% 存放位姿仿真量
poses_ = zeros(3,loop);

% 存放位姿历史估计量
poses = zeros(3,loop);

 %  绘图
mapFig = figure(1);
cla;
axis([-5 5 -5 5])
axis square
%axis equal
% 所有路标点
WG = line('parent',gca,...
    'linestyle','none',...
    'marker','.',...
    'color','m',...
    'xdata',W(1,:),...
    'ydata',W(2,:));
% 仿真下机器人位置
RG = line('parent',gca,...
    'marker','+',...
    'color','r',...
    'xdata',R(1),...
    'ydata',R(2));
% 估计的机器人位置
rG = line('parent',gca,...
    'linestyle','none',...
    'marker','+',...
    'color','b',...
    'xdata',x(r(1)),...
    'ydata',x(r(2)));
% 估计的路标点位置
lG = line('parent',gca,...
    'linestyle','none',...
    'marker','+',...
    'color','k',...
    'xdata',[],...
    'ydata',[]);

% 估计的路标点协方差
eG1 = zeros(1,size(W,2));
for i = 1:numel(eG1)
    eG1(i) = line(...
        'parent', gca,...
        'color','k',...
        'xdata',[],...
        'ydata',[]);
end

% 估计的机器人位置
reG = line(...
    'parent', gca,...
    'color','r',...
    'xdata',[],...
    'ydata',[]);

% 传感器探测范围（以真实位置为圆心）
sensor1 = line(...
    'parent', gca,...
    'color','m',...
    'xdata',[],...
    'ydata',[],...
    'LineStyle','--');
sensor2 = line(...
    'parent', gca,...
    'color','m',...
    'xdata',[],...
    'ydata',[],...
    'LineStyle','--');

%传感器探测范围（以估计位置为圆心）
Sensor1 = line(...
    'parent', gca,...
    'color','m',...
    'xdata',[],...
    'ydata',[],...
     'LineStyle','--');
 Sensor2 = line(...
    'parent', gca,...
    'color','m',...
    'xdata',[],...
    'ydata',[],...
     'LineStyle','--');

 true_pose = line(...
    'parent', gca,...
    'color','r',...
    'xdata',[],...
    'ydata',[],...
    'LineWidth',0.8);
     %'LineStyle','--');
 
 estimate_pose = line(...
    'parent', gca,...
    'color','b',...
    'xdata',[],...
    'ydata',[],...
    'LineWidth',0.8);
    % 'LineStyle','--');
 
 % II. 主循环；
 % 机器人每前进一步，循环一次
for t = 1:loop
%     if t == 125
%         u(1) = 0.2;
%         sensor_r = 4;
%     end
%     if t == 375
%         u(1) = 0.2;
%         sensor_r = 5;
%     end
    %不同探测半径
%      if t == 200
%           sensor_r = 1;         
%      end
%      if t == 400
%          sensor_r =1.5; 
%      end 
%      if t == 600
%         sensor_r =2; 
%      end 
%      if t == 800
%         sensor_r =2.5; 
%      end 
%      if t == 1000
%          sensor_r = 3;         
%      end    

四、运行结果

五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 沈再阳.精通MATLAB信号处理[M].清华大学出版社，2015.
[2]高宝建,彭进业,王琳,潘建寿.信号与系统——使用MATLAB分析与实现[M].清华大学出版社，2020.
[3]王文光,魏少明,任欣.信号处理与系统分析的MATLAB实现[M].电子工业出版社，2018.