Associating Uncertainty to Extended Poses for on Lie Group IMU Preintegration With Rotating Earth

Abstract

The recently introduced matrix group <inline-formula><tex-math notation="LaTeX">$\mathbf {SE_2(3)}$</tex-math></inline-formula> provides a 5<inline-formula><tex-math notation="LaTeX">$\mathbf {\times }$</tex-math></inline-formula>5 matrix representation for the orientation, velocity, and position of an object in the 3-D space, a triplet we call &#x201C;extended pose.&#x201D; In this article, we build on this group to develop a theory to associate uncertainty with extended poses represented by 5<inline-formula><tex-math notation="LaTeX">$\mathbf {\times }$</tex-math></inline-formula>5 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 1-h-long experiments using our experimental car. It shows how handling rotating earth may be beneficial for long-term navigation within incremental smoothing algorithms.