Weighing the guesses

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

U7.4LocalisationUniversity35 min

Do this lesson in the simulator

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

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

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]

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

Five hundred particles, and after one reading perhaps thirty of them are doing any work. 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?