Invariant Smoothing for Localization: Including the IMU Biases

In this article we investigate smoothing, i.e., optimisation-based state estimation techniques, for robot localization using an IMU aided by other localization sensors. We more particularly focus on Invariant Smoothing (IS), a variant based on the use of Lie groups within the invariant filtering framework. We bring the recently introduced Two Frames Group (TFG) to bear on the problem of Invariant Smoothing, to better take into account the IMU biases, as compared to the state-of-the-art in localization and navigation. Experiments based on the KITTI dataset show the proposed framework compares favorably to the state-of-the-art smoothing methods in terms of robustness.