Analytical SLAM without linearization

Abstract

We apply a combination of linear time varying (LTV) Kalman filtering and nonlinear contraction tools to the problem of simultaneous mapping and localization (SLAM), in a fashion which avoids linearized approximations altogether. By exploiting virtual synthetic measurements, the LTV Kalman observer avoids errors and approximations brought by the linearization process in the EKF SLAM. Furthermore, conditioned on the robot position, the covariances between landmarks are fully decoupled, making the algorithm easily scalable. Contraction analysis is used to establish stability of the algorithm and quantify its convergence rate. We propose four versions based on different combinations of sensor information, ranging from traditional bearing measurements and radial measurements to optical flows and time-to-contact measurements. As shown in simulations, the proposed algorithm is simple and fast, and it can solve SLAM problems in both 2D and 3D scenarios with guaranteed convergence rates in a full nonlinear context.