M9-T2 — The Particle Filter Algorithm

Learning objectives

After this lesson, you should be able to:

• State the three steps of the (bootstrap) particle filter and relate each to a line of the Bayes filter.
• Explain why resampling is needed, and when to do it (the effective-sample-size rule).
• Compare systematic, stratified and multinomial resampling.
• Describe how accuracy depends on the number of particles by comparison with an exact grid filter.

The intuition

Picture 500 copies of the robot, each placed somewhere in the corridor: each is a hypothesis. The filter repeats three steps.

1. Move. Every particle drives forward by the commanded 25 cm plus its own random error. The cloud shifts and widens (the prediction step of the Bayes filter).
2. Weight. The sensor reports “door”. Each particle is given a weight equal to the probability of that reading if the robot were at the particle’s position: 0.9 for a particle standing at a door, 0.1 for one that is not (the correction step).
3. Resample. Draw a new set of 500 particles from the old one with probabilities proportional to the weights. Hypotheses that explained the reading well are copied, those that did not disappear. All weights go back to 1/500.

At step 1 the particles are spread everywhere (the robot has no idea where it is). After a few door readings (step 10) they form three clusters, one for each door the robot may have passed; the clusters carry unequal weight because the gaps between the doors are 5 m and 6 m and the robot has moved 0.25 m per step. At step 24, when the robot has passed the second door, the pattern of readings fits only one start, and almost all the particles have joined the cluster at the true position: 95% of them are within 1 m of the robot (the exact grid filter says 96%). By step 60 the cloud is a narrow peak with a standard deviation of 0.34 m.

A Gaussian filter could not do this: it would need to be started with a single hump and could not split it. The particle cloud splits and merges freely because each particle is its own hypothesis.

A particle filter is the Bayes filter with the integrals replaced by samples. Its accuracy is limited by the number of particles, not by the shape of the belief.

Definitions

Particle $x_t^{[i]}$

One hypothesis of the state. A set of N of them, with weights $w_t^{[i]}$, represents the belief: $\mathrm{bel}(x_t)\approx\sum_i w^{[i]}_t\,\delta(x_t - x^{[i]}_t)$.

Motion sampling

Draw $x_t^{[i]}\sim p(x_t\mid x_{t-1}^{[i]},u_t)$: simulate the robot’s motion with its noise.

Weighting

$w_t^{[i]}\propto w_{t-1}^{[i]}\,p(z_t\mid x_t^{[i]})$: the likelihood of the reading at the particle.

Resampling

Draw N new particles from the weighted set (with replacement) and reset the weights to $1/N$.

Systematic resampling

One random offset and N equally spaced pointers on the cumulative weights. Lowest variance, and the usual choice.

Derivation: the particle filter from the Bayes filter

Recall the Bayes filter: $\overline{\mathrm{bel}}(x_t)=\int p(x_t\mid x_{t-1},u_t)\,\mathrm{bel}(x_{t-1})\,dx_{t-1}$ and $\mathrm{bel}(x_t)\propto p(z_t\mid x_t)\,\overline{\mathrm{bel}}(x_t)$. Suppose $\mathrm{bel}(x_{t-1})$ is represented by weighted particles.

1. $$\mathrm{bel}(x_{t-1})\approx\sum_i\tilde w^{[i]}_{t-1}\,\delta\bigl(x_{t-1}-x^{[i]}_{t-1}\bigr)$$

   The belief as a weighted set of samples.

2. $$\overline{\mathrm{bel}}(x_t)\approx\sum_i\tilde w^{[i]}_{t-1}\,p\bigl(x_t\mid x^{[i]}_{t-1},u_t\bigr)$$

   Substitute into the prediction integral: the integral over the sample points becomes a weighted sum, a mixture of motion models, one per particle.

3. $$x_t^{[i]}\sim p\bigl(x_t\mid x^{[i]}_{t-1},u_t\bigr);\Rightarrow;\{x^{[i]}_t,\tilde w^{[i]}_{t-1}\}\ \text{represents}\ \overline{\mathrm{bel}}(x_t)$$

   Drawing one sample from each component gives a weighted sample of the mixture. This is the proposal of importance sampling, and it is the prior of the new step.

4. $$\mathrm{bel}(x_t)\propto p(z_t\mid x_t)\,\overline{\mathrm{bel}}(x_t);\Rightarrow;w^{[i]}_t\propto\tilde w^{[i]}_{t-1}\,p\bigl(z_t\mid x^{[i]}_t\bigr)$$

   The correction is the importance-sampling weight of the previous lesson: target (prior times likelihood) over proposal (prior) is the likelihood.

Why resample

Without resampling, the weights multiply at every step. The weight of a particle is a product of likelihoods, so its logarithm is a sum of terms: a few particles near the truth acquire nearly all the weight, and the rest, still being moved by the motion model, carry negligible weight. The effective sample size falls towards 1 (this is called degeneracy). Resampling replaces the many negligible particles by copies of the good ones, so the computation is spent where the probability is. It adds noise (copies of one particle are identical until the next motion step), which is why it is done only when $N_\mathrm{eff}$ falls below a threshold.

Which resampling scheme

Multinomial resampling draws N independent indices, which adds the most variance: a particle of weight $w$ is copied a random number of times with variance $Nw(1-w)$. Stratified resampling draws one pointer in each of N equal strata, and systematic resampling uses a single offset for all N pointers; both give each particle either $\lfloor Nw\rfloor$ or $\lceil Nw\rceil$ copies, so the number of copies is nearly deterministic. Systematic is the cheapest and the usual choice.

Sample from the motion model, weight by the likelihood, resample when N_eff is low. Everything else in a particle filter (injection, adaptive sizes, better proposals) is a refinement of these three steps.

A worked example

The corridor of the last lesson of Module 8: 20 m, three identical doors at 3, 8 and 14 m, a door detector (90% hits, 10% false alarms), the robot starting at 1 m and moving 25 cm per step with 10 cm of noise. The filter starts with 500 particles spread uniformly and runs next to the exact 80-cell grid filter on the same readings.

1. The first steps. The effective sample size is 433 of 500 at step 1 (no resampling). The first door reading at step 5 drops it to 156, below the threshold of 250, and the filter resamples. 12% of the particles are within 1 m of the true position at step 1 and 18% at step 5 (grid: 11% and 15%).
2. Ambiguity. At steps 10 and 16 the particles hold three clusters; 41% and 38% are near the truth (grid: 32% and 30%). At step 24 the second door has resolved the ambiguity: 95% (grid: 96%).
3. Steady state. By step 60, 99% are near the truth, with a standard deviation of 0.34 m. Ten resampling events happened in 60 steps; the filter did not resample when the weights were still even.
4. How many particles? Over 40 runs, the number in which the estimate is within 0.5 m of the true position after 60 steps is 14 with 10 particles, 20 with 50, 28 with 200 and 35 with 500, against 36 for the grid filter, which is exact for this model up to the cell size. More particles approach the exact answer.

import math
from robotprob import Rng, ParticleFilter, hist_predict, hist_update, mean

L, DOORS = 20.0, [3.0, 8.0, 14.0]
P_HIT, P_FALSE = 0.9, 0.1
at_door = lambda x: any(abs(x - d) < 0.6 for d in DOORS)

def move(x, rng):                                  # the motion model, sampled: 0.25 m per step plus noise
    return min(max(x + 0.25 + rng.gauss(0.0, 0.1), 0.0), L)

def like(says):                                    # p(reading | position)
    def f(x):
        if says: return P_HIT if at_door(x) else P_FALSE
        return (1 - P_HIT) if at_door(x) else (1 - P_FALSE)
    return f

def run(seed, n, steps=60, verbose=False):
    rng = Rng(seed)
    x = 1.0
    pf = ParticleFilter([rng.uniform(0.0, L) for _ in range(n)], rng)       # global localisation: particles everywhere
    # a grid filter on the same readings, as a reference
    N, cell = 80, 0.25
    b = [1.0 / N] * N
    out = []
    for t in range(1, steps + 1):
        x = x + 0.25 + rng.gauss(0.0, 0.1)
        says = rng.random() < (P_HIT if at_door(x) else P_FALSE)
        pf.predict(move)
        pf.update(like(says))
        neff = pf.n_eff()
        did = pf.resample(0.5)
        b = hist_predict(b, {0: 0.1, 1: 0.8, 2: 0.1})
        b = hist_update(b, [like(says)((i + 0.5) * cell) for i in range(N)])
        near_pf = sum(w for p, w in zip(pf.particles, pf.weights) if abs(p - x) < 1.0)
        near_h = sum(bi for i, bi in enumerate(b) if abs((i + 0.5) * cell - x) < 1.0)
        mu = pf.mean()
        sd = math.sqrt(sum(w * (p - mu) ** 2 for p, w in zip(pf.particles, pf.weights)))
        gmu = sum(bi * (i + 0.5) * cell for i, bi in enumerate(b))
        out.append((t, x, says, neff, did, near_pf, near_h, mu, sd, gmu))
    return out

o = run(8, 500)
print('step  robot x  door?  Neff  resampled  mass within 1 m: particles  grid   estimate  std')
for t, x, says, neff, did, npf, nh, mu, sd, gmu in o:
    if t in (1, 2, 5, 10, 16, 24, 32, 40, 60):
        print(str(t).rjust(4), str(round(x, 2)).rjust(8), str('yes' if says else 'no').rjust(6), str(round(neff)).rjust(5), str('yes' if did else 'no').rjust(9), str(round(npf, 2)).rjust(27), str(round(nh, 2)).rjust(6), str(round(mu, 2)).rjust(9), str(round(sd, 2)).rjust(5))
print('resampling events in 60 steps:', sum(1 for r in o if r[4]))

print('global localisation: runs (of 40) with the estimate within 0.5 m of the robot after 60 steps')
grid = 0
for n in (10, 50, 200, 500):
    ok = 0
    for seed in range(1, 41):
        r = run(seed, n)[-1]
        if abs(r[7] - r[1]) < 0.5: ok += 1
        if n == 10 and abs(r[9] - r[1]) < 0.5: grid += 1
    print(' particles', str(n).rjust(5), ' localised', str(ok).rjust(3))
print(' grid filter (80 cells)   localised', grid)

step  robot x  door?  Neff  resampled  mass within 1 m: particles  grid   estimate  std
   1     1.33     no   433        no                        0.12   0.11     10.36  5.84
   2     1.66     no   405        no                        0.12   0.11     10.76  5.83
   5     2.64    yes   156       yes                        0.18   0.15     10.36  5.81
  10     4.49     no   335        no                        0.41   0.32      8.53  4.66
  16     5.93    yes   213       yes                        0.38    0.3      9.81  4.51
  24     8.21    yes   226       yes                        0.95   0.96      8.33  1.47
  32     9.99     no   459        no                        0.94   0.97     10.35  1.48
  40     12.1     no   459        no                        0.93   0.95     12.36  1.47
  60    17.02     no   491        no                        0.99   0.98     17.27  0.34
resampling events in 60 steps: 10
global localisation: runs (of 40) with the estimate within 0.5 m of the robot after 60 steps
 particles    10  localised  14
 particles    50  localised  20
 particles   200  localised  28
 particles   500  localised  35
 grid filter (80 cells)   localised 36

In robotics

Why this is the standard for localisation

Monte Carlo localisation lets a robot find its pose in a known map from a laser scan, with no starting point and with symmetric environments. The motion model can be arbitrary, the sensor model any function that returns a likelihood, and the cost is linear in the number of particles. The Navigation course uses the algorithm in its localisation lessons, and Module 9 lesson 4 builds on it for scans.

Choosing the number of particles

The right number depends on the state dimension and the sharpness of the sensor. A 3-D pose with a laser needs hundreds to thousands; a 20-dimensional state needs a different method, because the number of particles required grows rapidly with the dimension. A rule of thumb is to increase N until the answer stops changing, and to use adaptive sizing (KLD sampling): many particles when the belief is spread, few when it is concentrated.

Cost and parallelism

The cost per step is $O(N)$ for the motion and weights, and $O(N)$ for systematic resampling. Weights and motion are independent per particle, so they parallelise trivially; resampling needs a cumulative sum.

The estimate

The weighted mean of the particles is a good summary for one cluster, and misleading for several (the lesson on Gaussian failures). Report the mean and spread only after checking that the cloud is unimodal, or report the best cluster (the mode with the largest weight). Angles need a circular mean, as in the unscented filter.

Common misconceptions

Key takeaways

1. The bootstrap particle filter has three steps: sample each particle from the motion model, weight it by the likelihood of the reading, resample.
2. It is importance sampling applied sequentially, with the prior as the proposal; the weight is the likelihood.
3. Resample when N_eff = 1/Σw̃² falls below about N/2, using systematic (or stratified) resampling for the lowest variance.
4. It represents any belief the particles can cover: it solved the three-doors corridor that a Gaussian could not, and with 500 particles it matched the exact grid filter (35 localised against 36 of 40 runs).
5. Accuracy is limited by the number of particles; the mean of a multimodal cloud is meaningless.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, sections 4.3 (particle filters) and 8.3 (Monte Carlo localisation). Paid book.
• C. Stachniss, “Particle Filter and Monte Carlo Localization” (video lecture) . Free to watch.
• S. Särkkä and L. Svensson, Bayesian Filtering and Smoothing, 2nd edition, 2023 , chapter 11 (particle filtering). Free to read as the authors’ online PDF; the book is copyrighted.
• M. S. Arulampalam, S. Maskell, N. Gordon and T. Clapp, “A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking”, IEEE Transactions on Signal Processing 50 (2), 2002. Paid journal article.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

The algorithm has weak points that show as soon as the sensor is accurate or the filter runs long: weights collapse, diversity vanishes and a lost robot cannot recover. The next lesson looks at them and at their remedies.

Related content

Nothing below is required to finish this lesson.

• Before
  M9-T1: Monte Carlo Belief and Importance Sampling

  Represent a distribution by weighted samples, and estimate an expectation under one distribution with samples drawn from another.

• Lab
  M9-L1: Monte Carlo Localization

  Rover is placed in a known room and must find itself from range readings to four beacons. Implement the weights and the resampler, then make the filter survive a kidnapping.

Other courses on this site treat neighbouring topics from their own angle:

• SLAM M3-T8: Particle Filters and FastSLAM
• SLAM M3-L4: Explore Particle-Filter Localization