  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.
