[Paper Review] 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 Localization54 references77 citations

TL;DR

The paper develops a theory to attach uncertainty to extended poses on SE2(3) for IMU preintegration with rotating Earth, derives exact preintegration formulas, and validates via simulations and LiDAR-IMU fusion on a car.

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.

Motivation & Objective

• 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.

Proposed method

• 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.

Experimental results

Research questions

• 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?

Key findings

• 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.