0. 论文信息
标题：Affine EKF: Exploring and Utilizing Sufficient and Necessary Conditions for Observability Maintenance to Improve EKF Consistency
作者：Yang Song, Liang Zhao, Shoudong Huang
机构：University of Technology Sydney、The University of Edinburgh
原文链接：https://arxiv.org/abs/2412.10809
代码链接：https://github.com/yangsong-slam/affine-ekf
1. 导读
不一致性问题是基于扩展卡尔曼滤波(EKF)的状态估计方法性能的一个关键挑战，它主要受EKF模型和底层动态系统之间可观测性差异的影响。本文首先证明了可观测性保持的一些充要条件。我们发现，在一定条件下，如果相应的线性化使得不可观测子空间与状态值无关，EKF自然可以保持正确的可观测性。基于这一理论发现，提出了一种新的仿射EKF (Aff-EKF)框架，通过仿射变换来克服标准EKF (Std-EKF)的不一致性，该框架不仅自然地满足可观测性约束，而且具有清晰的设计过程。通过数学分析证明了我们的Aff-EKF框架相对于一些常用方法的优势。我们提出的方法的有效性在具有不同类型的特征、典型的点特征、水平面上的点特征和平面特征的三个同时定位和绘图(SLAM)应用上得到验证。具体地说，按照提出的方法，自然相容的Aff-EKF可以显式地导出。蒙特卡洛模拟验证了这些Aff-EKF的一致性改进。
2. 效果展示
Aff-EKF可以显著提高SLAM的定位精度
Std-EKF、RI-EKF和两个提出的Aff-EKF的RMSE和NEES也如图2所示。在整个轨迹中，RI-EKF和两个Aff-EKF都保持了一致性，而Std-EKF在一段时间后变得明显不一致。特别地，图2还展示了在一次蒙特卡洛模拟中，这四个EKF的机器人方向误差和位置误差，以及3σ边界。我们可以发现，Std-EKF的误差超出了相应的3σ边界（99%范围），这清楚地揭示了Std-EKF的不一致性问题。
3. 引言
对于非线性动态系统而言，状态估计备受关注。尽管可以使用所有测量数据进行离线优化来获得非常准确的估计，但状态规模的不断增长阻碍了实时计算。在在线计算技术中，扩展卡尔曼滤波器（EKF）仍然是机器人领域中最受欢迎的算法之一。然而，EKF性能的一个关键挑战是不一致性问题，即不确定性被低估。根据对EKF不一致性的研究，可观测性约束的违反是导致此类问题的主要因素。对于那些部分可观测的非线性动态系统，例如同时定位与地图构建（SLAM），标准EKF（Std-EKF）模型的可观测方向可能比底层非线性系统少。沿着缺失的不可观测方向产生的虚假信息增益最终会产生过于自信的结果，从而导致估计结果不佳。Std-EKF的这一局限性不仅在各种SLAM任务中得到了体现，而且还体现在其他应用中，如腿式机器人的状态估计、弹道轨迹估计和基于地图的定位。
为了解决不一致性问题，目前主要有两类方法：一类以首次估计雅可比EKF（FEJ-EKF）或可观测性约束EKF（OC-EKF）为代表；另一类以不变EKF（I-EKF）为代表。上述方法的EKF模型保持了正确的可观测性属性，即它们与底层动态系统具有相同的可观测性维度。具体来说，为了满足这一可观测性约束，FEJ-EKF和OC-EKF中的雅可比矩阵不是在更新后的估计值上评估的，而是使用人为调整的值。这可能会导致准确性和收敛性方面的性能不佳[10]。相比之下，I-EKF采用了由特定李群结构生成的特定线性化，因此它们自然满足可观测性约束。I-EKF中的所有项都是在当前更新后的估计值上评估的，而不是任何人为调整的值，因此I-EKF通常比FEJ-EKF和OC-EKF表现更好。
对于任何一般的非线性动态系统，我们希望找到一种类似于I-EKF中的线性化方法，基于该方法，EKF可以自然满足可观测性约束。然而，最近的I-EKF框架需要一种特殊的李群结构，这可能很难找到。在特定问题上设计I-EKF所采用的那些线性化似乎是经验性的。据我们所知，现有文献没有为一般状态估计问题提供找到这种特定线性化的明确方法。
推荐课程：
彻底剖析激光-视觉-IMU-GPS融合SLAM算法：理论推导、代码讲解和实战
。
本文旨在通过提供一个清晰的过程来填补这一空白，即为状态估计问题设计一个理论上合理的、可保持可观测性的EKF。本文的灵感来源于我们的发现，即在某些问题中，Std-EKF的可观测性差异可能是由不可观测子空间对状态的依赖性引起的。如果这一属性对一般问题都成立，那么
是否意味着只要我们设计的线性化消除了这种依赖性，相应的EKF就可以自然满足可观测性约束？
为了回答这个问题，我们进一步从不可观测子空间对状态的依赖性角度证明了可观测性保持的一些充分必要条件。利用这些新证明的基于可观测性的一致性理论，我们开发了一个框架，称为仿射EKF（Aff-EKF），它通过对其线性化执行特定的仿射变换来修正不一致的Std-EKF，以保持正确的可观测性属性。这是一种将任何Std-EKF转换为自然保持正确可观测性的通用方法，并提供了详细的技术过程。更有趣的是，通过比较Aff-EKF和Std-EKF的公式，我们发现Std-EKF线性化上的那些仿射变换相当于修正了其协方差矩阵的计算。这直观地展示了Aff-EKF如何解决Std-EKF的不一致性问题。
4. 主要贡献
具体而言，本文的主要贡献包括：
• 我们深入研究了EKF能够自然满足可观测性约束的条件。具体而言，从EKF的可观测性差异与不可观测子空间对状态的依赖性之间的关系出发，证明了一些充分必要条件。这些新证明的可观测性定理有助于我们确定哪种线性化方法可以使EKF满足可观测性约束。
• 提出了一种新的框架，称为Aff-EKF，用于设计可保持可观测性的估计器，该估计器通过适当的仿射变换来修正不一致的Std-EKF。基于已证明的可观测性保持的充分必要条件，提供了设计此类适当仿射变换的通用过程。
• 我们通过三种具有不同类型特征的SLAM应用验证了Aff-EKF框架的有效性：典型的点特征、水平面上的点特征和平面特征。此外，我们还从理论上分析了与文献中方法相比，本文所提框架的优势。我们在三个具有不同类型特征的SLAM应用中展示了Aff-EKF方法：典型的点特征、水平面上的点特征和平面特征。对于每个应用，以下小节将依次介绍相应的问题公式化、可观测性分析、Aff-EKF的实现和计算复杂度
5. 改进SLAM
对于基于典型点特征的SLAM（即时定位与地图构建），我们比较了各种EKF（扩展卡尔曼滤波）框架。相应的EKF算法包括Std-EKF（标准扩展卡尔曼滤波）、FEJ-EKF、OC-EKF、FEJ2-EKF、DRI-EKF、RI-EKF，以及我们提出两个版本的Aff-EKF（仿射扩展卡尔曼滤波）。
不同噪声水平下整个轨迹的平均RMSE（均方根误差）和NEES（归一化估计误差平方）如表II所示。结果表明，除Std-EKF外，所有EKF方法均能产生一致的估计。然而，RI-EKF和我们提出的两个Aff-EKF在准确性方面通常优于其他方法。
综上所述，上述实验表明，我们提出的框架极大地提高了Std-EKF的性能，并且相应的两个Aff-EKF达到了与RI-EKF相当的性能。
在本部分中，我们针对表I描述的环境2和环境3中的基于约束点特征的SLAM问题，将我们的Aff-EKF与Std-EKF和Ideal-EKF（Std-EKF的一种变体，其中雅可比矩阵在真实值处评估）进行了比较。此外，我们还比较了典型点SLAM的RI-EKF，因为对于这种约束点情况，目前没有可用的I-EKF。为了执行此操作，该RI-EKF忽略了点特征上的平面约束。
场景1和场景2下不同噪声水平的平均RMSE和NEES分别列在表III和表IV中。特别地，在噪声水平为（0.005, 0.01, 0.1）时，图3和图4还分别展示了场景1（高度已知）和场景2（高度未知）中一次模拟的机器人估计的RMSE和NEES，以及3σ边界。
在表III和表IV中，RI-EKF、Ideal-EKF和我们提出的Aff-EKF的平均NEES值都非常接近1，而Std-EKF的平均NEES值明显大于1。图3和图4还显示，除了标准EKF外，这些方法的NEES值在整个轨迹中大约都为1。我们还可以发现，Std-EKF的机器人误差超出了相应的3σ边界，而RI-EKF、Ideal-EKF和我们提出的Aff-EKF的机器人误差都在这些边界内。上述结果表明，Std-EKF对于所考虑问题的不确定性估计过于自信。相比之下，RI-EKF和我们提出的Aff-EKF都具有良好的一致性，类似于Ideal-EKF。
然而，我们可以注意到，在表III和表IV中，RI-EKF在机器人方向和位置以及特征位置的平均RMSE方面都是最大的。我们的Aff-EKF的估计误差小于RI-EKF和Std-EKF，并且接近Ideal-EKF的性能。图3和图4中的RMSE展示也表明，忽略平面约束的RI-EKF在机器人姿态估计方面的误差明显大于利用该约束的其他三种EKF方法。
因此，由于缺乏平面约束，典型的RI-EKF在准确性方面甚至无法与Std-EKF相比，尽管它的一致性更好。相比之下，我们提出的Aff-EKF在准确性和一致性方面都优于Std-EKF，并且性能接近Ideal-EKF。
6. 总结 & 未来工作
本文首先从不可观测子空间对状态值依赖性的角度出发，证明了观测保真EKF的一些充分和必要条件。基于这些新证明的定理，提出了Aff-EKF框架来解决EKF在状态估计问题中的不一致性问题，这是一种理论上有根据的方法，具有明确的设计程序。作为演示，我们展示了我们的AfF-EKF在三个SLAM应用中的实现，包括典型的点特征、平面约束点特征和平面特征。
正如我们的框架在基于点特征的SLAM中的应用所示，我们可以为同一个问题得到多个一致的AfF-EKF估计器。在进一步的研究中，我们将研究在准确性和效率方面选择最佳估计器的规则。此外，我们还将扩展我们的框架到连续动态系统的状态估计，并将其应用于其他问题，如视觉-惯性导航系统的估计和动态/可变形环境中的SLAM。
对更多实验结果和文章细节感兴趣的读者，可以阅读一下论文原文~
本文仅做学术分享，如有侵权，请联系删文。
3D视觉交流群，成立啦！
目前我们已经建立了3D视觉方向多个社群，包括
2D计算机视觉
、
最前沿
、
工业3D视觉
、
SLAM
、
自动驾驶
、
三维重建
、
无人机
等方向，细分群包括：
工业3D视觉
：相机标定、立体匹配、三维点云、结构光、机械臂抓取、缺陷检测、6D位姿估计、相位偏折术、Halcon、摄影测量、阵列相机、光度立体视觉等。
SLAM
：视觉SLAM、激光SLAM、语义SLAM、滤波算法、多传感器融合、多传感器标定、动态SLAM、MOT SLAM、NeRF SLAM、机器人导航等。
自动驾驶
：深度估计、Transformer、毫米波|激光雷达|视觉摄像头传感器、多传感器标定、多传感器融合、3D目标检测、路径规划、轨迹预测、3D点云分割、模型部署、车道线检测、Occupancy、目标跟踪等。
三维重建
：3DGS、NeRF、多视图几何、OpenMVS、MVSNet、colmap、纹理贴图等
无人机
：四旋翼建模、无人机飞控等
2D计算机视觉
：图像分类/分割、目标/检测、医学影像、GAN、OCR、2D缺陷检测、遥感测绘、超分辨率、人脸检测、行为识别、模型量化剪枝、迁移学习、人体姿态估计等
最前沿
：具身智能、大模型、Mamba、扩散模型、图像/视频生成等
除了这些，还有
求职
、
硬件选型
、
视觉产品落地、产品、行业新闻
等交流群
添加小助理: cv3d001，备注：
研究方向+学校/公司+昵称
（如
3D点云+清华+小草莓
）, 拉你入群。
▲长按扫码添加助理：cv3d001
3D视觉工坊知识星球
