Resampling

Keeping the good guesses without losing the diversity that lets the filter recover.

U7.5LocalisationUniversity30 min

Do this lesson in the simulator

Print it: worksheet (PDF) · answers (PDF) · as a web page

Guide: Particle filters explained

After a few updates, almost all the weight sits on a handful of particles and the rest are wasting compute on guesses that have been ruled out. Resampling fixes that: draw a new set of particles from the old one, in proportion to their weights.

Likely particles get copied several times. Unlikely ones disappear. Every particle comes out with equal weight, and the cloud is now concentrated where the belief is.

Low variance resampling

The naive way is to draw N independent samples. It works and it is noisier than it needs to be: by luck, a particle with 10 percent of the weight might get no copies at all.

The standard method uses one random number and then equal strides through the cumulative weights:

step = 1.0 / N
r = random.uniform(0, step)
c = weights[0]
i = 0
fresh = []
for m in range(N):
    u = r + m * step
    while u > c and i < N - 1:
        i += 1
        c += weights[i]
    fresh.append(particles[i])

A particle with weight w gets either floor(w*N) or ceil(w*N) copies, never zero if it deserves one. It is also O(N) rather than O(N log N), which matters on a microcontroller.

Low variance resampling: eight equally spaced pointers through the particles' weights laid end to endA0B1C1D2E2F1G1H0weightsparticlecopiespointersstride 1/8
Eight particles from y = 63 (A) to 77 (H), weighted against a reading of 130 with sigma 3. One random start, then a pointer every eighth of the way along. Each particle gets one copy per pointer that lands in its stretch: D gets 2, and A and H, too thin to catch one, get none.

Particle deprivation

Resampling has a failure mode, and it is the one that bites.

Copies are exact, so after enough resampling every particle is a copy of the same ancestor. The cloud has collapsed to a single point, it has no diversity left, and if that point is wrong the filter can never recover, because there is no particle anywhere else to rescue it. The filter will be confidently, permanently wrong.

Resampling with no jitter: the number of different values in the cloud only ever goes down12 differentstart9 different17 different25 different34 different44 different54 different64 different74 different84 different94 different10round6467707376y, cm
12 particles near y = 70, standing still, resampled against a fresh reading each round with no jitter. A bigger dot is more copies of the same value; the green ones are the values still there at the end. A value that gets no copies is gone for good and nothing can bring it back: 12 different values at the start, 4 after 10 rounds.

Three defences, and a real system uses all three:

  1. Jitter. Add a small random amount to each copy, so no two particles are identical. One line, and it keeps the cloud alive.
  2. Resample only when needed. If neff > N/2, do not resample this tick. Diversity is only spent when there is something to buy with it.
  3. Inject random particles. A few per tick, scattered over the whole map. It is insurance against the robot being somewhere the cloud has written off, which is exactly the kidnapped robot problem.

Watching it work

from bugbot import *
import math, random
connect()
random.seed(1)

N, SIGMA = 500, 3.0
particles = [random.uniform(0, 200) for i in range(N)]
# standing still, square on to the far wall
measured = sum(distance() for i in range(5) if wait(0.1) is None) / 5

weights = []
for y in particles:
    d = (200 - y) - measured
    weights.append(math.exp(-d * d / (2 * SIGMA * SIGMA)) + 1e-12)
total = sum(weights)
weights = [w / total for w in weights]
print("neff before", round(1.0 / sum(w * w for w in weights), 1))

step, r, c, i, fresh = 1.0 / N, random.uniform(0, 1.0 / N), weights[0], 0, []
for m in range(N):
    u = r + m * step
    while u > c and i < N - 1:
        i += 1
        c += weights[i]
    fresh.append(particles[i] + random.gauss(0, 0.5))
particles = fresh

mean = sum(particles) / N
spread = (sum((y - mean) ** 2 for y in particles) / N) ** 0.5
print("after: mean", round(mean, 1), "spread", round(spread, 2))

Run this in the simulator

Before: 500 particles over the whole mat, with an effective sample size of 22, so a couple of dozen carry the weight. After: 500 particles, all equal, in a band around y = 70.3 with a spread of 3.5 cm, close to the 3 cm of SIGMA. The robot is at y = 70. The band is only in y: the reading was of the far wall, square on, so it says nothing about x.

The lesson's cloud before and after resampling, between y = 50 and 9050607080900125250before: 500 particles, each with a weightweight x 500particles (grey)(400 moreoutside)per 2 cm50607080900125250y on the mat, cmafter: 500 particles, all equalparticles(0 outside)per 2 cm
Before, the particles are spread evenly (400 of them outside this window) and a couple of dozen carry the weight (neff 22). Resampling turns weight into numbers: the bars after have the same shape as the weight before, a band about as wide as sigma.

Task: resample

Weight a scattered cloud against one reading, then resample it. Print before: and after:, the effective sample size each side, and spread:, the standard deviation of the resampled cloud.

from bugbot import *
import math, random
connect()

N, SIGMA = 500, 3.0
particles = [random.uniform(0, 200) for i in range(N)]

Challenges

  1. Resample ten times in a row without jitter and print how many distinct values are left.
  2. Add jitter and repeat. How many now?
  3. Inject 5 percent random particles each round and describe what it costs you when the filter is already right.