Weighing the guesses

The measurement update: how likely is this reading if the robot were there?

U7.4LocalisationUniversity35 min

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

Guide: Particle filters explained

The measurement update asks each particle one question: if the robot really were here, how likely is the reading I just got?

for each particle:
    predicted = what the sensor would read at this pose
    weight = likelihood(measured given predicted)

For a sensor with Gaussian noise of standard deviation sigma:

d = predicted - measured
weight = math.exp(-d * d / (2 * sigma * sigma))

A particle that predicts the reading exactly gets weight 1. One that is two sigma out gets 0.14. One that is ten sigma out gets a number so small it might as well be zero, which is the point: it is almost certainly not where the robot is.

The sensor model is the interesting part

predicted is a simulation of the sensor from that particle's pose, and writing it is most of the work in a real system. On this mat, facing the far wall, it is easy:

predicted = 200 - particle_y

That line has a condition hidden in it: the robot must be facing the far wall square on. Turn it and the sensor looks along a slant, which is longer. The simulator's depth sensor also looks along a narrow cone, about 3 degrees either side of straight ahead, and reports the nearest thing in it. Square on, the nearest point in the cone is straight ahead, so the cone and 200 - y agree, and they keep agreeing to within a centimetre or so for the first 10 degrees of a turn. After that they part company fast. Standing 80 cm from the far wall and turning left, the robot read:

Weigh a cloud against the 60 degree reading with 200 - y and every guess 70 cm from the truth wins. Even a model that knows the heading and draws one straight line is 10 cm out, because the near edge of the cone reaches the wall first. The simple fix, and the one the rest of this module uses, is to weigh the cloud only while the robot is square on to the wall the model is about, and to skip readings while it turns. The heading the robot has from its gyro (odometry()) drifts by 2 to 3 degrees, which the cone forgives.

With obstacles it becomes a ray cast: march along the particle's heading until you hit something. With a full laser scanner it is a ray cast per beam per particle, which is why real implementations precompute a likelihood field instead of casting rays.

Add a floor to the likelihood, always:

weight = math.exp(...) + 1e-12

Without it, one unexpected reading can make every weight exactly zero, the normalisation divides by zero, and the filter dies. With it, the filter survives a surprise and recovers.

Normalising, and the effective sample size

Weights are relative, so divide by their total. Then a single number tells you how healthy the cloud is:

neff = 1.0 / sum(w * w for w in weights)

The effective sample size. If all weights are equal it is N. If one particle has all the weight it is 1. It is the honest count of how many particles are still contributing, and it is what U7.5 uses to decide when to resample.

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]

best = sum(w * y for w, y in zip(weights, particles))
print("weighted mean", round(best, 1))
print("neff", round(1.0 / sum(w * w for w in weights), 1), "of", N)

Run this in the simulator

The robot is at y = 70 and the weighted mean comes out at 70.3. But look at neff: five hundred particles, and after one reading only about 22 of them are doing any work. The rest were more than a few sigma from the reading and now weigh next to nothing. That collapse is exactly why resampling exists.

Do not lie about sigma

Using a sigma smaller than the sensor's real noise makes the filter over-confident: it will throw away the true pose because one reading was unlucky. Slightly larger than the truth is the safer error, and a common practical choice.

Task: weigh the guesses

Scatter particles over the mat, take a reading, weight them, and print best y: (the weighted mean) and neff:.

from bugbot import *
import math, random
connect()

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

Challenges

1. Use sigma = 0.5 and look at neff. How many particles survive?
2. Weight the cloud against two readings taken from the same spot. Does neff fall further?
3. Plot the weights against the particle's y. What shape is it?