Rao-Blackwellized Particle Smoothing for Simultaneous Localization and Mapping

TL;DR

This paper introduces a framework for probabilistic SLAM using particle smoothing that does not only incorporate observed data in current state estimates, but it also backtracks the updated knowledge to correct for past drift and ambiguities in both the map and in the states.

Abstract

Simultaneous localization and mapping (SLAM) is the task of building a map representation of an unknown environment while at the same time using it for positioning. A probabilistic interpretation of the SLAM task allows for incorporating prior knowledge and for operation under uncertainty. Contrary to the common practice of computing point estimates of the system states, we capture the full posterior density through approximate Bayesian inference. This dynamic learning task falls under state estimation, where the state-of-the-art is in sequential Monte Carlo methods that tackle the forward filtering problem. In this paper, we introduce a framework for probabilistic SLAM using particle smoothing that does not only incorporate observed data in current state estimates, but it also back-tracks the updated knowledge to correct for past drift and ambiguities in both the map and in the states. Our solution can efficiently handle both dense and sparse map representations by Rao-Blackwellization of conditionally linear and conditionally linearized models. We show through simulations and real-world experiments how the principles apply to radio (BLE/Wi-Fi), magnetic field, and visual SLAM. The proposed solution is general, efficient, and works well under confounding noise.

Figures (7)

• Figure 1: Illustration of the different SLAM modalities used throughout this paper. (a) Visual SLAM uses a sparse map representation of distinctive corner points observed as projections onto the camera frustum; (b) radio SLAM either uses a sparse map of the radio emitter locations or models the RSSI anomalies as a dense field; (c) magnetic SLAM leverages anomalies in the local magnetic vector field
• Figure 2: Left: A magnetic anomaly map mapped by a robot (smartphone for scale) equipped with a 3-axis magnetometer. Map opacity follows the marginal variance (uncertainty), and mapping (training) paths shown by dashed lines. Right: Localization by map matching. Current estimate is characterized by a particle cloud, the dashed line shows the ground-truth, and the solid line the weighted mean path
• Figure 3: Highlight of the non-degeneracy of the particle smoothing approach with (a) the simulation setup, and simulation results for radio SLAM with (b) filtering samples showing that the filter is degenerate and (c) non-degenerate samples of the smoother
• Figure 4: (a) Illustrative example of a back and forth path on an RSSI map; (b) the only source of uncertainty in the odometry is the turn angle at the farthest most point, which leads to spread; (c--d) Particle filtering SLAM reduces the spread, but does not backtrack the smooth odometry information; (e) Our particle smoothing solution gives a tighter estimate and backtracks the smoothness along the sample paths
• Figure 5: Robustness study with magnetometer perturbed with a constant bias $o$. Left: Setup showing the ground truth path and one realization of the magnetic field map. Right: Box plots of RMSE in the final estimated SLAM path. Particle and extended Kalman filtering methods drawn with outlines, particle smoothing method in solid colours. The EKF performs well under negligible calibration errors, the particle filter (PF) and smoother (PS) perform well under large calibration errors