Developing a Cubature Multi-state Constraint Kalman Filter for Visual-Inertial Navigation System

The objective of this paper is to develop a cubature Multi-State Constraint Kalman Filter (MSCKF) for a VisualInertial Navigation System (VINS). MSCKF is a tightly-coupled EKF-based filter operating over a sliding window of multiple sequent states. In order to decrease the complexity and the computational cost of the original EKF-based measurement, the measurement model is built on the Trifocal Tensor Geometry (TTG). The predicted measurement does not need to reconstruct the 3D position of the visual landmarks. In order to employ that nonlinear TTG-based measurement model, this paper will implement cubature approach (i.e. popularly associated with Cubature Kalman Filter (CKF)). Compared to other advanced nonlinear filter, specifically Unscented Kalman Filter (UKF), the CKF has removed the positive-definite condition of the covariance matrix computation, which may halt or fail the filter operation. The proposed filter is validated with three KITTI datasets [1] of residential area to evaluate its performance.