[论文解读] Associating Uncertainty to Extended Poses for on Lie Group IMU Preintegration with Rotating Earth

Martin Brossard, Axel Barrau+2 more|arXiv (Cornell University)|Jul 28, 2020

Robotics and Sensor-Based Localization参考文献 54被引用 77

一句话总结

本文提出了一种将不确定性附加到 SE2(3) 上的拓展位姿的理论，用于包含地球自转的 IMU 预积分，推导出精确的预积分公式，并通过仿真和在车辆上的激光雷达-IMU 融合进行验证。

ABSTRACT

The recently introduced matrix group SE2(3) provides a 5x5 matrix representation for the orientation, velocity and position of an object in the 3-D space, a triplet we call "extended pose". In this paper we build on this group to develop a theory to associate uncertainty with extended poses represented by 5x5 matrices. Our approach is particularly suited to describe how uncertainty propagates when the extended pose represents the state of an Inertial Measurement Unit (IMU). In particular it allows revisiting the theory of IMU preintegration on manifold and reaching a further theoretic level in this field. Exact preintegration formulas that account for rotating Earth, that is, centrifugal force and Coriolis force, are derived as a byproduct, and the factors are shown to be more accurate. The approach is validated through extensive simulations and applied to sensor-fusion where a loosely-coupled fixed-lag smoother fuses IMU and LiDAR on one hour long experiments using our experimental car. It shows how handling rotating Earth may be beneficial for long-term navigation within incremental smoothing algorithms.

研究动机与目标

• Extend uncertainty representation to extended poses (orientation, velocity, position) using SE2(3).
• Derive IMU preintegration formulas on the SE2(3) group including rotating Earth effects.
• Provide rigorous uncertainty propagation through IMU kinematics and through discrete preintegration steps.
• Validate the framework with simulations and real one-hour car experiments; release accompanying code and supplementary material.

提出的方法

• Represent extended poses as elements of SE2(3) with 5x5 matrix structure.
• Describe uncertainty via concentrated Gaussians: T = T̂ exp(xi) with xi ~ N(0, Σ).
• Use BCH-based tools and Ad_T operators to propagate means and covariances through IMU kinematics.
• Derive exact discretization for T_{i+1} = Γ_i Φ(T_i) Υ_i and propagate uncertainty via a_i = Ad_{Υ_i^{-1}} F ξ with higher-order corrections.
• Incorporate rotating Earth to account for centrifugal and Coriolis forces in the preintegration framework.
• Provide second- and fourth-order (S4th) covariance corrections and discuss bias handling.

实验结果

研究问题

• RQ1 How can uncertainty be consistently associated with extended poses (R, v, p) represented by SE2(3) for IMU applications?
• RQ2 How does one propagate extended-pose uncertainty through IMU kinematic models and through IMU preintegration on Lie groups?
• RQ3 What are the benefits of including rotating-Earth effects in exact IMU preintegration, especially for long-term navigation?
• RQ4 Can the proposed SE2(3) framework yield accurate factor covariances in sensor fusion scenarios (e.g., IMU-LiDAR) over long sequences?

主要发现

• Uncertainty on extended poses can be propagated exactly through IMU kinematics on SE2(3) with the mean updating via T̂_{i+1} and covariance via Σ_{i+1} = Ad_{Υ_i^{-1}} F Σ_i (Ad_{Υ_i^{-1}} F)^T.
• When incorporating noisy IMU measurements, the fourth-order BCH terms yield covariance corrections S_4th that improve accuracy compared to second-order approximations.
• The framework preserves concentrated Gaussians on SE2(3) through the noise-free model, with a mean evolution that remains reasonable under small perturbations.
• Exact discretization (no discretization error) is achieved for the pose update, with practical Euler-type steps used for implementation and numerical experiments.
• Real experiments demonstrate the method's effectiveness in a loosely-coupled iSAM2 framework with one-hour IMU-LiDAR sequences on a car, showing benefits for long-term incremental smoothing.
• Python scripts and a GTSAM fork accompanying the work enable replication of key equations and plots.