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.

Contraction Theory for Nonlinear Stability Analysis and Learning-based Control: A Tutorial Overview

“…Due to all these useful properties, extensions of contraction theory have been considered in many different settings. These include, but are not limited to, stochastic contraction (Gaussian white noise [13,16,17,34], Poisson shot noise and Lévy noise [35]), contraction for discrete and hybrid nonlinear systems [7,8,13,17,37,42], partial contraction [11], transverse contraction [43], incremental stability analysis of nonlinear estimation (the Extended Kalman Filter (EKF) [44], nonlinear observers [16,45], Simultaneous Localization And Mapping (SLAM) [46]), generalized gradient descent based on geodesical convexity [47], contraction on Finsler and Riemannian manifolds [48][49][50], contraction on Banach and Hilbert spaces for PDEs [51][52][53], non-Euclidean contraction [54], contracting learning with piecewise-linear basis functions [55], incremental quadratic stability analysis [56], contraction after small transients [57], immersion and invariance stabilizing controller design [58,59], and Lipschitz-bounded neural networks for robustness and stability guarantees [60][61][62].…”

“…Among these are Lagrangian systems [5, p. 392], where one easy choice of positive definite matrices that define a contraction metric is the inertia matrix, or feedback linearizable systems [63][64][65][66][67], where we could solve the Riccati equation for a contraction metric as in LTV systems. This is also the case in the context of state estimation (e.g., the nonlinear SLAM problem can be reformulated as an LTV estimation problem using virtual synthetic measurements [16,46]). Once we find a contraction metric and Lyapunov function of a nominal nonlinear system for the sake of stability, they could be used as a Control Lyapunov Function (CLF) to attain stabilizing feedback control [3,68,69] or could be augmented with an integral control law called adaptive backstepping to recursively design a Lyapunov function for strict-and output-feedback systems [70][71][72][73].…”

Particle observers for state estimation and adaptation in deterministic systems with random piecewise constant inputs

“…Using the flow of auxiliary system (26)-(27) in place of the flow of original system (17) to estimate the state of the original system through a VRPF, may be justified through the notion of virtual system, see Section IV-C below.…”

“…in [16]. To address such questions one could combine the contracting observer of [17], based on the construction of simple synthetic measurements for SLAM, with the particle approach developed in this article. Another possible application is contact detection in robotics, see also Section IV-D.…”

A Trajectory-Based Approach to Multi-Session Underwater Visual SLAM Using Global Image Signatures

“…Since well-established approaches to SLAM, such as Extended Kalman Filter SLAM (EKF-SLAM) or Graph-SLAM [2], are widely used and new approaches mainly concentrate on alleviating their intrinsic limitations [3], SLAM is often considered a solved problem. New research tends to focus on practical-and application-related issues, which depend on or are strongly related to the sensors used to build the map and to estimate the vehicle motion and pose.…”