Integrating velocity

The odometry update, one step at a time, and what the step size costs you.

U3.1Odometry and driftUniversity25 min

Do this lesson in the simulator

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

Guide: Gyroscope vs accelerometer

The robot measures how fast it is going. You want to know where it is. Between the two sits an integral, and in a program an integral is a loop with a running total.

x = x + velocity * dt

That line is dead reckoning. Everything else in this module is about what it does to the errors.

The simplest case

Drive in a straight line and add up the speed:

from bugbot import *
connect()

DT = 0.1
travelled = 0.0
forward(70)
for i in range(30):
    vx, vy = flow()
    travelled += vy * DT
    wait(DT)
stop()
print("my estimate:", round(travelled, 1), "cm")
print("the truth:   ", position())

Run this in the simulator

It prints 35.4 cm against a true 35.3, which is closer than it deserves: two of the errors below happen to cancel on this run. Keep the loop going for another second while the robot coasts to a stop and it says 39.0 against a true 38.2. Close, and not exact. Three separate reasons, and it is worth keeping them apart.

The estimate minus the truth over the run, all errors together, and the part that is Euler's alone01234-1-0.500.51time, sestimate minus truth, cmstop()dashed: Euler's part alonered: all the errors together(what the loop prints, minus the truth)+0.8
From the lesson's own run. While the robot speeds up, Euler falls behind by about 0.8 cm (dashed); while it coasts to a stop it gives nearly all of that back. The reading that is 3 percent high pushes the red line up the whole time, and cancels Euler almost exactly at 3 s. After the coast only the scale is left: +0.8 cm.

Reason one: the step is not an instant

The loop reads a speed and then assumes the robot held it for the whole tenth of a second. It did not: it was speeding up or slowing down through the step. The reading is the speed at one instant, used as if it were the average over the step. That is Euler integration, and its error grows with the step size.

The cheap improvement is the trapezium rule: use the average of this reading and the last one.

travelled += 0.5 * (v_now + v_last) * DT

Same loop, same sensor, better while the speed is changing, and identical once it is steady. Mind the ends, though: the trapezium sum stops at the last reading, so in the same loop it covers one step less than the Euler sum does.

Speed against time over the first second: the true speed, Euler's rectangles and the trapezium's slopes00.20.40.60.8104812time, sspeed, cm/sthe true speedEuler: each reading held for the whole steptrapezium: a straight line between readings
The first second of the same run, with a perfect sensor's readings (the true speed at each tick). The gaps between the tops of the red bars and the thin line are what Euler misses while the robot speeds up. By 1 s the robot has really gone 9.8 cm; Euler's bars add up to 9.0 cm and the trapezium's to 9.6 cm.

Reason two: the reading is not the speed

flow() is a measurement with noise on it and a scale that is a few percent off: on this robot it reads about 3 percent high, which is where the 0.8 cm after the coast comes from. Integrating it integrates all of that too, which is the subject of U3.3.

Reason three: the loop is not exactly 0.1 s

wait(0.1) waits at least a tenth of a second. Sensor reads and arithmetic take time on top. If the real period is 0.11 s and you multiply by 0.10, every single step is 10 percent short, and the error is one-sided, so it accumulates rather than cancelling. In the simulator the period is exactly 0.1 s, because reading a sensor costs no simulated time, so this one only bites on the real robot.

Ten steps of 0.10 s on the program's clock against ten steps of 0.11 s on the real onewhat the program multiplies by: 0.10 s a stepwhat really passes: 0.11 s a step1.0 s1.1 s0.1 s nevercounted
Each step is only 10 ms short, but every one is short the same way, so the dashed lines lean further at every step. After ten steps 0.1 s of driving is missing: at this robot's cruising speed of about 12.8 cm/s that is 1.3 cm, and it keeps growing.

The fix is to measure the step rather than assume it:

now = clock()
dt = now - last
last = now
travelled += v * dt

This is one of those corrections that costs two lines and removes an entire class of mysterious results.

Task: integrate a velocity

Drive at least 50 cm in a straight line, integrating flow() as you go, and print your own answer as my y: 54.3. It is marked against where the robot really ended up.

from bugbot import *
connect()

DT = 0.1
y = 0.0
forward(70)

Challenges

  1. Do the same run with the trapezium rule and compare the two estimates against the truth.
  2. Measure the real loop period with clock() and use that instead of the constant. How much does it change?
  3. Integrate with DT = 0.5. Where does the error come from now?