Lie Algebra Based Multi-State Extended Kalman Filter with Incremental Pose Measurement

This work presents a fusion framework for pose estimation that integrates incremental pose measurements with IMU data within a multi-state (augmented state) extended Kalman filter. While relative transformations between consecutive camera (or LiDAR) frames can be estimated via numerous algorithms, fusing these incremental transformations with IMU measurements for enhanced pose accuracy demands a rigorous mathematical approach. We address this problem by employing the logarithmic map of SE,(3) to represent incremental pose as a measurement model within the Kalman filter framework, with error states defined through the exponential map of SE,(3). We validate the proposed method on the publicly available KITTI, dataset using Monte Carlo runs, highlighting the effectiveness of our approach in improving trajectory accuracy.