A Linear-Complexity EKF for Visual-Inertial Navigation with Loop Closures

Abstract

Enabling real-time visual-inertial navigation in unknown environments while achieving bounded-error performance holds great potentials in robotic applications. To this end, in this paper, we propose a novel linear-complexity EKF for visual-inertial localization, which can efficiently utilize loop closure constraints, thus allowing for long-term persistent navigation. The key idea is to adapt the Schmidt-Kalman formulation within the multi-state constraint Kalman filter (MSCKF) framework, in which we selectively include keyframes as nuisance parameters in the state vector for loop closures but do not update their estimates and covariance in order to save computations while still tracking their cross-correlations with the current navigation states. As a result, the proposed Schmidt-MSCKF has only O(n) computational complexity while still incorporating loop closures into the system. The proposed approach is validated extensively on large-scale real-world experiments, showing significant performance improvements when compared to the standard MSCKF, while only incurring marginal computational overhead.