[论文解读] $SE_2(3)$ based Extended Kalman Filtering and Smoothing Framework for Inertial-Integrated Navigation

Yarong Luo, Chi Guo+3 more|arXiv (Cornell University)|Feb 25, 2021

Inertial Sensor and Navigation参考文献 17被引用 7

一句话总结

该论文提出了一种基于 $SE_2(3)$ 的扩展卡尔曼滤波（EKF）框架，用于惯性集成导航，采用矩阵李群表示状态，实现了姿态、速度和位置的统一参考系误差表示。通过利用群仿射动力学和左/右不变误差定义，该方法在转移矩阵中消除了特定力项，实现了与不变EKF的等价性，从而在大失准角场景下提升了一致性与鲁棒性。

ABSTRACT

This paper proposes an $SE_2(3)$ based extended Kalman filtering (EKF) framework for the inertial-integrated state estimation problem. The error representation using the straight difference of two vectors in the inertial navigation system may not be reasonable as it does not take the direction difference into consideration. Therefore, we choose to use the $SE_2(3)$ matrix Lie group to represent the state of the inertial-integrated navigation system which consequently leads to the common frame error representation. With the new velocity and position error definition, we leverage the group affine dynamics with the autonomous error properties and derive the error state differential equation for the inertial-integrated navigation on the north-east-down (NED) navigation frame and the earth-centered earth-fixed (ECEF) frame, respectively, the corresponding EKF, terms as $SE_2(3)$ based EKF has also been derived. It provides a new perspective on the geometric EKF with a more sophisticated formula for the inertial-integrated navigation system. Furthermore, we design two new modified error dynamics on the NED frame and the ECEF frame respectively by introducing new auxiliary vectors. Finally the equivalence of the left-invariant EKF and left $SE_2(3)$ based EKF have been shown in navigation frame and ECEF frame.

研究动机与目标

• 解决传统EKF在惯性集成导航中状态误差表示不一致的问题，即误差通过无参考系考虑的直线向量差定义。
• 基于 $SE_2(3)$ 矩阵李群构建几何一致的EKF框架，用于表示完整状态（姿态、速度、位置），并在统一参考系中定义误差。
• 在NED和ECEF导航框架上推导左不变与右不变误差公式，确保群仿射动力学与误差的自主性。
• 证明左不变EKF与基于 $SE_2(3)$ 的EKF之间的等价性，验证所提框架的理论一致性。
• 通过一种简单直观的基于 $SE_2(3)$ 的RTS平滑器，将框架扩展至平滑处理，区别于现有的不变平滑方法。

提出的方法

• 状态被嵌入到 $SE_2(3)$ 矩阵李群中，该群将旋转、平移和速度整合为单一流形，实现几何一致的误差表示。
• 通过群乘积 $\eta = \hat{\mathcal{X}} \mathcal{X}^{-1}$ 定义误差状态，确保所有误差——姿态、速度与位置——均以统一参考系表达。
• 利用群仿射性质推导误差状态动力学，得到的转移矩阵自然消除了右不变形式中的特定力项。
• 在NED和ECEF框架上引入两种改进的误差动力学，通过辅助速度向量提升数值稳定性和误差传播性能。
• 分别在NED和ECEF框架上构建EKF，采用左不变与右不变误差定义，实现不同导航场景下的鲁棒性。
• 基于RTS平滑器开发了一种基于 $SE_2(3)$ 的平滑算法，利用矩阵指数与群运算实现完整状态更新。

实验结果

研究问题

• RQ1 基于 $SE_2(3)$ 的统一EKF框架是否能通过为所有状态误差使用统一参考系，提升惯性集成导航中的误差表示一致性？
• RQ2 $SE_2(3)$-基EKF是否如右不变形式所示，在转移矩阵中消除了特定力项？若能，这对数值稳定性有何影响？
• RQ3 在同时考虑NED与ECEF框架中的杠杆臂误差时，$SE_2(3)$-基EKF是否与左不变EKF等价？
• RQ4 在NED与ECEF框架上引入的改进误差动力学如何提升EKF在大失准角场景下的性能？
• RQ5 基于 $SE_2(3)$ 的框架是否可扩展至平滑处理，且其公式比现有不变平滑方法更简单直观？

主要发现

• 基于 $SE_2(3)$ 的EKF框架实现了姿态、速度与位置的一致统一参考系误差表示，消除了传统EKF公式中因参考系不一致导致的问题。
• 右不变的 $SE_2(3)$-基EKF公式成功从转移矩阵中消除了特定力项，提升了数值鲁棒性。
• 当考虑杠杆臂误差时，所提框架在NED与ECEF框架中均验证了左不变EKF与 $SE_2(3)$-基EKF的等价性，证实了理论一致性。
• 通过辅助速度向量引入的NED与ECEF框架上的改进误差动力学，显著增强了大失准角场景下的稳定性与收敛性。
• 基于 $SE_2(3)$ 的平滑算法比不变RTS平滑器更简单直观，其完整状态更新依赖于矩阵指数与群运算。
• 实验结果证实，基于 $SE_2(3)$ 的EKF在初始集成导航中对大初始失准角具有优异鲁棒性，优于传统方法。