稀疏 landmark 与稠密 landmark 下 EKF SLAM 性能对比实验，预测更新同时进行与非同时进行对比 EKF SLAM 性能对比实验，EKF SLAM 在有色噪声下性能实验

程序猿鑫

于 2024-07-02 18:33:05 发布

阅读量726

点赞数 11

文章标签： matlab

本文链接：https://blog.csdn.net/Ke_Yan_She/article/details/140133921

版权

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

⛳️赠与读者

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、数据、文档讲解

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学，什么是电的时候，不要觉得这些问题搞笑。哲学是科学之母，哲学就是追究终极问题，寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能让人胸中升起一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它居然给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

本篇报告全面说明了 KF，EKF，IEKF 算法的基本原理；构建推导出四轮前驱自主移动机器人的运动学模型和观测模型；详尽全面的解释推导了实验所中使用的 EKF SLAM 算法的系统模型、预测过程、过修正程、已知知 ID 和未知 ID 数据关联算法及状态向量增广的详细过程，实验内容包括数据生成实验，EKF SLAM 预测与单纯模型预测对比实验，稀疏 landmark 与稠密 landmark 下 EKF SLAM 性能对比实验，预测更新同时进行与非同时进行对比 EKF SLAM 性能对比实验，EKF SLAM 在有色噪声下性能实验，IEKFSLAM 与 EKF SLAM 在高斯噪声和有色噪声下性能对比。

EKF 全称 Extended Kalman Filter，即扩展卡尔曼滤波器，一种高效率的递归滤波器 (自

回归滤波器)。基本的卡尔曼滤波器在线性系统中有着良好的性能，但在非线性系统中效果不

理想，一般适用于线性系统。扩展卡尔曼滤波器通过对非线性系统进行线性化使得卡尔曼滤

波器在非线性系统中也可以有良好的性能。

基本的 EKF 算法假设噪声形式为高斯噪声，但实际情况下噪声形式往往为有色噪声形

式，并且 EKF 算法的线性化工作点往往不是输入状态真实的均值，而是一个估计的均值，这

样的偏差会导致计算出的雅可比矩阵也不是最好的，且线性化过程丢弃了许多高阶项，这会

导致 EKF 算法的性能下降，甚至导致滤波器发散。迭代扩展卡尔曼滤波算法 IEKF 能有效

的改善这一问题，个人理解其基本的思想与残差逼近类似。IEKF 会将上一步修正后的输出

作为下一次迭代修正误差的输入值，通过不断迭代来修正误差减小误差，具体的 IEKF 算法

的步骤可表达如下

本次实验所使用的移动机器人模型如图3所示，为 2 维平面上的自主移动车，其前轮可

以进行转向操作，后轮为固定的驱动轮，假设车身中心装有传感器（如激光、声纳、里程计

等），传感器固联坐标系与自主移动车辆的本体系一致，可对路标进行观测获得传感数据。车

辆状态根据运动学模型定义为 xv = [x, y, φ] T，控制输入量为舵角 γ 和速度 V ，相当于实际车

辆的方向盘转角和油门。此模型符合实际的汽车模型，因而具有一定的实用性。

详细文档见第4部分。

📚2 运行结果

运行视频：

链接：https://pan.baidu.com/s/1b99NJSsJyKaGJgcA8GH0vg
提取码：feun
--来自百度网盘超级会员V5的分享

部分代码：

% 获取控制量
Vn = states(k).Vn;
Gn = states(k).Gn;

if ASYNCHRONOUS == 0
% EKF更新状态预测值和协方差
[x,P] = EKF_predict (x,P, Vn,Gn,QE, WHEELBASE,dt);
% 获取仅通过模型预测的位姿
x_model_pre = vehicle_model(x_model_pre, Vn,Gn, WHEELBASE,dt);
end

if states(k).observation_update == 1

if ASYNCHRONOUS == 1
% EKF更新状态预测值和协方差
[x,P] = EKF_predict (x,P, Vn,Gn,QE, WHEELBASE,dt);
% 获取仅通过模型预测的位姿
x_model_pre = vehicle_model(x_model_pre, Vn,Gn, WHEELBASE,dt);
end
% 获取观测值
z = states(k).zn;
ftag_visible = states(k).ftag_visible;

if REDUCE_OB_FEATURES == 1
% 削减观测到的landmark数目
if size(z,2) > 1
z = z(:,1);
ftag_visible = ftag_visible(1);
end
end

% 数据关联
if SWITCH_ASSOCIATION_KNOWN == 1
[zf,idf,zn, da_table]= data_associate_known(x,z,ftag_visible, da_table);
else
[zf,idf, zn]= data_associate(x,P,z,RE, GATE_REJECT, GATE_AUGMENT);
end

% 更新状态向量
if SWITCH_USE_IEKF == 1
[x,P]= update_iekf(x,P,zf,RE,idf, 5);
else
[x,P]= EKF_update(x,P,zf,RE,idf, 1);
end

% 添加新的landmark到状态向量中
[x,P]= augment(x,P, zn,RE);
end

xtrue = states(k).xtrue;
iwp = states(k).next_keypoint;

% 清除图像
cla;
axis equal

ture_trajectory(:,k) = xtrue(1:3);
model_pre_trajectory(:,k) = x_model_pre(1:3);
EKF_pre_trajectory(:,k) = x(1:3);

% 画出历史轨迹
plot( ture_trajectory(1, 1:k), ture_trajectory(2, 1:k), 'k--','linewidth',3);

% 画出历史EKF预测轨迹
plot( EKF_pre_trajectory(1, 1:k), EKF_pre_trajectory(2, 1:k), 'r','linewidth',3 );

% 画出历史model预测轨迹
plot( model_pre_trajectory(1, 1:k), model_pre_trajectory(2, 1:k), 'b-.','linewidth',3);

% 画出landmarks
scatter( landmarks(1, :), landmarks(2, :), 'b*' );

% 画出路径关键点
plot( wp(1,:), wp(2, :), 'r.','markersize',26 );

% 画出目标点的位置
if iwp~=0
plot(wp(1,iwp),wp(2,iwp),'bo','markersize',13,'linewidth',1);
end

% 画出车的位姿
draw_car(xtrue,5,'k');