Resampling

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

U7.5LocalisationUniversity30 min

Do this lesson in the simulator

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.

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.

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()

N, SIGMA = 500, 3.0
particles = [random.uniform(0, 200) for i in range(N)]
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, a handful carrying the weight. After: 500 particles in a small band around the answer, all equal.

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.