Efficient and Consistent Two Key-Frame Visual-Inertial Navigation Using Matrix Lie Groups

Abstract This paper presents the design of a two key-frame visual inertial navigation system (2KF-VINS) using combined Lie group SE2(3) extended Kalman filter (EKF) design framework. The conventional 2KF-VINS filter is unobservable for translations along all three axis and rotation about the gravity direction. As a result, the filter suffers from estimation inconsistencies related to unobservable transformations of the estimation problem. The proposed combined Lie group SE2(3) framework remedies this issue by implicitly preserving the observability consistency property of the filter. Monte Carlo numerical simulations are used to validate the theoretical performance of the right -SE2(3) 2KF-VINS, along with experimental validation using EuRoC MAV dataset to evaluate the performance in real-world scenarios. Additionally, the proposed algorithm is compared with state of the art MSCKF VINS, right-invariant MSCKF VINS, left-SO(3) and right-SO(3) 2KF-VINS versions with identical and realistic tuning parameters in order to validate the performance related to accuracy, consistency, and computational speed of the method.