Section 29.3: Localization (Monte Carlo / particle filters)

"A particle filter is a map of competing explanations, not just a pose estimate."

A Loop Closure That Came Back With Receipts

Problem First

Picture a robot stopped in a corridor of four identical doors, equally sure it stands before each one: a single Gaussian forces it to bet on a single door and lose that bet three times out of four. Particle filters solve this by representing belief as many weighted hypotheses instead of one mean. In that same hallway, a single-Gaussian filter is wrong roughly 75 % of the time (by symmetry, assuming a uniform prior over the four doors); a 500-particle filter carries all four hypotheses, collapses to the correct door after two turns, and reduces wrong-door delivery to under 3 % in simulation studies (as of 2024). The Bayesian filtering machinery that makes this possible is the same one formalized for pose below.

Monte Carlo localization (MCL), the particle-filter method this section develops, alternates prediction, measurement weighting, and resampling. As Figure 29.3.1 suggests, the technique becomes useful only when the visual idea of a particle cloud is tied to a concrete state variable, an uncertainty model, and the robot's next action. Particles spread under motion noise, then measurements increase the weight of poses whose expected observations match the sensor data through a sensor noise and uncertainty model. Resampling focuses compute on likely poses while preserving enough diversity to recover from ambiguity. Figure 29.3.2 diagrams this predict-weight-resample loop and the way the particle cloud spreads, gains weight, and concentrates at each stage. A filter that discards its competing hypotheses too early is not localizing: it is guessing and forgetting the alternatives.

Resampling matters in embodied systems because an onboard processor cannot maintain thousands of particles indefinitely. Without resampling, unlikely poses consume as much compute as likely ones, and the filter degrades under real-time constraints. The scale of this waste is striking. In a 500-particle filter after ten motion steps with no resampling, fewer than 10 particles typically carry over 95% of the total weight. The other 490 particles burn CPU time to represent hypotheses the sensor data has already ruled out. A robot that misses its resampling window reaches the planner with a stale, spread-thin belief, and it may halt or execute the wrong maneuver.

Mechanically, resampling draws a fresh set of N particles by sampling the current weighted set with replacement, so high-weight particles are duplicated and low-weight particles are discarded. The new set has uniform weights, and the distribution now concentrates around likely poses. Low-variance (systematic) resampling sweeps a single evenly-spaced pointer across the cumulative weight array, ensuring each probable region retains at least one representative particle without the clustering that pure random sampling can introduce.

Formal Model

Those reporting requirements only make sense once the underlying belief is written down precisely, so the particle cloud is expressed here as a formal posterior. For Monte Carlo localization, the posterior is a weighted particle set over robot pose conditioned on motion and sensor likelihoods. The key interface is not the mean pose alone; it is mode count, effective sample size (a single number, formally introduced in the algorithm below, that estimates how many particles are actually contributing useful information after weighting), and covariance before planning commits.

Effective sample size is what the Algorithm callout below computes at each step and the Worked Diagnostic section quantifies on a concrete example; keep that forward reference in mind as the term recurs.

$$ w_t^{(i)} \propto w_{t-1}^{(i)}p(z_t\mid x_t^{(i)},m),\quad \hat{x}_t=\sum_i w_t^{(i)}x_t^{(i)} $$

The evidence terms are proposal motion, sensor likelihood, resampling rule, particle count, and map quality. A particle filter earns trust when it practices structured uncertainty instead of premature commitment, preserving ambiguity long enough instead of collapsing onto the wrong corridor. This same weighted-hypothesis posterior is what the graph-based and visual SLAM methods reuse once the map itself must be estimated jointly with the pose.

Worked Diagnostic

Code Fragment 1 is the particle-filter diagnostic: update a tiny particle set, inspect weight normalization and resampling, then check whether ambiguity is preserved in a symmetric observation.

# Weight three pose hypotheses from range residuals.
# Smaller residuals receive larger likelihood before normalization.
import numpy as np

residuals_m = np.array([0.10, 0.60, 1.20])
sigma_m = 0.35
weights = np.exp(-0.5 * (residuals_m / sigma_m) ** 2)
weights = weights / weights.sum()
print(np.round(weights, 3))
print(f"effective_particles={1.0 / np.sum(weights ** 2):.2f}")

Tool Workflow

Hand-tracing the weight and resampling math by hand builds the intuition, but once that intuition is in place you reach for a maintained stack rather than reimplementing the sensor model from scratch.

Validate likelihoods and resampling with the hand update, then scale on AMCL, Nav2, or a factor-graph backend (an alternative estimator that represents poses and constraints as a graph and solves for all of them jointly, covered in section 29.5, rather than tracking a particle cloud). The hand test catches silent normalization errors that leave particles looking plausible while behaving badly.

Choosing the particle count N itself is not a fixed constant to memorize but a budget set against two failure modes covered above: too few particles and the filter cannot represent multiple hypotheses through an aliased corridor (deprivation risk climbs); too many and the per-step compute cost threatens the real-time budget the planner needs. Nav2 AMCL exposes this directly as min_particles and max_particles, and uses KLD-sampling (Kullback-Leibler divergence sampling) to shrink N automatically once the particle spread over occupied cells is tight enough that more particles would not change the estimate. In practice, start near the low end of a few hundred particles for a corridor-scale 2D map, watch the effective sample size and pose-jump rate from the Practical Example log, and raise max_particles only if aliasing episodes leave too few surviving particles per hypothesis to recover.

Consider a specific case. A robot runs Nav2 AMCL with 500 particles in a warehouse corridor lined with identical shelving units spaced 2 m apart. After a 10 m straight run, the effective particle count (ESS) is 420, which appears healthy. But those 420 particles cluster at three equally-spaced positions 2 m apart, each with weight near 0.33. The robot then turns into a geometrically unique cross-aisle. One cluster suddenly receives all the likelihood weight, the other two lose it, and AMCL snaps at random to one of the three candidate poses rather than the true one. The symptom is a sudden 2 m jump in the estimated pose even though odometry showed a smooth path. This is sensor aliasing producing a confident wrong answer, not a noisy uncertain one.