基于不变扩展卡尔曼滤波器RI-EKF的同时定位与地图构建SLAM算法的收敛性和一致性特性研究（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

基于不变扩展卡尔曼滤波器（RI-EKF）的SLAM算法收敛性与一致性特性研究

一、RI-EKF的基本原理与改进机制

不变扩展卡尔曼滤波器（RI-EKF）是传统扩展卡尔曼滤波器（EKF）的改进版本，其核心在于利用李群理论重新定义状态误差和观测误差，从而克服传统EKF在非线性系统中的局限性。RI-EKF的特点包括：

1. 误差定义的几何适应性：采用右不变误差（Right-Invariant Error）代替线性误差，使得误差传递方程与状态估计值独立。
2. 系统矩阵的恒定性：RI-EKF的系统矩阵FF为常量，与输入和当前状态无关，避免了传统EKF因状态依赖导致的线性化误差累积问题。
3. 协方差更新的优化：通过伴随矩阵处理噪声项，简化积分过程，并确保协方差更新的稳定性。

二、收敛性特性分析

1. 收敛性定义与保证 SLAM算法的收敛性指状态估计随时间推移逐渐逼近真实值的能力。RI-EKF的收敛性优势体现在以下方面：

• 无需地面真值假设：传统EKF需在真实状态处评估雅可比矩阵，而RI-EKF的收敛性证明无需此类限制性假设。
• 全局收敛特性：RI-EKF在任意随机刚体变换下保持输出不变性（Invariance），避免因坐标系变换导致的误差发散。
• 蒙特卡洛仿真验证：在3D点特征SLAM中，RI-EKF的位置误差（RMSE=0.25米）和旋转误差（0.0058弧度）均显著优于SO(3)-EKF和FEJ-EKF。
  
  编辑
  
  编辑

2. 收敛性提升的机理

• 李群表示：姿态和位置的状态空间采用李群结构，避免欧拉角奇异性问题，增强数值稳定性。
• 线性化误差抑制：通过不变误差模型，减少线性化近似误差对状态传递的影响，从而抑制正反馈导致的发散。

三、一致性特性分析

1. 一致性的定义与挑战 一致性要求算法提供无偏估计，并正确反映估计的不确定性（协方差）。传统EKF因线性化误差和协方差低估导致过于自信的估计（Over-confidence）。RI-EKF通过以下机制改进一致性：

• 不变观测模型：基于几何自适应校正项更新增益矩阵，避免线性误差导致的协方差低估。
• 协方差闭合性：RI-EKF的协方差方程在更广泛的轨迹集上收敛至常数值，而非仅在平衡点附近。

2. 实验验证

• NEES指标：RI-EKF的归一化估计误差平方（NEES）在99%置信区间内，而传统EKF常超出3σ边界，表明其一致性更优。
• 不变性对一致性的影响：随机刚体变换下的输出不变性确保算法对外部扰动鲁棒，减少地图构建中的累积漂移（Drift）。

四、RI-EKF与传统EKF的性能对比

实验数据支持：在噪声水平为1%时，RI-EKF的位置误差比传统EKF降低30%以上，旋转误差降低40%。

五、实际应用案例与挑战

1. 无人机导航：RI-EKF在GPS和MEMS-AHRS融合中表现出更高的稳定性，卡尔曼增益和协方差动态更平稳。
2. 动态环境SLAM：RI-EKF在动态目标追踪中通过不变性减少闭环检测失败率，提升地图一致性。
3. 挑战与局限性：

• 计算负载：李群运算增加实时性负担，需硬件加速优化。
• 初始状态敏感性：初始误差较大时，收敛速度可能受限。

六、未来研究方向

1. 算法融合：结合因子图优化（Factor Graph）提升大规模环境下的效率。
2. 自适应噪声模型：动态调整过程噪声协方差以应对复杂环境变化。
3. 多传感器扩展：融合视觉、LiDAR等多模态数据，提升不变性适用范围。

七、结论

RI-EKF通过几何误差定义和李群理论，显著提升了SLAM算法的收敛性和一致性，尤其在动态环境和高噪声场景中表现优异。其核心优势在于系统矩阵的恒定性与协方差更新的鲁棒性，使其在3D SLAM中成为传统EKF的有效替代方案。然而，计算复杂度和初始状态敏感性仍需进一步优化，未来研究可结合深度学习与硬件加速技术推动实际应用。

📚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 参考文献

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