An Iterative Kalman Smoother for Robust 3D Localization and Mapping

In this paper, we present an iterative Kalman smoother (IKS) for robust 3D localization and mapping, using visual and inertial measurements. Contrary to extended Kalman filter (EKF) methods, smoothing increases the convergence rate of critical parameters (e.g., IMU’s velocity and camera’s clock drift), improves the positioning accuracy during challenging conditions (e.g., scarcity of visual features), and allows the immediate processing of visual observations. As opposed to existing smoothing approaches to VINS, based on the inverse filter (INVF), the proposed IKS exhibits superior numerical properties, allowing efficient implementations on mobile devices. Furthermore, we propose a classification of visual observations, for smoothing algorithms applied to VINS, based on their: (i) Track length, allowing their efficient processing as multi-state constraints, when possible and (ii) First observation, allowing their optional re-processing. Finally, we demonstrate the robustness of the proposed approach, over challenging indoor VINS scenarios, including, system (re)-initialization, and scarcity of visual observations.