Stereo MSCKF with Online Extrinsic Calibration using Invariant Extended Kalman Filter

Abstract

Rigid body motion can be modeled as the Lie group. For that reason, its uncertainty also has to be represented in the Lie group, and it is crucial for the proper localization of robots moving with high-speed. Also, the rigid body transformations between camera-IMU, multiple cameras that their errors can highly disturb robust state estimation are elements of the Lie group and can be calibrated while estimating the robot pose by composing them into the state of the visual-inertial odometry pipelines. Unfortunately, only a few researchers dealt with the state estimation problem on the matrix Lie group. In this paper, we present the states of stereo MSCKF to the matrix Lie group using IEKF to represent the state and the noise similar to the real world. Besides, our algorithm includes visual-inertial and camera extrinsic parameters as the matrix Lie group for online calibration and recovery from poor initial extrinsic parameters or an accident during the movement that can cause the physical skew. As results, we evaluate our algorithm and compare it with the original algorithm. Also, we demonstrate our online calibration results during the flight on the EuRoC MAV dataset.