Nonparametric localization: histogram grids, particle filters, recovery from kidnapping, and dynamic environments.
The EKF localization algorithm from Chapter 7 is limited to position tracking. It can't solve global localization because a single Gaussian can't represent "I might be here or there or over there." This chapter introduces two algorithms that break through this barrier:
| Algorithm | Representation | Capabilities |
|---|---|---|
| Grid localization | Histogram over a pose grid | Global localization, multi-modal beliefs |
| Monte Carlo localization (MCL) | Weighted particle set | Global localization, kidnapped robot, dynamic environments |
Both algorithms are direct implementations of the Bayes filter from Chapter 2, using the nonparametric representations from Chapter 4. The difference from EKF localization is purely in the representation of the belief.
Grid localization divides the pose space (x, y, θ) into a grid of cells. Each cell holds a probability value pk,t. The algorithm is the discrete Bayes filter applied to localization:
The prediction step sums over all cells, applying the motion model. The update step multiplies by the measurement likelihood and normalizes.
A typical indoor grid uses 15 cm resolution in x and y, and 5 degrees in θ. For a 50m × 50m environment, that's roughly 330 × 330 × 72 ≈ 8 million cells. This is a lot, but manageable with careful implementation.
The resolution of the grid creates a fundamental trade-off:
| Resolution | Accuracy | Computation | Use Case |
|---|---|---|---|
| Fine (15cm, 5°) | High | Very expensive | Metric localization |
| Coarse (rooms, junctions) | Low | Cheap | Topological navigation |
Coarse grids require noise inflation. When a grid cell is large, the measurement model p(z|x) varies wildly within a single cell. Evaluating it only at the cell center gives poor results. The standard fix: inflate the noise variance by half the cell diameter. This smooths out the likelihood function but loses information.
Motion on coarse grids is also tricky. If the robot moves 10cm/sec but the grid cells are 1m wide, the naive algorithm never transitions between cells. The fix: add random transitions to adjacent cells with probability proportional to (motion distance) / (cell diameter).
Advanced implementations use selective updating: only recompute cells where the belief is significant, skipping the vast majority of zero-probability cells. This can speed up grid localization by orders of magnitude.
Monte Carlo Localization (MCL) is the particle filter applied to localization. It represents the belief as a set of M weighted particles: Xt = {(xt[m], wt[m])}m=1..M.
The algorithm, at each time step:
1. Sample a new pose for each particle from the motion model: xt[m] ~ p(xt | ut, xt-1[m])
2. Weight each particle by the measurement likelihood: wt[m] = p(zt | xt[m], m)
3. Resample: Draw M new particles with replacement, probability proportional to weights.
For global localization, particles are initialized uniformly over the map. As measurements arrive, particles in wrong locations get low weights and are eliminated during resampling. Particles near the true pose survive and multiply.
MCL has several important properties that distinguish it from other localization methods:
| Property | Description |
|---|---|
| Consistency | As M → ∞, the particle distribution converges to the true posterior |
| Anytime | Works with any number of particles; more = better approximation |
| Multi-modal | Naturally represents multiple hypotheses |
| Non-parametric | No assumptions about the shape of the belief |
| Adaptive | Can adjust particle count based on uncertainty (KLD-sampling) |
Particle deprivation is MCL's Achilles heel. Once all particles have converged to one location, there are no particles left in other regions. If the robot is kidnapped to a new location, no particles will be near the true pose, and the algorithm cannot recover.
A related issue: during global localization in a large environment, the number of particles needed to adequately cover the space can be enormous. In a 50m × 50m building with 360° of heading, you might need millions of particles to have even one near the true pose.
Standard MCL cannot recover from kidnapping because of particle deprivation. The fix: random particle injection. At each step, add a small fraction of uniformly random particles to the set.
But how many random particles? Too few and recovery is slow. Too many and the algorithm wastes resources exploring impossible poses. The clever solution monitors the average measurement likelihood of the particle set:
When w̄t is high, the particles explain the data well — no recovery needed. When w̄t drops sharply, the particles are in the wrong place — inject more random particles proportional to the drop.
This augmented MCL can solve the kidnapped robot problem: when the robot is moved, the average weight drops, random particles are injected near the true pose (guided by the measurement), and within a few steps the filter converges to the correct location.
Standard MCL uses the motion model as its proposal distribution: new particles are sampled from p(xt | ut, xt-1), then weighted by p(zt | xt, m). This is simple but can be inefficient.
The problem: the motion model doesn't know about the current measurement. It might propose particles that are clearly wrong given what the sensor says. Those particles get near-zero weight and are wasted.
Improved proposal: Sample from p(xt | ut, xt-1, zt) — the motion model combined with the measurement. This concentrates particles where they're actually needed.
| Proposal | Pros | Cons |
|---|---|---|
| Motion model only | Simple, no extra computation | Wastes particles in low-likelihood regions |
| Measurement-guided | Concentrates particles where they matter | More complex, may miss surprises |
| Hybrid | Best of both worlds | Requires tuning the mixing ratio |
Real environments are dynamic: people walk around, doors open and close, furniture moves. These changes violate the static-world assumption underlying our sensor models.
The effect on localization: a person standing between the robot and a wall produces short range readings. The beam model handles this via pshort, but the likelihood field model does not — the person's legs look like walls in positions where no wall exists.
Filtering strategy: Preprocess the sensor data to remove readings caused by dynamic objects. For each beam:
1. Compare the measured range to the expected range (from the map).
2. If the measured range is much shorter than expected, the beam likely hit a dynamic object — discard it.
3. Only use beams whose ranges are consistent with the static map.
An alternative: include dynamic objects in the state vector. But this turns localization into a tracking problem with potentially hundreds of moving objects — far too expensive for real-time operation.
Watch Monte Carlo Localization solve the global localization problem. Particles start spread uniformly across a 1D hallway with 3 doors. As the robot senses and moves, particles converge to the true position.
Particles (orange dots) start uniform. Sensing a door assigns higher weights to particles near doors. Resampling eliminates bad particles. Click Step to advance.
| Algorithm | Position Tracking | Global Localization | Kidnapped Robot | Dynamic Env. |
|---|---|---|---|---|
| EKF (Ch 7) | Yes | No | No | No |
| Grid | Yes | Yes | Limited | Limited |
| MCL | Yes | Yes | With augmentation | With filtering |
Key lessons from this chapter:
• Grid localization is the discrete Bayes filter on a pose grid. Resolution matters — too fine is slow, too coarse is inaccurate.
• MCL uses particle filters. It is the dominant localization algorithm in practice.
• Standard MCL suffers from particle deprivation. Augmented MCL with random particle injection solves the kidnapped robot problem.
• Dynamic environments require sensor data filtering to remove transient objects.