Moving the cloud

The motion update: every particle drives, and every particle's own error goes with it.

U7.3LocalisationUniversity30 min

Do this lesson in the simulator

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

Guide: Particle filters explained

The motion update is the prediction step of U6.2, applied to every particle at once.

for each particle:
    move it the way the robot moved
    plus its own random error

The random part is the important half. Without it every particle moves identically, so the cloud keeps its shape for ever, and the filter believes it is exactly as certain after ten metres as it was at the start. With it, the cloud spreads, and the spread is the filter's honest statement that dead reckoning is losing accuracy.

Eight particles each wander away from the robot's own move, and the cloud spreads01234-6-3036time, sguess minus the robot's move, cmspread, 2.1 cm-2.1 cmno random term:all 300 identical8 of the 300particles
Take away the robot's own move and this is what is left: each grey line is one particle drifting by its own random error. With the random term the cloud widens, which is the filter admitting it knows less. Without it every particle stays on the red line, and the cloud claims to be exact for ever.
from bugbot import *
import random
connect()
random.seed(1)

DT, N = 0.1, 300
ys = [0.0] * N    # every guess starts where the robot starts

forward(70)
for i in range(40):
    v = flow()[1]
    ys = [y + v * DT + random.gauss(0, 0.35) for y in ys]
    mean = sum(ys) / N
    spread = (sum((y - mean) ** 2 for y in ys) / N) ** 0.5
    plot("mean y", mean)
    plot("robot y", position()[1])   # the truth, for the chart only
    plot("spread y", spread)
    wait(DT)
stop()

Run this in the simulator

Every particle starts at 0, so this cloud is in the same frame as position(), which also counts from where the robot started (here that is y = 40 on the mat, but neither of them knows it). The mean tracks the robot to within 2 cm all the way: at the last tick it says 48.2 cm and the robot has gone 46.7. The spread climbs, slowly, from 0.3 to 2.1 cm, in a curve that should look familiar: that is the random walk from U3.3, simulated rather than calculated. Forty steps of 0.35 cm each should spread it by 0.35 times the square root of 40, which is 2.2 cm.

The spread of the cloud over 40 steps, against 0.35 times the square root of the number of steps01020304000.511.522.5steps of 0.1 sspread y, cm0.3 after one step2.1, the cloud2.2, the ruledashed: 0.35 x square root of steps
The simulated cloud (green) against the random-walk rule (dashed). Four times the steps only doubles the spread: 1.11 cm after 10 steps, 2.21 cm after 40.

How much noise to add

The noise per step should match how wrong the motion really is. Too little and the cloud is over-confident: it shrinks onto a wrong answer and refuses to be corrected. Too much and the cloud spreads faster than the measurements can pull it in, and the estimate is vague.

A reasonable starting point is the process noise you would have used for Q in a Kalman filter, as a standard deviation instead of a variance. Then check the same way: does the truth stay inside the cloud?

This is where the model goes

Anything you know about how the robot moves belongs here, and unlike a Kalman filter there is no requirement that it be linear or differentiable:

  • speed proportional to the command, with the dead band;
  • more error when turning than when driving straight;
  • a wheel that slips when the command is large;
  • a robot that cannot pass through a wall, so any particle that would is simply killed.

That last one is free information and it is worth taking. A particle that has walked through a wall is an impossible guess, and removing it costs one line.

Task: move the cloud

Start every particle at zero, move them with the flow sensor plus their own noise as the robot drives at least 40 cm, plot mean y and spread y, and print both at the end.

from bugbot import *
import random
connect()

DT, N = 0.1, 300
ys = [0.0] * N

Challenges

  1. Run it with no random term at all. What does the spread do, and why is that a lie?
  2. Double the noise and compare the spread at the end. Does it double?
  3. Start every particle at 40, where the robot starts on the mat, so the cloud is in mat coordinates. Drive on towards the far wall and kill any particle that goes past 200. How many do you lose, and when?