Invariant filtering for Pose EKF-SLAM aided by an IMU

This paper presents a new method to combine measurements from motion sensors (wheel encoders or inertial measurements) with relative pose estimates inferred from scan matching of dense depth images taken at different time instants. These can optionally be completed with measurements of relative position to landmarks with known location. The fusion is performed through an invariant extended Kalman filter (IEKF) with augmented state, as an application of the stochastic cloning method. This results in a fusion algorithm allowing to include IMU information in pose EKF-SLAM and which inherits the desirable properties of IEKFs, that is, although the system is non-linear, the evolution of the error is independent of the trajectory, as in the linear case.