Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group

In this paper, we present a novel visual-inertial navigation algorithm for low-cost and computationally constrained vehicle in global positioning system denied environments by modeling the state space as the matrix Lie group (LG), based on the recent theory of the invariant Kalman filter. The multistate constraint Kalman filter (MSCKF) is a well-known visual-inertial odometry algorithm that performs the fusion of the visual and inertial information by constraining each other through the stochastically cloned pose within a sliding window. However, conventional MSCKF (MSCKF-Conv) suffers from the inconsistent state estimates caused by the spurious gain along the unobservable directions, resulting in large estimation errors. To tackle this problem, we extend the concepts of the state and noise of the MSCKF from Euclidean space to matrix LG. We model the state of the MSCKF as the element of the specially customized matrix LG and use the noise uncertainty modeling with the corresponding Lie algebra. The detailed derivation and observability analysis of the proposed filter are provided to prove that the proposed filter is more consistent than the MSCKF-Conv. The proposed MSCKF on matrix LG naturally enforces the state vector to exist in the state space that maintains the unobservability characteristics without any artificial remedies. The performance of the proposed filter is validated through the Monte-Carlo simulation and the real-world experimental dataset.