Predict and correct

The shape every filter has: a model that guesses forward, a measurement that pulls it back.

U6.2State estimationUniversity30 min

Do this lesson in the simulator

Every filter in this module, and every filter you will meet later, has the same two steps.

predict:  move the estimate forward using a model of how the world changes
correct:  pull it towards a measurement

That is it. The Kalman filter, the particle filter in U7, the whole family: they differ in how the state is represented and how the weighting is chosen, never in the shape.

On the robot

The state is how far the robot has travelled up the mat.

  • Predict with the flow sensor. est += flow()[1] * dt. Smooth, and it drifts.
  • Correct with the wall. The robot started 110 cm from the wall's face, so 110 - distance() is a measurement of the same quantity. Noisy, and it does not drift.

Two estimates of one number, again, and the same trick as U6.1 in a different costume.

from bugbot import *
connect()

DT = 0.1
START_GAP = 110.0
est = 0.0

forward(60)
for i in range(60):
    est += flow()[1] * DT                     # predict
    measured = START_GAP - distance()         # measure
    est = 0.9 * est + 0.1 * measured          # correct
    plot("measured", measured)
    plot("estimate", est)
    wait(DT)
stop()
print("my y:", round(est, 1))

Run this in the simulator

The chart shows what fusion buys: a jagged measurement, and an estimate that follows its trend without following its noise.

Why predict at all

You could skip the prediction and just filter the measurement. That was U4, and it is worse, for a reason worth stating clearly.

A low pass filter has no idea the robot is moving. It smooths towards an average, so while the robot drives, the filtered value lags behind, and the faster it drives the further behind it sits.

The prediction knows the robot is moving, because it uses a measurement of the motion. So the estimate keeps up, and the correction only has to fix the small residue. Fusion beats filtering because the model does most of the work.

What a model is

The prediction step is where your knowledge of the machine lives:

  • a velocity measurement, as here;
  • the commands you sent, plus the kinematics from U2, if there is no velocity sensor;
  • constant velocity, if you have nothing at all, which for a short step is often good enough.

The better the model, the less work the measurement has to do, and the more you can filter the measurement without falling behind.

The residual

measured - est has a name: the innovation, or residual. It is the part of the measurement your model did not predict, and it is the most useful diagnostic in the whole business.

  • Innovations scattered around zero: the filter is working.
  • Innovations consistently positive or negative: the model is biased, and the filter is fighting it.
  • Innovations much larger than expected: the measurement is worse than you told the filter, or something is wrong in the world.

Plot the innovation. It will tell you more than the estimate does.

Task: predict, then correct

Drive at least 40 cm towards the wall, running predict and correct, plotting measured and estimate, and print my y:. position() is not allowed.

from bugbot import *
connect()

DT = 0.1
START_GAP = 110.0
est = 0.0

Challenges

  1. Plot the innovation as well. Is it centred on zero?
  2. Set the blend to 0.5 and to 0.02 and compare. What is each one trusting?
  3. Break the prediction on purpose by multiplying the flow by 1.2. Where does that show up?