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

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

Guide: Model predictive control explained

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.
The robot starts 110 cm from the wall. The flow sensor adds up how far it has come; the distance sensor reads what is left, and 110 minus that is the same quantitywallstartflow()[1]START_GAP = 110 cmest: travelleddistance()measured = 110 − distance()
Two ways to the same number. The flow sensor says how fast the mat slides past, and adding that up gives how far the robot has come. The distance sensor reads the gap still left to the wall, and taking it from the 110 cm it started with gives the same length again.

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. Checked against the simulator's truth, the measurement is up to 16 cm out and the estimate never more than 3.4 cm.

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. Delete the predict line from the demo and try it: the same 0.9 blend prints 50.5 instead of 60.6, when the robot has really travelled 63 cm, so it ends about 12 cm behind.

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.

The distance travelled against time: the truth, the lesson's filter, and the same filter with the predict line deleted02460204060time, secondstravelled, cmtruth: 62.0predict andcorrect: 60.6no predict:50.5dots: the measurements
The same 0.9 blend, with and without the predict line, beside the truth (dashed). Without a prediction the filter only smooths the readings, so it trails behind a moving robot, and the gap grows while it drives. With the flow reading moving it on each tick, the estimate keeps up.

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.

One tick of the filter in three steps: the prediction moves the estimate on, the measurement lands some way off, and the correction moves the estimate a tenth of the way to it1. predict2. measure3. correct1718192021222324cm travelled22.423.5+ 1.1 from flowmeasured 17.3innovation -6.20.1 of it: 22.9
Tick 21 of the demo. The prediction moves the estimate on by the flow reading. The measurement lands 6.2 cm behind: that gap is the innovation. The correction moves the estimate only 0.1 of the way across it, because one noisy reading is not to be trusted on its own.
  • 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?