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

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

Close, and not exact. Three separate reasons, and it is worth keeping them apart.

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, noticeably better while the speed is changing, and identical once it is steady.

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. 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.

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?