The Cloud That Knows Where It Is

Chapter 1: A Belief Made of Samples

Let's make the idea precise. Stanford's Lecture 13 puts it in one line: represent the posterior belief by a set of random samples. Those samples are called particles. We collect the particles at time t into a set:

Read that carefully. Each x_t^[m] is one particle — a complete hypothesis about what the true state might be at time t. For a wheeled robot, each particle is itself a full pose: a triple (x, y, θ). The superscript [m] indexes which particle (the m-th of M); it is not an exponent. And M is the total number of particles — the size of the cloud.

Why does a pile of dots approximate a probability distribution? Imagine you wanted to describe a bell curve to a friend over the phone, but you could only send them dots. You would scatter dots thickly where the curve is tall and sparsely where it is short. If your friend then drew a histogram of your dots, it would recover the bell curve. That is exactly what particles do: they are samples drawn from the distribution, so the histogram of the cloud reconstructs the distribution.

Formally, the particle filter wants its particles to be distributed according to the belief itself:

The squiggle ∼ means "is sampled from." The right-hand side is the belief: the probability of state x_t given all measurements z_1:t and all controls u_1:t up to now (the colon means "from step 1 through step t"). So a particle is a single random draw from the robot's current belief about its own pose.

How many particles is enough?

The cloud is a sampled portrait of the belief, so more particles means a finer portrait — just as more dots make a smoother histogram. But the cost grows linearly with M (every step touches every particle), and the accuracy improves only like 1/√M — the usual Monte Carlo rate. Quadrupling the particles only halves the sampling error. So you pick the smallest M that keeps the cloud from looking ragged. Play with it below: drag M and watch how faithfully the cloud reconstructs a known two-peaked belief.

That 1/√M rate is worth dwelling on, because it cuts both ways. The good news: it does not depend on the shape of the distribution — a two-peaked belief is no harder to approximate than a single bump, which is the entire reason particle filters can be multimodal for free. The bad news: it is a slow rate. Going from M = 100 to M = 10,000 (a hundredfold cost increase) buys only a tenfold accuracy gain. This is why nobody runs a particle filter with a million particles to chase precision — past a point you get diminishing returns, and the right move is a different representation, not more dots. For a 3-D robot pose, M between a few hundred and a couple thousand is the sweet spot, and adaptive schemes (KLD-sampling) even shrink M automatically when the cloud is tightly converged and grow it when the robot is lost.

Notice what you can do with this representation that a Gaussian forbids: the true curve has two peaks, and the cloud reproduces both. Try dragging M low — with only 15 particles the histogram is jagged and one peak might vanish entirely (a foreshadowing of particle depletion, Chapter 5). Crank M up and the histogram snaps to the curve. That dial — samples versus fidelity — is the whole engineering tradeoff of a particle filter.

Reading a number off the cloud

If the belief is just a pile of dots, how do you get an answer — a single estimate of where the robot is — out of it? You compute statistics of the cloud, exactly as you would for any sample set. The most common is the mean estimate: average the particles' positions.

That is the cloud's center of mass — the orange estimate you'll see in the showcase. (If the particles still carry weights rather than being equal, you use the weighted mean, Σ w^[m] x^[m].) You can also read off the spread (how confident the robot is) as the cloud's standard deviation, or, when the cloud is multimodal, you can cluster it and report each clump as a separate hypothesis — something a Gaussian's single mean simply cannot do.

Chapter 2: Predict, Weight, Resample — The Loop

A particle filter is the Bayes-filter recursion done on a cloud. Every cycle has exactly three beats. Burn them into memory; everything else in this lesson is detail on one of these three.

Beat 1 — Predict (the motion model)

The robot just executed a control u_t (say, "drive forward 1 m, turn 10°"). We move every particle as if it were the true robot obeying that command. But wheels slip and odometry lies, so we don't move each particle deterministically — we sample a noisy outcome from the motion model:

The bar over x̄ marks a predicted particle (before we've looked at any measurement). Concretely, for each particle we compute where the command should take it, then add a little random jitter to its x, y, and θ. A cloud that started tight gets a bit fuzzier — motion always adds uncertainty, exactly as in the Bayes filter's predict step.

For a wheeled robot, the per-particle prediction is just the kinematics from Lesson 2 with noise stapled on. Given a command "drive distance d, turn angle δθ," each particle (x, y, θ) updates as:

where ε_d and ε_θ are small random draws from the motion-noise model — the slip in the wheels, the imperfection in the odometry. The crucial detail: every particle draws its own independent noise. So a cloud that was a tight dot before the move fans out into a little smear afterward, because each particle "imagines" a slightly different outcome of the same command. That fanning-out is the prediction step adding uncertainty — it is the cloud's honest admission that motion is imperfect and it now knows less precisely where it is.

Beat 2 — Weight (the measurement model)

Now the robot takes a measurement z_t (a laser range, a landmark bearing). We ask each predicted particle: if you were the true robot, how likely is this measurement? That likelihood is the particle's importance weight:

A particle whose predicted pose would have produced something close to the actual measurement gets a big weight; one that would have seen something completely different gets a tiny weight. Crucially, we do not move the particles in this step — we only attach a number to each. The cloud is now a set of weighted hypotheses.

Beat 3 — Resample (survival of the fittest)

Finally we build the new cloud 𝒳_t by drawing M particles with replacement from the predicted set, each chosen with probability proportional to its weight. High-weight particles get copied many times; low-weight particles vanish. After resampling the weights reset to equal, and the population — how many copies of each pose survived — now encodes the belief. Stanford calls this "a probabilistic implementation of the Darwinian idea of survival of the fittest."

A tiny worked example: 3 particles, one step

Let's run all three beats on a 1-D toy with M = 3, so we can do every number by hand. The robot moves along a line. Particles start at positions 2.0, 5.0, 8.0.

Predict. The command is "move +1.0." We add a sampled jitter to each (say the dice give +0.3, −0.2, +0.1):

Weight. The robot now measures "I am near position 6.0" with measurement noise σ = 1.0. Each particle's weight is the Gaussian likelihood of that measurement given the particle's predicted pose — we'll derive this exact formula in Chapter 3, but for now: the closer a particle is to 6.0, the bigger its weight.

Particle [2] is the clear favorite — it predicted a pose right where the robot measured itself to be. Resample. Normalizing the weights gives probabilities 0.062, 0.930, 0.008. Drawing 3 new particles with those odds, we'd almost certainly get three copies of particle [2] (pose ≈ 5.8) and drop [1] and [3]. The new cloud has collapsed onto the measurement — the correction happened.

Step through it live:

That is the entire algorithm. Stanford's pseudocode (Algorithm 1) is the same three beats in a loop over m: sample a predicted particle, compute its weight, collect into a temporary set; then resample M times with probability proportional to weight. Written in words, beat by beat:

Algorithm: Particle_Filter
# inputs: prior particle set X_{t-1}, control u_t, measurement z_t
# output: posterior particle set X_t

X_bar = empty      # the temporary (predicted + weighted) set
X     = empty      # the new posterior set

for m = 1 to M:                          # --- PREDICT + WEIGHT ---
    x_bar[m] = sample from p(x_t | u_t, x_{t-1}[m])   # push particle through motion model
    w[m]     = p(z_t | x_bar[m])                       # importance weight = measurement likelihood
    add (x_bar[m], w[m]) to X_bar

for m = 1 to M:                          # --- RESAMPLE ---
    draw index i with probability proportional to w[i]
    add x_bar[i] to X

return X

Map each line back to the three beats. The first loop is Predict (sample from p(x_t | u_t, x_{t-1})) and Weight (w[m] = p(z_t | x_bar[m])) fused together — one pass over the cloud. The second loop is Resample: M draws, each picking a survivor with probability proportional to its weight. Notice the new set X may contain the same particle several times (heavy ones) and omit others entirely (light ones) — that's resampling with replacement.

Here is that pseudocode as runnable Python, the whole filter in fifteen lines, building on the weighting and resampling helpers we'll define in Chapters 3 and 4:

python
import numpy as np

def particle_filter_step(particles, u, z):
    # 1. PREDICT: move every particle by the control + sampled noise
    particles = motion_model(particles, u) + np.random.randn(*particles.shape) * motion_sigma
    # 2. WEIGHT: score each particle by how well it explains z
    w = measurement_likelihood(z, particles)   # p(z | x) per particle
    w = w / w.sum()                            # NORMALIZE (Chapter 3)
    # 3. RESAMPLE: draw a new cloud, each particle proportional to its weight
    particles = low_variance_resample(particles, w)   # (Chapter 4)
    return particles

That's it. The entire conceptual content of a particle filter fits on a postcard. The next three chapters fill in the two helpers — measurement_likelihood (Chapter 3) and low_variance_resample (Chapter 4) — and the one safety valve, N_eff gating (Chapter 5). Everything after that is applying this loop to robot localization on a map.

Chapter 3: Importance Weighting — Scoring a Guess

The weight is the heart of the correction step, so let's understand exactly where it comes from and how to compute it by hand. The weight answers one question per particle: given that this particle's pose were true, how probable is the measurement we actually got?

The Gaussian measurement model

Suppose the robot's sensor reports a scalar measurement z (a range, say), and the true relationship is "the sensor reads the distance plus zero-mean Gaussian noise of standard deviation σ." Then for a particle that predicts an expected measurement ẑ^[m] (what the sensor should read if that particle were true), the likelihood of the actual reading z is the Gaussian bell evaluated at the gap between them:

Define each symbol. z is the actual measurement. ẑ^[m] is the measurement this particle predicts — computed by running the sensor model forward from the particle's pose (e.g. "if I were at this pose, the wall would be 2.3 m away"). σ is the sensor's noise standard deviation — how much you trust the reading. The exponent contains the squared error (z − ẑ)²: small error → exponent near 0 → weight near its peak; large error → large negative exponent → weight crushed toward zero.

Worked example: weighting 5 particles by hand

Five particles predict the expected ranges below. The robot's laser actually reads z = 4.0 m, with sensor noise σ = 1.0 m. We compute each weight, then normalize so they sum to 1.

Because the 1/√(2πσ²) factor is the same for every particle, it cancels during normalization — so for weighting we only need the un-normalized kernel exp(−(z−ẑ)² / 2σ²). With σ = 1, the denominator 2σ² = 2.

Walk row [4]: it predicted exactly 4.0, error 0, so exp(0) = 1 — the maximum. Row [5] predicted 1.0, off by 3.0, so exp(−9/2) = exp(−4.5) ≈ 0.011 — almost dead. The raw kernels sum to 0.980 + 0.882 + 0.325 + 1.000 + 0.011 = 3.198. Dividing each by 3.198 gives the normalized weights in the last column, which now sum to 1.0.

Multiple measurements multiply

Robots rarely take one reading. If a laser scan gives K independent range measurements, the joint likelihood is the product of the individual Gaussians (Stanford's conditional-independence assumption):

The big-∏ is a product over the K measurements. Why a product? Because the measurements are assumed conditionally independent given the pose (Stanford's standard assumption): once you fix where the robot is, the K beams are independent draws, and independent probabilities multiply. A particle must explain all the beams to score well — one badly-mispredicted beam can torpedo an otherwise-good particle, which is exactly the discriminating power we want.

The underflow trap and the log fix

That product is a numerical landmine. A single laser scan can have hundreds of beams; multiply a few hundred numbers each around 0.1 and you get something like 10⁻²⁰⁰ — smaller than the smallest float, so it silently rounds to exactly 0. Now every particle's weight is 0, the normalization divides by zero, and the filter produces NaNs that poison the whole cloud. The fix is the same logarithm trick the occupancy grid used in Lesson 1: work in log-likelihoods, where the product becomes a sum:

Sums of moderate negative numbers never underflow. To turn the log-weights back into a probability distribution for resampling, we use the log-sum-exp trick: subtract the maximum log-weight from all of them (this shifts everything up so the largest is 0, which doesn't change the ratios), then exponentiate and normalize. Subtracting the max guarantees the biggest term is exp(0) = 1, so nothing overflows and the dominant particles keep full precision.

From scratch in Python, scoring one particle against one Gaussian measurement looks like this:

python
import numpy as np

def gaussian_weight(z, z_pred, sigma):
    # likelihood of measurement z if this particle predicts z_pred
    err = z - z_pred
    return np.exp(-(err**2) / (2 * sigma**2))   # un-normalized kernel; constant cancels later

z_pred = np.array([4.2, 3.5, 5.5, 4.0, 1.0])
w = gaussian_weight(4.0, z_pred, sigma=1.0)   # raw weights
w = w / w.sum()                                # NORMALIZE -> a probability distribution
print(np.round(w, 3))   # [0.345 0.310 0.114 0.352 0.004]  -- matches the table

And the idiomatic vectorized form for a whole laser scan — sum log-likelihoods over K beams, then normalize in log-space for numerical safety:

python
def scan_weights(z, z_pred, sigma):
    # z, z_pred: (M, K) -- M particles, K beams each
    log_w = -((z - z_pred)**2).sum(axis=1) / (2 * sigma**2)
    log_w -= log_w.max()              # subtract max for stability (shifts, doesn't change ratios)
    w = np.exp(log_w)
    return w / w.sum()                # normalized importance weights

Drag the controls below to feel the weighting. Move particles, change the measurement, and shrink or widen σ. The bar heights are the normalized weights — watch how a tight σ concentrates all the weight on the one closest particle, while a loose σ spreads it out.

Chapter 4: Resampling — Survival of the Fittest

Weighting tells us which particles explain the data well, but it leaves us with an awkward object: a cloud where some particles are "heavy" and most are "light." If we kept iterating, almost all the computational effort would go to particles with negligible weight. Resampling fixes this by rebuilding the cloud so that population, not weight, carries the belief. We draw M new particles, each chosen with probability equal to its normalized weight, with replacement — so a heavy particle can be picked many times, and a light one not at all.

The naive way: multinomial resampling

The obvious method: to draw one particle, pick a uniform random number u in [0, 1) and find which particle's "slice" of the unit interval it lands in — where slice widths equal the normalized weights. Repeat M times, each draw independent. This is multinomial resampling, and it works, but it is noisy: with M independent draws, a particle with weight 0.2 might by bad luck get picked 0 times or 5 times out of M = 10. That sampling noise is itself a source of error — it can wipe out a good hypothesis purely by chance.

The better way: low-variance (systematic) resampling

The standard fix is low-variance resampling (also called systematic resampling). Instead of M independent random draws, we make one random draw and then step through the cloud at evenly-spaced intervals. Picture the normalized weights laid end-to-end as segments on a ruler of total length 1. We place M equally-spaced "comb teeth," spacing 1/M apart, with the first tooth at a random offset r drawn uniformly from [0, 1/M). Each tooth that lands in a particle's segment selects one copy of that particle.

Because the teeth are rigidly spaced, a particle whose weight occupies a fraction f of the ruler is guaranteed to be selected either ⌊fM⌋ or ⌈fM⌉ times — never wildly more or fewer. That eliminates the lottery-style variance of multinomial resampling. It is also faster: one pass, O(M), with a single random number.

Worked example: low-variance resample of 5 particles

Take the normalized weights from Chapter 3's table: 0.345, 0.310, 0.114, 0.352, 0.004. (They sum to 0.125 short of 1 due to rounding; let's use the clean set 0.35, 0.31, 0.11, 0.22, 0.01 which sums to exactly 1.00 for a tidy by-hand pass.) We resample M = 5 particles.

First, the cumulative weights — the right edge of each particle's segment on the ruler:

Spacing is 1/M = 1/5 = 0.20. Say the single random offset is r = 0.12. The five comb teeth fall at:

The new cloud is { [1], [1], [2], [3], [4] }: particle [1] survives twice (it was the heaviest), [2], [3], [4] survive once each, and particle [5] — weight 0.01 — is culled. Notice the cull is principled: [5]'s segment (0.99–1.00) is so thin no tooth landed in it. With multinomial draws, [5] might have gotten lucky and survived while a heavier particle died; low-variance resampling makes that essentially impossible.

From scratch in Python — the whole low-variance resampler is about eight lines:

python
import numpy as np

def low_variance_resample(particles, w):
    M = len(w)
    positions = (np.arange(M) + np.random.rand()) / M   # M evenly-spaced teeth, 1 random offset
    cumsum = np.cumsum(w)                                # right edges of each segment
    cumsum[-1] = 1.0                                  # guard against float round-off
    idx = np.searchsorted(cumsum, positions)             # which segment each tooth lands in
    return particles[idx]                              # copy the survivors; weights reset to 1/M

The verbose, explicit loop — the same algorithm written so every comb tooth is visible — helps if you've never seen the trick:

python
def low_variance_resample_explicit(particles, w):
    M = len(w)
    new = []
    r = np.random.rand() / M       # first tooth in [0, 1/M)
    c = w[0]                       # running cumulative weight
    i = 0
    for m in range(M):
        U = r + m / M              # position of the m-th tooth
        while U > c:                 # advance to the segment this tooth lands in
            i += 1
            c += w[i]
        new.append(particles[i])   # select that particle
    return np.array(new)

Press Resample below and watch the comb sweep the ruler. The bars are weights; the teeth are the M evenly-spaced sample points; the new cloud appears beneath. Run it repeatedly — notice that heavy particles always get roughly the right number of copies, with almost none of the random swing you'd see from independent draws.

Flip to multinomial mode and resample several times: the survivor counts jump around far more — sometimes a heavy particle gets unlucky, sometimes a light one gets copied. That extra jitter is "resampling variance," and it's exactly what the low-variance scheme is built to suppress.

Why multinomial is noisier — the same example, drawn independently

Re-run our 5-particle example, but this time the naive way. To draw one particle we pick a single uniform random number u in [0,1) and see which segment it lands in; we repeat that five independent times. Suppose the five independent draws come up u = 0.41, 0.08, 0.55, 0.40, 0.97. Tracing each against the cumulative edges (0.35, 0.66, 0.77, 0.99, 1.00):

This particular run gives { [1], [2], [2], [2], [4] } — particle [2] (weight 0.31) got picked three times, while the heavier particle [1] (weight 0.35) got only one copy and particle [3] (weight 0.11) got zero. With a different set of five random numbers you'd get a different count entirely. The expected count of each particle is M×weight (correct on average), but any single run can swing far from it. Low-variance resampling pins each particle to almost exactly its expected count, run after run — the comb's rigid spacing removes the lottery.

The two-phase cost of resampling

One subtlety worth internalizing: resampling does the measurement correction and destroys diversity in the same stroke. Copying winners is exactly how the measurement reshapes the belief — but copying means clones, and clones are not independent hypotheses. We've quietly created the problem the next chapter is entirely about. Hold that thought: resampling is necessary, but resampling too eagerly is the road to ruin.

Chapter 5: Particle Depletion — When the Cloud Goes Stale

Resampling is a double-edged sword. It refocuses the cloud onto promising regions — but every resample copies survivors and discards the rest, which means the cloud loses diversity. Copy a particle five times and you have five identical dots, not five independent hypotheses. Do this every step and the cloud can collapse onto a tiny number of distinct poses. Stanford calls the failure mode weight collapse; in the literature it's also called particle depletion or sample impoverishment.

Measuring health: the effective sample size N_eff

How do we know when the cloud is getting unhealthy? We compute the effective sample size, a single number that estimates how many of our M particles are "really pulling their weight":

The sum is over the squared normalized weights. Let's read the two extremes to see why this formula is exactly right:

• All weights equal (perfectly healthy): each w^[m] = 1/M. Then Σ(1/M)² = M·(1/M²) = 1/M, so N_eff = 1/(1/M) = M. Every particle counts — the maximum.
• One weight is 1, rest are 0 (total collapse): the sum is 1² = 1, so N_eff = 1. The whole belief rests on a single particle — the minimum.

So N_eff ranges from 1 (collapsed) to M (perfectly diverse). The standard rule: resample only when N_eff drops below a threshold, typically M/2. Above the threshold, the weights are healthy enough that you skip resampling and just carry the weights forward — preserving diversity for free.

Worked example: N_eff by hand

Take the clean weights from Chapter 4: 0.35, 0.31, 0.11, 0.22, 0.01 (M = 5). Square each and sum:

So out of 5 particles, the cloud is "effectively" about 3.6 useful ones — reasonably healthy (3.58 > M/2 = 2.5, so we'd skip resampling this round). Now compare a near-collapsed cloud, weights 0.92, 0.05, 0.02, 0.01, 0.00: Σw² = 0.8464 + 0.0025 + 0.0004 + 0.0001 = 0.8494, giving N_eff ≈ 1.18. That's well below 2.5 — this cloud is dying and we'd resample (and worry).

Two remedies for depletion

1. Adaptive resampling. As above: gate resampling on N_eff < M/2. Resample only when the cloud is genuinely concentrated, never on every step.

2. Roughening (jitter). After resampling, add a small amount of random noise to every particle's pose. This breaks up the clones — five identical copies become five slightly-different hypotheses again — restoring diversity at the cost of a little extra spread. It's the particle-filter equivalent of mutation in evolution: copying alone leads to a monoculture; a dash of mutation keeps the population varied.

Roughening has its own Goldilocks dial. Too little jitter and the clones barely separate — depletion is only delayed. Too much and you smear the belief, blurring a tight, confident estimate back into a fog and throwing away hard-won information. A common heuristic scales the roughening noise with the spread of the cloud and shrinks it as M grows (larger clouds need less artificial diversity). In practice the motion model's own process noise often is the roughening: because the predict step adds noise every cycle anyway, a filter that resamples sparingly and always moves between resamples rarely needs explicit roughening at all.

One more worked N_eff scenario

Suppose you have M = 1000 particles and, after a measurement, the normalized weights are spread so that effectively a few hundred carry most of the mass. You compute Σw² = 0.004, giving N_eff = 1/0.004 = 250. Your threshold is M/2 = 500. Since 250 < 500, you resample — the cloud has concentrated enough that carrying the uneven weights forward would waste particles. After resampling (with a touch of roughening) the weights reset to uniform, Σw² returns to 1000·(1/1000)² = 0.001, and N_eff snaps back to the full 1000. The next few steps, if the weights stay even, N_eff stays high and you skip resampling — preserving diversity until the cloud genuinely needs another cull.

python
def n_eff(w):
    return 1.0 / np.sum(w**2)          # w must be normalized

# adaptive resampling + roughening, the production pattern
if n_eff(w) < len(w) / 2:               # only when the cloud is concentrated
    particles = low_variance_resample(particles, w)
    particles += np.random.randn(*particles.shape) * rough_sigma   # roughen: re-inject diversity
    w = np.ones(len(w)) / len(w)         # weights reset to uniform

Watch depletion happen, then watch the fixes prevent it. Below, a cloud is repeatedly resampled. With resample every step the diversity (and N_eff) crashes toward 1 within a few rounds. Switch on adaptive + roughen and the cloud stays varied indefinitely.

Chapter 6: Monte Carlo Localization — The Cloud on a Map

Now we point the particle filter at the actual robot problem: localization — figuring out the robot's pose relative to a known map. Stanford defines the localization problem precisely as "determining the pose of a robot relative to a given map of the environment." When the belief is carried by particles, the algorithm has a name: Monte Carlo Localization (MCL).

The localization taxonomy — which problem are we solving?

Before the algorithm, a map of the territory (Stanford devotes a whole section to it). Localization problems split along several axes, and which filter you want depends on where your problem lands:

• Position tracking — initial pose known, only small incremental drift to correct. Unimodal, so a Gaussian filter (Lesson 13) is ideal and cheap.
• Global localization — initial pose unknown. The belief is genuinely multi-hypothesis, so you need a non-parametric filter. This is MCL's home turf.
• Kidnapped robot — global localization plus the possibility of sudden, undetected teleportation mid-operation. Hardest of all; needs MCL plus a recovery mechanism (Chapter 7).

There are further axes — static vs. dynamic environments (people moving around), passive vs. active (does the robot steer to localize better?), single- vs. multi-robot. This lesson, like Stanford's course, focuses on the core: local & global, static, passive, single-robot localization. Get that solid and the variations are extensions, not new ideas.

MCL is just the particle-filter loop with one addition: the map m enters both the motion model and the measurement model. The map is "a list of objects in the environment along with their properties" — for us, a set of point landmarks, each at a known global coordinate. Stanford's MCL pseudocode (Algorithm 4) is identical to the particle filter except that prediction and weighting both condition on m:

Global localization: the cloud starts everywhere

The thing MCL does that the EKF cannot is global localization — finding the robot when its initial pose is completely unknown. Stanford is explicit: position tracking (known start) suits Gaussian filters; global localization (unknown start) "is more appropriately addressed via non-parametric, multi-hypothesis filters, such as the particle filter."

The recipe is beautiful in its simplicity. For global localization, you initialize the cloud as a uniform distribution: scatter particles across the entire map, every reachable pose equally likely. The initial belief is bel(x₀) = 1/|X|, uniform over all states. Then you run the loop: sense, weight, resample, move. Each sensing throws weight onto particles whose pose would have produced the observed landmarks; resampling kills the rest. The cloud, initially a fog over the whole building, condenses step by step onto the true pose.

The landmark / ray-cast measurement model

How does a particle predict what it should see? For a range-and-bearing sensor — the workhorse of feature-based localization — a particle at pose (x, y, θ) predicts, for each landmark j at known position (m_j,x, m_j,y), an expected range and bearing:

The range r̂_j is just the Euclidean distance from the particle to the landmark. The bearing φ̂_j is the angle to the landmark in the robot's own frame — the global angle (the atan2) minus the robot's heading θ. The particle's weight is then the Gaussian likelihood (Chapter 3) of the actual sensor reading given these predictions, multiplied over all visible landmarks. A particle whose pose explains every landmark observation gets a high weight; one that would have seen the landmarks in the wrong direction or at the wrong distance gets crushed.

(For a dense laser in an occupancy map you'd instead ray-cast from the particle to find expected wall distances along each beam — same idea, different geometry. Lesson 12 built the occupancy map; here we use sparse landmarks to keep the arithmetic clean.)

Worked example: predicting and weighting one landmark

Make it concrete with numbers. A landmark sits at global position m_j = (10, 4). A particle hypothesizes the robot is at pose (x, y, θ) = (7, 4, 0) — i.e. 3 m west of the landmark, facing east (θ = 0). What range and bearing does this particle predict?

• Predicted range: r̂ = √((10−7)² + (4−4)²) = √9 = 3.0 m.
• Predicted bearing: the global angle to the landmark is atan2(4−4, 10−7) = atan2(0, 3) = 0 rad (due east). Subtract the heading θ = 0: φ̂ = 0 − 0 = 0 rad — the particle thinks the landmark is dead ahead.

Now the robot's actual sensor reports r = 3.2 m, φ = 0.05 rad, with range noise σ_r = 0.3 and bearing noise σ_φ = 0.1. The particle's weight is the product of two Gaussians (range and bearing are independent, so the joint likelihood factors):

A particle that predicted the landmark at (3.0 m, 0 rad) earns a healthy weight 0.71 because its prediction is close to the actual (3.2 m, 0.05 rad). Now imagine a rival particle on the opposite side of the landmark, hypothesizing pose (13, 4, π): it would predict range 3.0 (same distance!) but bearing 0 in its frame too — and if the world were symmetric, its weight could be just as high. That is how the multimodal belief arises naturally: two genuinely different poses can explain the same reading, and both keep their particles.

MCL from scratch

Here is the whole MCL loop in Python, landmark version. It scatters a uniform cloud, then runs predict–weight–resample. Note how the three beats from Chapter 2 map onto three blocks, and how the landmark measurement model from above (range to each landmark) is the heart of the weighting:

python
import numpy as np

def predicted_ranges(particles, landmarks):
    # particles: (M,3) [x,y,theta]; landmarks: (L,2). returns (M,L) ranges
    dx = landmarks[:, 0][None, :] - particles[:, 0][:, None]
    dy = landmarks[:, 1][None, :] - particles[:, 1][:, None]
    return np.hypot(dx, dy)

def mcl_step(particles, u, z, landmarks, mot_sig, sen_sig):
    # 1. PREDICT: move each particle by control u + independent noise
    particles = motion_model(particles, u)
    particles += np.random.randn(*particles.shape) * mot_sig
    # 2. WEIGHT: Gaussian likelihood of z given each particle (log-space)
    z_pred = predicted_ranges(particles, landmarks)         # (M,L)
    log_w  = -((z[None, :] - z_pred)**2).sum(axis=1) / (2 * sen_sig**2)
    log_w -= log_w.max()                                    # log-sum-exp stability
    w = np.exp(log_w); w /= w.sum()                         # normalize
    # 3. RESAMPLE only when the cloud is concentrated (Chapter 5)
    if 1.0 / np.sum(w**2) < len(w) / 2:
        particles = low_variance_resample(particles, w)
    return particles

# GLOBAL localization: start the cloud uniform over the whole map
M = 1000
particles = np.column_stack([
    np.random.uniform(0, map_w, M),
    np.random.uniform(0, map_h, M),
    np.random.uniform(0, 2*np.pi, M)])

Every line traces back to a chapter: the uniform initialization is global localization, the Gaussian log_w is Chapter 3's weighting, the Neff gate is Chapter 5, and low_variance_resample is Chapter 4. MCL is not a new algorithm — it's the particle filter you already built, pointed at a map.

Watch global localization converge. Below, a robot lives in a 2-D world with landmarks. Press Run and a cloud that starts as a uniform fog across the whole world condenses, over several sense-weight-resample cycles, onto the true pose (the white robot).

Chapter 7: The Kidnapped Robot — Recovering From Total Failure

Here is the hardest localization problem, and the reason it matters. Imagine your robot is happily tracking its pose — cloud nicely converged on one spot — when someone picks it up and sets it down somewhere completely different. Or, less dramatically, it slips on a wet floor, or a wheel encoder dies for a moment, and its pose estimate silently becomes garbage. This is the kidnapped robot problem: the robot must recover from a localization estimate that has gone catastrophically wrong — and, crucially, it must be able to do so even though it has high confidence in a now-false belief.

Why plain MCL fails to recover

Run standard MCL and kidnap the robot, and watch it fail. After convergence, every particle sits in the (now wrong) old location. The robot's new sensor readings match none of them — so every particle gets a tiny weight. But resampling normalizes the weights, so it dutifully resamples from the cluster of equally-terrible particles, keeping the cloud glued to the wrong spot. The fatal flaw: there is no particle at the true new pose, and the filter has no mechanism to create one. A cloud can only ever resample poses it already contains. Once diversity is gone, the truth is unreachable.

Picture it numerically. After convergence, all 1000 particles sit within a few centimeters of the old pose. The robot is kidnapped 8 m away. Now the sensor reads ranges consistent with the new location, but every particle predicts ranges for the old location — errors of meters, so every raw weight is essentially exp(−huge) ≈ 0. Normalization rescues the arithmetic (it divides the near-zeros by their near-zero sum, yielding a uniform-ish distribution over the doomed cluster), so the filter happily resamples 1000 fresh copies of... the same wrong cluster. It will do this forever. The cloud is a sealed terrarium with no door to the truth.

The fix: Augmented MCL (inject random particles)

The standard cure is Augmented MCL: at every step, replace a small fraction of the cloud with brand-new particles scattered randomly across the map. These random injections are the filter's way of always asking, "...or am I somewhere completely different?" Most of the time the random particles get low weights and die immediately — cheap insurance. But after a kidnap, those random particles are the only ones that can land near the true new pose. When one does, it gets a huge weight, resampling multiplies it, and within a few steps the cloud teleports to the correct location.

Trace the recovery. Step 1 after the kidnap: inject, say, 30 random particles (3% of 1000). By luck, one lands within sensing range of the true new pose; its predicted ranges roughly match the real sensor, so its raw weight dwarfs the 970 stranded particles (which all score ≈0) and the other 29 random ones (mostly elsewhere). After normalization that one good particle holds nearly all the mass. Step 2: resampling copies it dozens of times; now there's a small cluster at the true pose. Step 3: that cluster, refined by roughening and more measurements, tightens; the stranded old cluster has been resampled out of existence because it kept scoring 0. Within a handful of steps the cloud has migrated 8 m to the truth — something plain MCL could never do.

How many to inject? Augmented MCL does it adaptively: it tracks a short- and long-term average of the measurement likelihood, and when the short-term average suddenly drops below the long-term one — the tell-tale sign that the cloud has stopped explaining the data — it ramps up the injection rate. When localization is healthy, injection drops to near zero. A simpler-but-effective version just injects a fixed small fraction (say 2–5%) of random particles every step.

The likelihood-ratio detector, by hand

How does the filter know it's lost? The honest signal is the total measurement likelihood — the sum of the raw (un-normalized) weights before normalizing. When the cloud explains the data well, that sum is healthy; when every particle is far from the truth, the sum craters. Augmented MCL keeps two running averages of this quantity: a fast one w_fast and a slow one w_slow, and injects random particles in proportion to:

Read it: when things are stable, w_fast ≈ w_slow, the ratio is ≈1, and p_inject ≈ 0 — almost no injection. The instant a kidnap tanks the current likelihood, w_fast drops fast while w_slow lags behind, so the ratio falls below 1 and p_inject jumps — flooding the map with fresh particles exactly when the robot is lost. As recovery succeeds, the likelihood climbs back, the ratio recovers, and injection switches off. It is an automatic "am I lost?" alarm wired straight to the recovery mechanism.

python
# Augmented MCL: one step with random-particle injection
def augmented_mcl_step(particles, w, u, z, m, inject_frac=0.03):
    particles = motion_update(particles, u)          # predict
    w = measurement_weights(particles, z, m)         # weight (per Ch.3)
    w = w / w.sum()                                  # normalize
    if n_eff(w) < len(w) / 2:
        particles = low_variance_resample(particles, w)
    n_inject = int(inject_frac * len(particles))     # a few % of the cloud
    particles[:n_inject] = random_poses(n_inject, m) # scatter fresh particles over the map
    return particles, np.ones(len(particles)) / len(particles)

Kidnap the robot yourself. Below, MCL converges, then the Kidnap button teleports the true robot. Toggle Augmented off and the cloud sits stranded at the old pose; toggle it on and watch random particles seed the new location and the cloud recover.

Chapter 8: Particle Filter vs Kalman Filter — When to Use Which

You now know two whole families of Bayes filter: the parametric Gaussian filters of Lesson 13 (KF, EKF, UKF) and the non-parametric particle filter of this lesson. They solve the same recursion; they make opposite tradeoffs. Knowing which to reach for is a real engineering decision.

The catch: the curse of dimensionality

If particle filters are so flexible, why doesn't everyone use them for everything? Because of the brutal arithmetic of high-dimensional spaces. To cover a state space with particles densely enough to find the answer, the number of particles you need grows exponentially with the number of state dimensions. This is the curse of dimensionality.

The intuition: to tile a 1-D interval with particles spaced every 0.1 takes 10 particles. A 2-D square at the same resolution takes 10² = 100. A 3-D cube takes 10³ = 1000. A robot pose (x, y, θ) is 3-D — about a thousand particles cover it nicely, exactly Stanford's "M ≈ 1000." But a 7-joint robot arm's configuration is 7-D: at the same resolution, 10⁷ = ten million particles. A full SLAM state with dozens of landmarks is hopeless for a naive particle filter.

This is not a tuning problem you can engineer around — it's geometry. The number of particles needed for a fixed resolution is (1/spacing)^d, and exponentials in d defeat any constant-factor cleverness. The lesson: count your state dimensions before reaching for a particle filter.

Hybrids: the best of both worlds

The escape hatch, when part of your state is multimodal/nonlinear and part is comfortably Gaussian, is to split them. Rao-Blackwellized particle filters represent only the troublesome low-dimensional part with particles, and attach a small Kalman filter to each particle to handle the Gaussian remainder analytically. FastSLAM is the famous instance: it particle-filters the robot's trajectory (low-D, multimodal) while each particle carries a bank of tiny EKFs — one per landmark — for the map (Gaussian given the trajectory). You pay for particles only on the dimensions that truly need them. That's how a particle method scales to SLAM despite the curse — by refusing to particle-ize the dimensions a Gaussian already handles.

Compare the two filters side by side on the same nonlinear, multimodal problem. The Gaussian (EKF-style) belief is forced into one ellipse; the particle cloud is free to split. Step through and watch where the Gaussian goes wrong.

Chapter 9: Showcase — Drive the Robot, Watch the Cloud Breathe

Everything now lives in one simulation. Below is a robot in a known 2-D world with rooms and landmarks. The particle cloud begins as a fog scattered across the whole map (global localization). As you drive the robot with the controls, it senses ranges to nearby landmarks, weights the cloud, and resamples — and the cloud collapses onto the true pose. You can watch the belief breathe: it tightens when the robot sees distinctive landmarks, loosens when it drives blind.

Experiment to feel the tradeoffs, exactly the ones the chapters named:

• Drop M to 50. The cloud is ragged; it sometimes converges to the wrong clump in symmetric regions, because too few particles miss the true pose entirely (Chapter 1's fidelity-vs-cost dial, and Chapter 5's depletion).
• Crank Sensor σ up. The weighting goes forgiving; the cloud stays loose and converges slowly — a "low-trust" sensor (Chapter 3).
• Crank Sensor σ down to 0.3. The cloud snaps tight but is brittle: a single noisy reading can throw it, and it depletes faster.
• Crank Motion noise up. The cloud puffs out every step — the predict step adds uncertainty (Chapter 2). The filter needs frequent good measurements to stay converged.
• Kidnap with Augmented off. The orange estimate stays glued to the abandoned spot; error stays huge. Turn Augmented on and re-kidnap — recovery in a few steps (Chapter 7).

This is Monte Carlo Localization as it actually runs on real robots — ROS's amcl node is this exact algorithm (Adaptive Monte Carlo Localization) on an occupancy-grid map with a laser scanner. You just built its mental model from particles up.

Chapter 10: Connections & Cheat-Sheet

You now hold the non-parametric half of robot localization. Here is everything compressed, plus where it sits in the series and on the wider site.

Cheat-sheet

Where this sits in the series

Related lessons on Engineermaxxing

Go deeper on the pieces we touched:

• The Particle Filter — our standalone estimation-theory lesson on the same algorithm, with a different worked example set. The natural companion to this robot-autonomy framing.
• The Bayes Filter — the predict–correct recursion that both the Kalman and particle filters specialize. Start here if the loop felt abstract.
• Kalman, EKF & UKF — the Gaussian filters; read it back-to-back with this one to feel the parametric/non-parametric split.
• SLAM & Factor Graphs — how the map itself gets built, and where particle filters reappear inside FastSLAM.