Closed-form Error Propagation on the SE_n(3) Group for Invariant Extended Kalman Filtering with Applications to VINS

TL;DR

This paper derives a closed-form error propagation formula for the Invariant Extended Kalman Filter on the SE_n(3) group and demonstrates its superior performance in vision-aided inertial navigation tasks through simulations and real-world experiments.

Contribution

It introduces a novel closed-form error propagation method for IEKF on SE_n(3), enhancing accuracy in pose estimation for robotic navigation.

Findings

Outperforms quaternion-based EKF and other state-of-art filters

Validated through simulations and EuRoC MAV dataset experiments

Improves pose estimation accuracy in vision-aided inertial navigation

Abstract

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 paper, 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. We evaluate our algorithm 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 outperforms some state-of-art filter-based methods such as the quaternion-based EKF, first estimates Jacobian EKF, etc.