Likelihood-based iterated cubature multi-state-constraint Kalman filter for visual inertial navigation system

Abstract

In this paper, we present an advanced real-time Visual Inertial Navigation System (VINS) based on Multi-State Constraint Kalman Filter (MSCKF). This filter uses Cubature Kalman Filter (CKF) for nonlinear measurement update and Maximum Likelihood Estimate (MLE) to optimize the estimate, which in turn provides better system accuracy and stability. The measurement model is developed basing Trifocal Tensor Geometry (TTG), which allows replacing the 3D feature-point reconstruction step as in traditional VINS systems. Alternatively the available Unscented MSCKF [1] based on Unscented Kalman Filter has an implementation issue of executing the square-root operation of the covariance matrix due to the negatively-weighted sigma points, and this may halt the filter operation or even causes the system to fail. The proposed CKF structure has the ability to carry the highly-nonlinear TTG-based measurement model as well as overcome the issue associated with the covariance square-root operation. The MLE based iteration is applied to optimize the visual measurement update where it performs multiple corrections on a single measurement. This procedure helps to minimize the error accumulation allowing the filter to operate for longer durations. The proposed Iterated Cubature MSCKF is tested using KITTI datasets [2] and compared against the Unscented MSCKF and non-iterated Cubature MSCKF.