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

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.

from bugbot import *
import random
connect()

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

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("spread y", spread)
    wait(DT)
stop()

Run this in the simulator

The mean tracks the robot. The spread climbs, slowly, in a curve that should look familiar: that is the random walk from U3.3, simulated rather than calculated.

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. Particles that walk through walls are guesses that are not merely improbable but impossible, and removing them 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 after 5 seconds. Does it double?
  3. Kill any particle that goes past 200 and count how many you lose.