Optimal-state-constraint EKF for visual-inertial navigation

Abstract

As a visual-inertial navigation system (VINS) becomes prevalent thanks to recent advancements in cameras and inertial sensors, optimal sensor fusion algorithms are demanding. In this paper, we introduce a new optimal-state-constraint (OSC)-EKF for VINS, which performs tightly-coupled visual-inertial sensor fusion over a sliding window of poses only (i.e., without including features in the state vector), and thus has complexity independent of the size of the environment. The key idea of the proposed OSC-EKF is to design a novel measurement model that utilizes all feature measurements available within the sliding window and derives probabilistically optimal constraints between poses while without estimating these features as part of the state vector. To this end, for each sliding window, we perform structure and motion using only the available camera measurements and subsequently marginalize out the structure (features) to obtain the optimal motion constraints that will be used in the EKF update. The proposed approach is validated in the proof-of-concept, real-world experiments.