Project: the long lap

Two metres of driving, one landmark, and an estimate that has to stay honest.

U3.7Odometry and driftUniversity45 min

Do this lesson in the simulator

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

Guide: Dead reckoning and odometry explained

Everything in the module, in one program.

Drive a lap of at least two metres, come back to where you started, and keep your own estimate of the pose the whole way. At the end, print what you think and be marked against what is true, to within 10 cm after two metres of driving.

What it takes

  1. Calibrate the gyro first. Stand still, average, subtract for the rest of the run. Without this the rest does not matter.
  2. Run the odometry every tick, including while turning and while stopped. An update skipped is a piece of the path missing, and stopping is exactly when robots are most often nudged.
  3. Rotate by the midpoint heading, so the turns do not lean.
  4. Take a fix if you pass the tag. Tag 3 is on the far wall. It is optional, and it is the difference between a good estimate and a lucky one.
  5. Come home on the estimate. The lap has to end in the home zone, and the way back is to steer towards (0, 0) in your own estimated frame, with the inverse kinematics from U2. If the estimate is good you arrive. If it is not, you do not, and that is the test.
Steering to (0, 0) in the estimated frame brings the robot home exactly as far off as its estimate ishome zone, 30 cm square(0, 0), the startwhere it thinks it is(10 cm out)where it really isdashed: the drive itplans, to (0, 0)solid: the driveit really makesit ends here, 10 cm from the start
The robot steers by its estimate, so it drives the dashed arrow's length and direction from where it really is. It ends up exactly as far from home as the estimate was wrong, 10 cm here, the other way. With the heading right, 10 cm out still lands in the 30 cm zone; a much worse estimate would not.

The shape of the program

from bugbot import *
import math
connect()

DT = 0.1
x = y = h = 0.0
bias = 0.0

def calibrate(seconds=4.0):
    global bias
    rates = []
    for i in range(int(seconds / DT)):
        rates.append(imu()[1])
        wait(DT)
    bias = sum(rates) / len(rates)

def step(n=1):
    """n odometry updates, one per tick, whatever the robot is doing."""
    global x, y, h
    for i in range(n):
        vx, vy = flow()
        rate = imu()[1] - bias
        a = math.radians(h + 0.5 * rate * DT)
        x += (vx * math.cos(a) + vy * math.sin(a)) * DT
        y += (-vx * math.sin(a) + vy * math.cos(a)) * DT
        h += rate * DT
        plot("x", x)
        plot("y", y)
        wait(DT)

calibrate()
forward(75)
step(20)
stop()
step(4)
print("after one leg:", (round(x, 1), round(y, 1)), "truth", position())

Run this in the simulator

The robot's true position up the mat through one leg, with the odometry updates, stop() and the coast after it44.555.566.570102030time, strue y, cmstop()green ticks: an odometry updatestep(4):2.6 cmno updates:0.5 cm
From the lesson's own cell. After stop() the robot coasts another 3.1 cm and takes 0.8 s to come to rest. The four updates of step(4) count 2.6 cm of that; the last 0.5 cm happens after the loop has stopped updating, so the estimate never hears of it. Without step(4) the whole 3.1 cm would be missing.

Task: the long lap

Two metres of driving, back into the home zone under your own navigation, with my x: and my y: printed at the end and within 10 cm of the truth.

from bugbot import *
import math
connect()

DT = 0.1
# calibrate, then drive the lap, updating every tick

Challenges

  1. Plot your estimate and the truth for both axes on one chart. Where does the gap open up?
  2. Do the lap in the other direction. Is the error the same size? If not, what does that say about the bias?
  3. Run the lap at set_noise(0.5), 1 and 2, and plot the final error against the noise level. Is it a straight line?

What comes next

U4 stops treating a sensor reading as a number and starts treating it as a number plus a distribution. That is where filtering comes from, and it is what lets U6 decide how much to trust a fix instead of guessing 0.8.