基于不变扩展卡尔曼滤波器RI-EKF的同时定位与地图构建SLAM算法的收敛性和一致性特性研究（Matlab代码实现）

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

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

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

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

目录

 ⛳️赠与读者

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、数据、文章

---

 ⛳️赠与读者

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

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

💥1 概述

摘要——本文探讨了基于不变扩展卡尔曼滤波器（RI-EKF）的同时定位与地图构建（SLAM）算法的收敛性和一致性特性。证明了该算法的基本收敛性质，这些证明不需要在运动和观测模型的雅可比矩阵处于地面真实值时进行限制性假设。同时指出，与仅在确定性刚体变换下不变的基于SO(3)的EKF SLAM算法（SO(3)-EKF）相比，RI-EKF的输出在任意随机刚体变换下均保持不变。还讨论了这些不变性质对估计器一致性的影响。蒙特卡罗仿真结果表明，RI-EKF在基于3D点特征的SLAM中优于SO(3)-EKF、Robocentric-EKF和“第一估计雅可比矩阵” EKF算法。

关键词——定位、地图构建、SLAM

扩展卡尔曼滤波器（EKF）在过去广泛应用于解决同时定位与地图构建（SLAM）问题。然而，传统基于EKF的点特征SLAM的一个局限性是可能的估计器不一致性。这里的不一致性指的是算法低估了估计的不确定性，导致结果过于自信。早在2001年[1]就已经意识到这个问题，并在后续的文献[2][3]中进行了详细讨论。文献中也有一些研究旨在增强EKF SLAM的一致性。例如，Robocentric EKF SLAM [4]在机器人局部坐标系中估计地标的位置。因此，尽管地标在固定的全局坐标系中是静止的，但它们的位置估计会不断变化。然而，研究表明，这种以机器人为中心的表述在估计器的一致性方面表现更好。Guerreiro等人[5]还报告了一个在机器人中心坐标系中制定的SLAM问题的卡尔曼滤波器。此外，在文献[6]中还指出，EKF SLAM中的不一致性与SLAM问题的部分可观测性[7][8]密切相关。这一见解促使开发了许多显著提高一致性的EKF SLAM算法，例如“第一估计雅可比矩阵” EKF SLAM [6]、可观测性约束的EKF SLAM [9][10]。

📚2 运行结果

部分代码：

[REKF_RMS_ave, REKF_NEES_ave] = compute_rms_nees_ave( REKF_RMS, REKF_NEES );
[FEKF_RMS_ave, FEKF_NEES_ave] = compute_rms_nees_ave( FEKF_RMS, FEKF_NEES );
[RocEKF_RMS_ave, RocEKF_NEES_ave] = compute_rms_nees_ave( RocEKF_RMS, RocEKF_NEES );
[EKF_RMS_ave, EKF_NEES_ave] = compute_rms_nees_ave( EKF_RMS, EKF_NEES );

figure;
%subplot(2,2,1);
plot(1:size(REKF_RMS_ave.position, 2), REKF_RMS_ave.position, 'r'); hold on;
plot(1:size(FEKF_RMS_ave.position, 2), FEKF_RMS_ave.position, 'g'); hold on;
plot(1:size(EKF_RMS_ave.position, 2), EKF_RMS_ave.position, 'k'); hold on;
plot(1:size(RocEKF_RMS_ave.position, 2), RocEKF_RMS_ave.position, 'b'); hold on;

legend('R-EKF', 'FEJ-EKF', 'EKF', 'Roc-EKF' ,'Location','northeast');
title('RMS:position(meter)');xlim([0,size(REKF_RMS_ave.position, 2)]);

figure;
plot(1:size(REKF_RMS_ave.orientation, 2), REKF_RMS_ave.orientation, 'r'); hold on;
plot(1:size(FEKF_RMS_ave.orientation, 2), FEKF_RMS_ave.orientation, 'g'); hold on;
plot(1:size(EKF_RMS_ave.orientation, 2), EKF_RMS_ave.orientation, 'k'); hold on;
plot(1:size(RocEKF_RMS_ave.orientation, 2), RocEKF_RMS_ave.orientation, 'b'); hold on;

legend('R-EKF', 'FEJ-EKF',  'EKF', 'Roc-EKF', 'Location','northeast');
title('RMS:orientation(rad)');xlim([0,size(REKF_RMS_ave.orientation, 2)]);

figure;
semilogy(1:size(REKF_NEES_ave.orientation, 2), REKF_NEES_ave.orientation, 'r'); hold on;
semilogy(1:size(FEKF_NEES_ave.orientation, 2), FEKF_NEES_ave.orientation, 'g'); hold on;
semilogy(1:size(EKF_NEES_ave.orientation, 2), EKF_NEES_ave.orientation, 'k'); hold on;
semilogy(1:size(RocEKF_NEES_ave.orientation, 2), RocEKF_NEES_ave.orientation, 'b'); hold on;

legend('R-EKF', 'FEJ-EKF', 'EKF', 'Roc-EKF', 'Location','northeast');
title('NEES:orientation');xlim([0,size(REKF_NEES_ave.orientation, 2)]);

figure;
% semilogy(1:size(REKF_NEES_ave.pose, 2), REKF_NEES_ave.pose, 'r'); hold on;
% semilogy(1:size(FEKF_NEES_ave.pose, 2), FEKF_NEES_ave.pose, 'g'); hold on;
% semilogy(1:size(EKF_NEES_ave.pose, 2), EKF_NEES_ave.pose, 'k'); hold on;
% semilogy(1:size(RocEKF_NEES_ave.pose, 2), RocEKF_NEES_ave.pose, 'b'); hold on;

plot(1:size(REKF_NEES_ave.pose, 2), REKF_NEES_ave.pose, 'r-'); hold on;
plot(1:size(FEKF_NEES_ave.pose, 2), FEKF_NEES_ave.pose, 'g+'); hold on;
plot(1:size(EKF_NEES_ave.pose, 2), EKF_NEES_ave.pose, 'ko'); hold on;
plot(1:size(RocEKF_NEES_ave.pose, 2), RocEKF_NEES_ave.pose, 'b'); hold on;
plot(1:size(RocEKF_NEES_ave.pose, 2), 1.12*ones( 1,size(RocEKF_NEES_ave.pose, 2)  ), 'r--'); hold on;
plot(1:size(RocEKF_NEES_ave.pose, 2), 0.89*ones( 1,size(RocEKF_NEES_ave.pose, 2)  ), 'r--'); hold on;
hleg=legend('R-EKF', 'FEJ-EKF', 'SO(3)-EKF', 'Robocentric-EKF', '95% confidence bound' , 'Location','northwest');
%hleg=legend('R-EKF', '95% confidence bound' , 'Location','northwest');
set(hleg,'FontSize',18)
ylabel('NEES of robot pose','fontsize',18);
xlabel('Time steps','fontsize',18);

xlim([0,size(REKF_NEES_ave.pose, 2)]);
title('NEES:pose');

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

🌈4 Matlab代码、数据、文章

资料获取，更多粉丝福利，MATLAB|Simulink|Python资源获取