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

一、获取代码方式

获取代码方式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

  

 

  
1
 

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    

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
  
23
  
24
  
25
  
26
  
27
  
28
  
29
  
30
  
31
  
32
  
33
  
34
  
35
  
36
  
37
  
38
  
39
  
40
  
41
  
42
  
43
  
44
  
45
  
46
  
47
  
48
  
49
  
50
  
51
  
52
  
53
  
54
  
55
  
56
  
57
  
58
  
59
  
60
  
61
  
62
  
63
  
64
  
65
  
66
  
67
  
68
  
69
  
70
  
71
  
72
  
73
  
74
  
75
  
76
  
77
  
78
  
79
  
80
  
81
  
82
  
83
  
84
  
85
  
86
  
87
  
88
  
89
  
90
  
91
  
92
  
93
  
94
  
95
  
96
  
97
  
98
  
99
  
100
  
101
  
102
  
103
  
104
  
105
  
106
  
107
  
108
  
109
  
110
  
111
  
112
  
113
  
114
  
115
  
116
  
117
  
118
  
119
  
120
  
121
  
122
  
123
  
124
  
125
  
126
  
127
  
128
  
129
  
130
  
131
  
132
  
133
  
134
  
135
  
136
  
137
  
138
  
139
  
140
  
141
  
142
  
143
  
144
  
145
  
146
  
147
  
148
  
149
  
150
  
151
  
152
  
153
  
154
  
155
  
156
  
157
  
158
  
159
  
160
  
161
  
162
  
163
  
164
  
165
  
166
  
167
  
168
  
169
  
170
  
171
  
172
  
173
  
174
  
175
  
176
  
177
  
178
  
179
  
180
  
181
  
182
  
183
  
184
  
185
  
186
 

四、运行结果

五、matlab版本及参考文献

1 matlab版本
2014a

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

文章来源: qq912100926.blog.csdn.net，作者：海神之光，版权归原作者所有，如需转载，请联系作者。

原文链接：qq912100926.blog.csdn.net/article/details/122139363