[论文解读] A Mathematical Framework for IMU Error Propagation with Applications to Preintegration

Axel Barrau, Silvère Bonnabel|arXiv (Cornell University)|Mar 8, 2020

Inertial Sensor and Navigation参考文献 39被引用 56

一句话总结

本文提出基于李群的框架，使用 SE2(3) 来建模并传播扩展姿态（位置、速度、方向）的 IMU 误差，并推导包括地球自转效应在内的精确预积分公式，同时处理不确定性和偏置。

ABSTRACT

To fuse information from inertial measurement units (IMU) with other sensors one needs an accurate model for IMU error propagation in terms of position, velocity and orientation, a triplet we call extended pose. In this paper we leverage a nontrivial result, namely log-linearity of inertial navigation equations based on the recently introduced Lie group $SE_2(3)$, to transpose the recent methodology of Barfoot and Furgale for associating uncertainty with poses (position, orientation) of $SE(3)$ when using noisy wheel speeds, to the case of extended poses (position, velocity, orientation) of $SE_2(3)$ when using noisy IMUs. Besides, our approach to extended poses combined with log-linearity property allows revisiting the theory of preintegration on manifolds and reaching a further theoretic level in this field. We show exact preintegration formulas that account for rotating Earth, that is, centrifugal force and Coriolis effect, may be derived as a byproduct.

研究动机与目标

• 将 IMU 误差传播的估计从姿态（SE(3)）扩展到包含位置、速度、方向的扩展姿态（SE2(3)）。
• 在 SE2(3) 中证明 IMU 导航方程的对数线性，以实现对流形上鲁棒的预积分。
• 给出考虑地球自转（科氏力和离心力效应）的精确预积分公式，并将其与在流形上的滤波联系起来。
• 开发一种框架，使扩展姿态具有不确定性，并推导 IMU 数据中的噪声和偏置的传播方程。
• 解决 IMU 预积分中的偏置问题，并在 SE2(3) 的指数坐标下提出一阶偏置修正。

提出的方法

• 将扩展姿态建模为 SE2(3) 的元素，并使用指数映射来表示九维李代数中的扰动。
• 显示群仿射性对 IMU 动力学的性质并推导精确解形式 T_t = Γ_t Φ_t(T_0) Υ_t。
• 通过引入速度扩增 V′ = V + Ω×X 并将其嵌入 SE2(3)，推导包含地球自转的预积分公式。
• 推导对 SE2(3) 上的集中高斯分布的精确离散时间误差传播，并通过 Ad_Υ^{-1} 和 F 算子获得线性化误差传播。
• 将 SE2(3) 上的高斯分布定义为 T = T̄ exp(ξ) 其中 ξ ∼ N(0, Σ)，并在 IMU 模型中进行传播。
• 给出精确的误差累积公式 exp(ξ_k) = exp(F_0^{k-1} ξ_0) · ∏_{i=0}^{k-1} exp(F_{i+1}^{k-1} η_i)，并讨论不确定性的含义。
• 在预积分中使用 SE2(3) 的指数坐标讨论一阶偏置修正，并推导更新规则。

实验结果

研究问题

• RQ1 如何在 SE2(3) 内将扩展姿态（位置、速度、方向）中的 IMU 误差传播公式化？
• RQ2 在考虑地球自转导致的科氏力与离心力效应时，SE2(3) 上的 IMU 动力学是否具有对数线性性质？
• RQ3 是否可以在 SE2(3) 上推导出考虑地球自转和偏置/噪声的精确或闭式预积分公式？
• RQ4 如何将不确定性附着到扩展姿态，并在基于 IMU 的预积分中传播？
• RQ5 在 SE2(3) 预积分框架中，IMU 偏置的影响与纠正机制是什么？

主要发现

• SE2(3) 上的 IMU 导航方程表现出对数线性，从而在考虑地球自转的情况下实现对流形上的预积分。
• 推导出考虑科氏力和离心力效应的精确预积分公式，其中包含速度扩增技巧 V′ = V + Ω×X。
• 定义为 T = T̄ exp(ξ) 且 ξ ∼ N(0, Σ) 的 SE2(3) 上的高斯分布可以在无噪声的 IMU 模型中精确传播，从而在指数坐标中获得线性化的误差传播。
• 对于带有噪声的 IMU 数据，建立了显式的误差累积公式 exp(ξ_k) = exp(F_0^{k-1} ξ_0) · ∏_{i=0}^{k-1} exp(F_{i+1}^{k-1} η_i)，实现精确或闭式的不确定性跟踪（一阶通过 BCH）。
• 通过在 SE2(3) 的指数坐标实现一阶偏置修正的预积分，提供了偏置更新的改进雅可比矩阵。
• 该框架将基于 SE2(3) 的不确定性处理与预积分理论联系起来，并支持高频 IMU 融合的因子图方法。