[论文解读] An Invariant-EKF VINS Algorithm for Improving Consistency

Teng Zhang, Kanzhi Wu+3 more|arXiv (Cornell University)|Feb 25, 2017

Robotics and Sensor-Based Localization参考文献 20被引用 10

一句话总结

本文提出了一种用于视觉-惯性导航系统（RIEKF-VINS）的右不变误差扩展卡尔曼滤波器，通过在随机不可观测变换下保持不变性——特别是沿重力方向的平移和旋转——来确保一致性。通过使用李群几何重新表述误差状态和不确定性表示，RIEKF-VINS 消除了传统 EKF-VINS 中固有的不一致性，显著提升了仿真和真实世界实验中的估计可靠性。

ABSTRACT

The main contribution of this paper is an invariant extended Kalman filter (EKF) for visual inertial navigation systems (VINS). It is demonstrated that the conventional EKF based VINS is not invariant under the stochastic unobservable transformation, associated with translations and a rotation about the gravitational direction. This can lead to inconsistent state estimates as the estimator does not obey a fundamental property of the physical system. To address this issue, we use a novel uncertainty representation to derive a Right Invariant error extended Kalman filter (RIEKF-VINS) that preserves this invariance property. RIEKF-VINS is then adapted to the multistate constraint Kalman filter framework to obtain a consistent state estimator. Both Monte Carlo simulations and real-world experiments are used to validate the proposed method.

研究动机与目标

• 解决传统基于 EKF 的视觉-惯性导航系统（VINS）中的不一致性问题，该问题源于在不可观测变换下未能保持不变性，导致状态不确定性被低估。
• 识别出核心问题在于传统 EKF 在随机不可观测变换（即平移和绕重力轴的旋转）下缺乏不变性。
• 开发一种右不变误差 EKF（RIEKF-VINS），通过在李群 SE(3) 和 SO(3) 上使用几何一致的误差表示，保持系统的不变性。
• 将 RIEKF-VINS 集成到多状态约束卡尔曼滤波器（MSCKF）框架中，构建一个一致的状态估计器用于 VINS。
• 通过广泛的蒙特卡洛仿真和真实世界实验验证所提方法，证明其在一致性与精度方面优于基线 EKF-VINS 和 MSCKF。

提出的方法

• 在李群 SE(3) 和 SO(3) 上使用右不变误差表示状态误差，确保位姿和方向估计的几何一致性。
• 推导一种新型不确定性表示方法，通过右不变误差变换误差状态，保持在不可观测变换下的不变性。
• 修改 EKF 更新方程，采用右不变误差公式，确保滤波器输出在平移和重力轴旋转下保持不变。
• 通过重新定义状态和误差传播，将 RIEKF-VINS 框架集成到 MSCKF 架构中，以在多个状态间保持不变性。
• 使用预积分框架处理 IMU 测量，以在连续-离散 EKF 框架中保持一致性。
• 将不变误差表示应用于状态转移模型和测量模型，确保雅可比矩阵和增益矩阵以几何一致的方式计算。

实验结果

研究问题

• RQ1 为何传统 EKF-VINS 尽管被广泛使用，仍会产生不一致的状态估计？
• RQ2 如何在基于 EKF 的 VINS 估计器中保持物理系统在平移和绕重力轴旋转下的不变性？
• RQ3 能否构建一种右不变误差 EKF（RIEKF-VINS），以确保滤波器输出在随机不可观测变换下保持不变？
• RQ4 将 RIEKF-VINS 集成到 MSCKF 框架中，相较于标准 MSCKF，如何提升一致性和估计精度？
• RQ5 在真实世界和仿真条件下，RIEKF-VINS 在一致性与精度方面相较于传统 EKF-VINS 的提升程度如何？

主要发现

• 传统 EKF-VINS 算法存在不一致性，因其在随机不可观测变换（如平移和绕重力轴旋转）下未能保持不变性。
• 所提出的 RIEKF-VINS 算法通过在李群 SE(3) 上使用右不变误差表示，确保滤波器输出在这些变换下保持不变。
• 蒙特卡洛仿真表明，RIEKF-VINS 显著提升了估计的一致性，其状态不确定性估计更加可靠，更准确地反映了实际估计误差。
• 将 RIEKF-VINS 集成到 MSCKF 框架中，可获得更一致且更精确的估计器，尤其在长期导航场景中表现更优。
• 真实世界实验表明，RIEKF-VINS 的精度高于原始 MSCKF 和 ConEKF-VINS，漂移更小，不确定性校准更优。
• 理论分析证明，RIEKF-VINS 在确定性和随机不可观测变换下均满足不变性条件，证实其一致性。