Particle-filter SLAM: FastSLAM and grid-based mapping

EKF-SLAM keeps one big Gaussian over the robot and every landmark, and pays a quadratic price for it. Particle-filter SLAM takes a different route, based on a simple but powerful observation: if the robot's trajectory were known exactly, the landmarks would be independent of each other, and each could be estimated with its own tiny filter. So sample the trajectory with particles, and give each particle its own map. This idea, called Rao-Blackwellization, produced FastSLAM in 2002 and, for occupancy grids, GMapping in 2007, which for a decade was the default 2-D lidar SLAM package in ROS. This lesson explains the factorization, the FastSLAM algorithm, grid-based Rao-Blackwellized SLAM, and the particle depletion problem that ultimately limits the approach.

The key factorization

Look again at the SLAM network of lesson 5. Landmarks are connected to each other only through the robot poses that observed them. If all those poses are known, the landmarks become conditionally independent. Mathematically, the full SLAM posterior factorizes exactly as

The first factor, the posterior over trajectories, is hard and non-Gaussian, so it is represented with particles. Each remaining factor, the posterior over one landmark given a known trajectory, is a small, nearly linear estimation problem, so it is represented with a 2 × 2 EKF. Rao-Blackwellization is the general technique of sampling only some variables and computing the rest analytically conditioned on the samples; it reduces the variance of the estimate and the number of particles needed. Kevin Murphy and Arnaud Doucet applied it to grid maps in 1999 and 2000, and Michael Montemerlo, Sebastian Thrun, Daphne Koller and Ben Wegbreit introduced FastSLAM for landmark maps in 2002.

The FastSLAM algorithm

Each FastSLAM particle k holds a pose \( x_t^{[k]} \) (implicitly a whole trajectory, through its history) and, for each landmark j, a mean \( \mu_j^{[k]} \) and covariance \( \Sigma_j^{[k]} \). Each time step runs four stages. First, sample a new pose for each particle from the motion model, exactly as in Monte Carlo localization. Second, for each observed landmark, perform data association per particle (each particle can make its own association decision, which is a major robustness advantage) and update that particle's landmark EKF with the measurement, or initialize a new landmark. Third, compute each particle's importance weight, which is the likelihood of the measurement under that particle's map: a Gaussian with the innovation covariance \( Q_j = H \Sigma_j H^{\mathsf{T}} + Q \). Fourth, resample.

Each particle's landmark update is cheap, because it involves only that landmark's 2 × 2 filter. Stored naively, copying maps during resampling costs O(N) per particle; storing landmarks in a balanced tree shared between particles reduces this to \( O(M \log N) \) per step for M particles, which is what makes the method fast.

FastSLAM 2.0 (2003) improved the proposal distribution. Instead of sampling the new pose from odometry alone, it samples from a distribution that also takes the current measurement into account, computed by linearizing around the predicted pose. When the sensor is much more accurate than odometry, which is the usual case with a lidar, this places new particles where the measurement says the robot is, so far fewer particles are wasted, and the filter works even with a single particle in favorable conditions.

import numpy as np
rng = np.random.default_rng(11)
LM = np.array([[5, 4], [10, -3], [15, 4], [20, -3], [25, 4], [12, 9], [12, -8]], float)
M, Q = 60, np.diag([0.15, np.radians(2)]) ** 2
def wrap(a): return (a + np.pi) % (2 * np.pi) - np.pi
class P:                                      # one particle: pose + per-landmark EKFs
    def __init__(s): s.x = np.zeros(3); s.mu = {}; s.S = {}
parts = [P() for _ in range(M)]; true = np.zeros(3)
for k in range(30):
    true += [1.0, 0, 0]                         # drive +x at 1 m/step
    for p in parts:                             # 1) sample from noisy odometry
        th = p.x[2] + rng.normal(0, np.radians(1.0)); d = 1.0 + rng.normal(0, 0.05)
        p.x = p.x + [d * np.cos(th), d * np.sin(th), th - p.x[2]]
    w = np.ones(M)
    for j, L in enumerate(LM):
        dd = L - true[:2]; r = np.hypot(*dd)
        if r > 7: continue
        z = np.array([r, wrap(np.arctan2(dd[1], dd[0]) - true[2])]) + rng.normal(0, [0.15, np.radians(2)])
        for i, p in enumerate(parts):           # 2) per-particle landmark EKF, 3) weight
            if j not in p.mu:
                p.mu[j] = p.x[:2] + z[0] * np.array([np.cos(z[1] + p.x[2]), np.sin(z[1] + p.x[2])])
                p.S[j] = np.eye(2) * 0.5; continue
            e = p.mu[j] - p.x[:2]; q = e @ e; sq = np.sqrt(q)
            H = np.array([[e[0] / sq, e[1] / sq], [-e[1] / q, e[0] / q]])
            zh = np.array([sq, wrap(np.arctan2(e[1], e[0]) - p.x[2])])
            Qj = H @ p.S[j] @ H.T + Q; inn = z - zh; inn[1] = wrap(inn[1])
            K = p.S[j] @ H.T @ np.linalg.inv(Qj)
            p.mu[j] = p.mu[j] + K @ inn; p.S[j] = (np.eye(2) - K @ H) @ p.S[j]
            w[i] *= np.exp(-0.5 * inn @ np.linalg.solve(Qj, inn)) / np.sqrt(np.linalg.det(2 * np.pi * Qj))
    w /= w.sum(); neff = 1 / np.sum(w ** 2)
    if neff < M / 2:                            # 4) resample only when needed
        idx = rng.choice(M, M, p=w)
        new = []
        for i in idx:
            c = P(); c.x = parts[i].x.copy(); c.mu = {j: v.copy() for j, v in parts[i].mu.items()}
            c.S = {j: v.copy() for j, v in parts[i].S.items()}; new.append(c)
        parts = new; w = np.ones(M) / M
best = parts[int(np.argmax(w))]
err = np.mean([np.hypot(*(best.mu[j] - LM[j])) for j in best.mu])
print("landmarks mapped: %d, mean landmark error: %.2f m" % (len(best.mu), err))
print("robot position error: %.2f m after 30 m" % np.hypot(*(best.x[:2] - true[:2])))

landmarks mapped: 5, mean landmark error: 0.32 m
robot position error: 0.77 m after 30 m

Rao-Blackwellized particle filters for occupancy grids

The same factorization works when the map is an occupancy grid: each particle carries its own trajectory and its own grid, built with the mapping algorithm of lesson 4 from that particle's poses. The weight of a particle is the likelihood of the current scan under that particle's map. Carrying a full grid per particle is expensive, so the number of particles must be kept small, which demands a good proposal distribution and careful resampling.

GMapping, by Giorgio Grisetti, Cyrill Stachniss and Wolfram Burgard (2007), made this practical with two improvements. First, an improved proposal: for each particle it runs a scan matcher starting from the odometry prediction, finds the peak of the scan likelihood, and samples the new pose from a Gaussian approximated around that peak. Because lidar scan matching is far more precise than odometry, particles land in the right place, and 30 to 100 particles suffice for buildings that would otherwise need thousands. Second, selective resampling: it resamples only when the effective sample size \( N_{eff} = 1/\sum_k (w^{[k]})^2 \) drops below half the number of particles, which slows the loss of diversity. GMapping was the standard ROS 1 SLAM package for 2-D lidars, and its ideas live on in many systems even though graph-based methods such as Cartographer and slam_toolbox have largely replaced it in ROS 2.

Particle depletion and loop closure

The fundamental limitation of particle-filter SLAM is particle depletion. The filter represents a distribution over whole trajectories, but resampling repeatedly throws away particles and duplicates others. After many resampling steps, all surviving particles descend from a few ancestors, so their histories, and therefore their maps, are nearly identical for the early part of the trajectory. When the robot finally closes a large loop, the filter needs a particle whose history happens to be consistent with the loop; if all particles share the same slightly wrong early history, none of them is, and the loop cannot be closed correctly. The longer the loop, the more particles are needed to keep enough diversity, and the number grows rapidly with loop length.

Remedies include better proposals (so fewer resamplings are needed), selective resampling, adaptive particle counts, and recovering diversity before a loop closure is expected. In practice, these methods work well for buildings with loops of a few hundred meters and struggle beyond that. Graph-based SLAM, which revisits the whole trajectory with an optimizer, has no equivalent limitation, which is the main reason it became dominant.