Invariant Kalman Filtering for Visual Inertial SLAM

Abstract

Combining visual information with inertial measurements is a popular approach to achieve robust and autonomous navigation in robotics, specifically in GPS-denied environments. In this paper, building upon both the recent theory of Unscented Kalman Filtering on Lie Groups (UKF-LG) and more generally the theory of invariant Kalman filtering (IEKF), an innovative UKF is derived for the monocular visual simultaneous localization and mapping (SLAM) problem. The body pose, velocity, and the 3D landmarks' positions are viewed as a single element of a (high dimensional) Lie group SE <sub xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">2+p</sub> (3), which constitutes the state, and where the accelerometers' and gyrometers' biases are appended to the state and estimated as well. The efficiency of the approach is validated both on simulations and on five real datasets.