Write your own odometry

flow() and imu() in, a pose out, in about ten lines. Then check it against the truth.

U3.2Odometry and driftUniversity30 min

odometry() is provided so that you can check your work against it. Writing it yourself is the exercise, and it is about ten lines.

The update

Three state variables, one update per tick:

h  = h + turn_rate * dt
x  = x + (vx*cos(h) + vy*sin(h)) * dt
y  = y + (-vx*sin(h) + vy*cos(h)) * dt

with (vx, vy) from flow() and turn_rate from imu()[1]. The rotation in the middle is exactly the matrix from U2.2: the velocity is measured in the body frame and the state is kept in the world frame, so it has to be rotated on the way in.

Which heading to rotate by

Turn first and then move, as above? Move and then turn? Or rotate by the average of the two headings?

For a step of 0.1 s and a turn rate of 60 deg/s the difference is 6 degrees, which over a 2 cm step is about 2 mm. Small, but it is a bias, not noise: it always leans the same way, so it accumulates all run. Using the midpoint heading is the standard compromise and costs nothing:

h_mid = h + 0.5 * rate * dt

This is a second order method hiding inside a one-line change, which is a fair description of most of numerical integration.

The whole thing

from bugbot import *
import math
connect()

DT = 0.1
x = y = h = 0.0

def step():
    global x, y, h
    vx, vy = flow()
    rate = imu()[1]
    h_mid = h + 0.5 * rate * DT          # rotate by the middle of the step
    a = math.radians(h_mid)
    x += (vx * math.cos(a) + vy * math.sin(a)) * DT
    y += (-vx * math.sin(a) + vy * math.cos(a)) * DT
    h += rate * DT

forward(70)
for i in range(25):
    step()
    plot("mine", y)
    plot("truth", position()[1])
    wait(DT)
stop()
print("mine", (round(x, 1), round(y, 1), round(h, 1)), "truth", position(), round(heading(), 1))

Run this in the simulator

Two lines on the chart, starting together and slowly parting. That picture is the subject of the next three lessons.

Why not use the heading straight from the IMU

imu()[0] gives a fused heading directly, without integrating anything. Why add up the rate instead?

Because they fail differently. The fused heading has noise and a slow wander but no growing error, so it is better over minutes. The integrated rate is smooth and responsive over a second but drifts without limit. Using one of them means accepting its weakness.

Using both, weighted by how much you trust each, is a complementary filter, and it is U6.2.

Task: your own odometry

Drive at least 90 cm with a corner in it, running your own odometry, and print my x: and my y: at the end. position() is not allowed anywhere in this task, not even to print. Turn with drive() inside your loop: a blocking turn_right(angle=90) runs the world without running your loop, so the turn never reaches your estimate.

from bugbot import *
import math
connect()

DT = 0.1
x = y = h = 0.0

Challenges

1. Compare your estimate with odometry() after the same run. They should agree closely; if they do not, one of you has a bug.
2. Plot your heading against heading() for a run with three turns in it.
3. Add the trapezium rule to the velocity as well as the midpoint to the heading. Does it help on this drive?