An Improved Multi-State Constraint Kalman Filter for Visual-Inertial Odometry

TL;DR

This paper introduces an enhanced Multi-State Constraint Kalman Filter for visual-inertial odometry, significantly improving speed and accuracy for autonomous robot pose estimation in GPS-denied environments.

Contribution

The paper proposes a faster, more accurate version of MSCKF by implementing feature marginalization and state pruning strategies, suitable for resource-constrained systems.

Findings

FMSCKF is about six times faster than standard MSCKF.

FMSCKF achieves at least 20% better accuracy in position estimation.

Validated on open-source datasets and real-world experiments.

Abstract

Fast pose estimation (PE) is of vital importance for successful mission performance of agile autonomous robots. Global Positioning Systems such as GPS and GNSS have been typically used in fusion with Inertial Navigation Systems (INS) for PE. However, the low update rate and lack of proper signals make their utility impractical for indoor and urban applications. On the other hand, Visual-Inertial Odometry (VIO) is gaining popularity as a practical alternative for GNSS/INS systems in GPS-denied environments. Among the many VIO-based methods, the Multi-State Constraint Kalman Filter (MSCKF) has received a greater attention due to its robustness, speed and accuracy. To this end, the high computational cost associated with image processing for real-time implementation of MSCKF on resource-constrained vehicles is still a challenging ongoing research. In this paper, an enhanced version of the MSCKF is proposed. To this aim, different feature marginalization and state pruning strategies are suggested that result in a much faster algorithm. The proposed algorithm is tested both on an open-source dataset and in real-world experiments for validation. It is demonstrated that the proposed Fast-MSCKF (FMSCKF) is about six times faster and at least 20% more accurate in final position estimation than the standard MSCKF algorithm.