Observability Analysis and Consistency Improvements for Visual-Inertial Odometry on the Matrix Lie Group of Extended Poses

Abstract

In this paper, we present a novel extended Kalman filter (EKF)-based visual-inertial odometry for robotic platforms by modeling the state space as the recently proposed matrix Lie group of extended poses. Specifically, we found that the proposed estimator suffers from an inconsistency similar to that of the conventional $SO\\left ({3}\\right)\\times \\mathbb {R}^{6}$ uncertainty representation from the standpoint of an observability analysis. The inconsistency mainly is a result of spurious information along the unobservable directions. An inconsistent estimator would lead to overconfidently reducing the state uncertainty and larger estimation errors that would in turn cause system divergence. We applied the first-estimate Jacobian (FEJ) framework and observability constrained (OC) techniques to avoid spurious information and improve consistency. The performance of the proposed estimator is validated using both simulated and real-world datasets.