Unscented Kalman Filtering on Lie Groups for Fusion of IMU and Monocular Vision

Abstract

Combining visual information with inertial measurements represents one popular approach to achieve robust and autonomous navigation in robotics, specifically for low-cost aerial vehicles in GPS-denied environments. In this paper, building upon both the recent theory of Unscented Kalman Filtering on Lie Groups (UKF-LG) and the theory of invariant Kalman filter based SLAM we proposed recently, an innovative UKF for the monocular visual inertial problem is derived, where the body pose, velocity, and the 3D landmarks' positions are viewed as a single element of a (high dimensional) Lie group SE2+p(3), and where accelerometers' and gyrometers' biases are appended to the state and estimated as well. The efficiency of the approach is validated on a real data set, where the proposed filter is shown to compare favorably to the conventional UKF, and to some recent UKF variants of the literature.