Closed-Form Error Propagation on SEn(3) Group for Invariant EKF With Applications to VINS

Pose estimation is important for robotic perception, path planning, etc. Robot poses can be modeled on matrix Lie groups and are usually estimated via filter-based methods. In this letter, we establish the closed-form formula for the error propagation for the Invariant extended Kalman filter (IEKF) in the presence of random noises and apply it to vision-aided inertial navigation. Moreover, we use the theoretic results to add the compensation parts for IEKF. We evaluate our algorithms via numerical simulations and experiments on the OPENVINS platform. Both simulations and the experiments performed on the public EuRoC MAV datasets demonstrate that our algorithm in particular parameters settings outperforms some state-of-art filter-based methods such as the quaternion-based EKF, first estimates Jacobian EKF, etc. The techniques of choice on the parameters are worth further investigating.