Developing Computationally Efficient Nonlinear Cubature Kalman Filtering for Visual Inertial Odometry

This paper presents a computationally efficient sensor-fusion algorithm for visual inertial odometry (VIO). The paper utilizes trifocal tensor geometry (TTG) for visual measurement model and a nonlinear deterministic-sampling-based filter known as cubature Kalman filter (CKF) to handle the system nonlinearity. The TTG-based approach is developed to replace the computationally expensive three-dimensional-feature-point reconstruction in the conventional VIO system. This replacement has simplified the system architecture and reduced the processing time significantly. The CKF is formulated for the VIO problem, which helps to achieve a better estimation accuracy and robust performance than the conventional extended Kalman filter (EKF). This paper also addresses the computationally efficient issue associated with Kalman filtering structure using cubature information filter (CIF), the CKF version on information domain. The CIF execution avoids the inverse computation of the high-dimensional innovation covariance matrix, which in turn further improves the computational efficiency of the VIO system. Several experiments use the publicly available datasets for validation and comparing against many other VIO algorithms available in the recent literature. Overall, this proposed algorithm can be implemented as a fast VIO solution for high-speed autonomous robotic systems.