Invariant Extended Kalman Filter on SE(3) for Pose Estimation

In this paper, two invariant extended Kalman filters on SE(3) for pose estimation are proposed, utilizing the measurements of inertial vectors and landmarks. With the velocity-free assumption, the invariant output errors coupled with invariant estimation errors are constructed under appropriate coordinate transformations, and expressed in the inertial frame and body-fixed frame, respectively. The resulted filters are termed as the right-invariant extended Kalman filter (RIEKF) and the left-invariant extended Kalman filter (LIEKF). Although the LIEKF does not satisfy the invariance theory strictly, it still outperforms the traditional extended Kalman filter in some cases. Monte Carlo simulations are conducted to demonstrate their advantageous performance.