Unscented Kalman Filter on Lie Groups for Visual Inertial Odometry

Fusing visual information with inertial measurements for state estimation has aroused major interests in recent years. However, combining a robust estimation with computational efficiency remains challenging, specifically for low-cost aerial vehicles in which the quality of the sensors and the processor power are constrained by size, weight and cost. In this paper, we present an innovative filter for stereo visual inertial odometry building on: (i) the recently introduced stereo multistate constraint Kalman filter; (ii) the invariant filtering theory; and (iii) the unscented Kalman filter (UKF) on Lie groups. Our solution combines accuracy, robustness and versatility of the UKF. We then compare our approach to state-of-art solutions in terms of accuracy, robustness and computational complexity on the EuRoC dataset and a challenging MAV outdoor dataset.