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

Ps.729

已于 2024-07-02 18:31:05 修改

阅读量756

点赞数 25

文章标签：前端

于 2024-07-02 18:30:27 首次发布

本文链接：https://blog.csdn.net/weixin_67304359/article/details/139959687

版权

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

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

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

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

目录

⛳️赠与读者

💥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 维平面上的自主移动车，其前轮可

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

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

辆状态根据运动学模型定义为 x v = [ 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');