Answers

U6.2 Predict and correct

State estimation · University · about 30 min

What this lesson is about

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

• On the robot
• Why predict at all
• What a model is
• The residual

Questions 7 marks in all

1. [1 mark]Put one tick of the lesson's fusion loop in order.

   Number the lines 1 to 4 to put them in the right order.

   1. measured = START_GAP - distance()
   2. est = 0.9 * est + 0.1 * measured
   3. plot("estimate", est)
   4. est += flow()[1] * DT
   Answer:

   est += flow()[1] * DT
   measured = START_GAP - distance()
   est = 0.9 * est + 0.1 * measured
   plot("estimate", est)

   Predict with the motion measurement, take the measurement, correct towards it, then plot.

2. [1 mark]Three ticks of predict and correct with made-up sensor values. What does it print?

   DT, START_GAP = 0.1, 110.0
   est = 0.0
   for speed, d in ((20.0, 107.0), (20.0, 103.0), (20.0, 104.0)):
       est += speed * DT
       measured = START_GAP - d
       est = 0.9 * est + 0.1 * measured
       print(round(est, 2))

   Answer:

   2.1
   4.39
   6.35

   Tick 1 predicts 2, measures 3, and blends to 2.1. Tick 2 predicts 4.1, measures 7, blends to 4.39. Tick 3 predicts 6.39, measures 6, blends to 6.35.

3. [1 mark]What is measured - est called?

   Answer: innovation. The innovation, or residual, is the part of the measurement the model did not predict, and it is the most useful diagnostic a filter has.

4. [1 mark]The plotted innovation is consistently positive. What does that say?

   1. AThe model is biased and the filter is fighting it
   2. BThe filter is working well
   3. CThe measurement noise is larger than expected
   4. DThe blend weight is too large
   Answer: A. Innovations should scatter around zero. A consistent sign means the prediction is systematically off, for example a flow scale error.

5. [1 mark]Why does predict and correct keep up with a moving robot better than a low pass filter on the distance alone?

   1. AThe prediction uses a measurement of the motion, so the correction only fixes a small residue
   2. BIt uses a smaller alpha
   3. CIt uses two sensors, which halves the noise
   4. DIt runs the loop faster
   Answer: A. A low pass filter smooths towards an average and lags a moving target. The model knows the robot is moving, so it does most of the work.

6. [1 mark]The correct step is changed to est = 0.98 * est + 0.02 * measured. What is the filter now trusting?

   1. AMostly the flow prediction, so drift is corrected only slowly
   2. BMostly the wall measurement, so the estimate is noisy
   3. CBoth equally
   4. DNeither, the estimate stops updating
   Answer: A. The measurement gets 2 percent per tick, so the estimate is smooth and follows the flow sensor, including its drift, for longer.

7. [1 mark]The robot has no velocity sensor. What does the lesson suggest as the model for the predict step?

   1. AThe commands sent, through the kinematics from U2
   2. BA copy of the last measurement
   3. CA low pass filter on the measurement
   4. DNo prediction; correct only
   Answer: A. The prediction is where knowledge of the machine lives. Without a velocity sensor, the commands plus the kinematics say how the state should change.

The task: predict, then correct

from bugbot import *
connect()

DT = 0.1
START_GAP = 110.0
est = 0.0

from bugbot import *
connect()

DT = 0.1
START_GAP = 110.0          # cm from the robot to the wall's face at the start
est = 0.0

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