A fix from a landmark

Dead reckoning between landmarks, and a reset whenever something known comes into view.

U3.5Odometry and driftUniversity30 min

Do this lesson in the simulator

Calibrated dead reckoning still drifts, just more slowly. The only thing that stops error growing is information from outside the robot.

A fix is a measurement of where you are, made against something whose position you already know. A tag on a wall, a wall itself, a charging dock, a ceiling light. Between fixes you dead reckon; at a fix you reset.

The oldest idea in navigation

This is exactly how a ship was navigated before satellites. Dead reckon on a log and a compass all day, accepting growing error, then take a star sight at dusk and throw the accumulated error away. The error saw-tooths: it grows between fixes and drops to nearly nothing at each one.

The whole of state estimation is a refinement of that pattern. U6 and U7 are about doing it continuously and in proportion to how much you trust each source, rather than all at once.

A fix from a tag

An AprilTag gives an id and a distance. If you know where tag 7 is on the mat, and you are 40 cm from it, you know a great deal about where you are.

from bugbot import *
connect()

set_cv("apriltag")
TAG_Y = 165.0                 # how far up the mat tag 7 is, from the start

DT = 0.1
y = 0.0
forward(70)
for i in range(40):
    y += flow()[1] * DT
    seen = [t for t in apriltags() if t[0] == 7]
    if seen:
        fixed = TAG_Y - seen[0][3]
        print("tick", i, "dead reckoning says", round(y, 1), "the tag says", round(fixed, 1))
        y = fixed
    wait(DT)
stop()
print("my y:", round(y, 1), " truth", position())

Run this in the simulator

Watch the two numbers in that printout. They agree at first, part company as the run goes on, and the moment a fix arrives the estimate jumps back.

Replacing and blending

The code above replaces the estimate with the fix. That is right when the fix is far better than the estimate, and wrong when it is not: a noisy fix used this way throws away good dead reckoning and makes the estimate jump about.

The gentler version blends:

y = 0.8 * y + 0.2 * fixed        # trust the estimate more than this fix

Where does 0.8 come from? Out of the air, here. The point of the Kalman filter in U6 is that the right weight is not a matter of taste: it follows from how uncertain each number is, and it changes every tick as that uncertainty changes.

When not to trust a fix

  • A far away tag. The distance estimate gets worse with range, roughly with its square.
  • A tag at a sharp angle. The detector gets a worse fix on something it is seeing edge on.
  • One reading. A single detection can be a mistake. Two in a row from the same id is a cheap safeguard.
  • The wrong tag. Checking the id is not optional. A fix against a landmark that is somewhere else entirely is worse than no fix at all.

Task: a fix from a tag

Tag 7 is on the far wall, 165 cm up the mat from the start. Drive at least a metre towards it, dead reckoning as you go, take a fix when the tag is close enough to be worth having, and print my y: at the end.

from bugbot import *
connect()

set_cv("apriltag")
TAG_Y = 165.0

Challenges

  1. Blend the fix at 0.2 instead of replacing, and compare the error.
  2. Only accept a fix when the tag is within 100 cm. Does that help or hurt?
  3. Print the size of the jump at each fix. What does a growing jump tell you about your dead reckoning?