A tag as a fix

Range and bearing to a landmark you know the position of, which is a position fix.

U11.4VisionUniversity40 min

A landmark whose position you know, seen at a known range and bearing, tells you where you are. That is a fix, and it is the same operation a navigator performs with a lighthouse and a sextant.

One tag, if you know your heading

The tag is at (Tx, Ty) on the mat. You measure a range d and a bearing b from your own nose. If your heading is h, then the direction from you to the tag in mat coordinates is h + b, so

x = Tx - d * sin(h + b)
y = Ty - d * cos(h + b)

Note the dependence on h. A bearing is measured in the robot's frame, so a heading error of one degree at a range of a metre moves your fix 1.7 cm sideways. A tag fix is only as good as the heading that interprets it.

Two tags, if you do not

With two known tags you have four measurements (two ranges, two bearings) and three unknowns (x, y, h), so the heading is observable and you can stop trusting the compass. The clean way to see it is as a fixed point:

guess a heading
    put yourself where each tag says you are, given that heading
    average those positions
    for each tag, work out the bearing that position predicts
    the average difference from the bearings you measured is your heading error
    correct, and go round again

Five or six rounds converge from a wild starting guess. It is a tiny Gauss-Newton solve, written out longhand, and it is exactly what a full bundle adjustment does with thousands of observations instead of two.

Where a tag's pose estimate is weak

A real AprilTag detector reports a full 6 degree of freedom pose, not just a range and a bearing, by solving for the plane that maps the tag's four known corners onto the four corners it found. That is more information, and it comes with a sharp caveat worth knowing before you rely on it.

Bearing is excellent. It comes from the centroid of the tag, which is an average over many pixels, so it is accurate to a fraction of a pixel and therefore to a fraction of a degree.
Range is mediocre, for the reasons in U11.3: it comes from apparent size, and it degrades as the square of the range.
Out-of-plane rotation is terrible, and worse, it is bistable. When a tag is nearly square on, the projected shapes produced by tilting it a few degrees one way and a few degrees the other are almost identical. The solver flips between the two, and a robot that steers on the tag's reported yaw will see its estimate jump by ten or twenty degrees between frames while nothing physical moves.

The standard defences are to use a larger tag, to view it obliquely rather than square on, or to use several tags in a rigid bundle so that the ambiguity is resolved by the group. The simplest defence, and the one worth internalising, is to use the parts of the estimate that are strong: bearing first, range second, orientation only if you must.

Fixing and driving

from bugbot import *
import math
connect()

F = 92.4
TAGS = {20: (40.0, 160.0), 21: (150.0, 140.0)}
set_cv("apriltag")
wait(0.3)

seen = [(t[0], math.degrees(math.atan((t[1] - 160) / F)), t[3]) for t in apriltags() if t[0] in TAGS]
h = 0.0
for step in range(6):
    x = sum(TAGS[i][0] - d * math.sin(math.radians(h + b)) for i, b, d in seen) / len(seen)
    y = sum(TAGS[i][1] - d * math.cos(math.radians(h + b)) for i, b, d in seen) / len(seen)
    err = sum(((math.degrees(math.atan2(TAGS[i][0] - x, TAGS[i][1] - y)) - b) - h + 180) % 360 - 180
              for i, b, d in seen) / len(seen)
    h += err
    print("round", step, "->", round(x, 1), round(y, 1), "facing", round(h, 2))

Run this in the simulator

Watch it settle. The first round is wrong because the heading guess was wrong; by the third the position has stopped moving.

A fix is not a filter

A fix is instantaneous and it is only available when a tag is in view. The moment the tag goes out of frame you are back to dead reckoning, and U6.6 is the machinery for stitching the two together properly: predict with odometry, correct with the fix, and carry a variance that says how much the fix is worth. A robot that only knows where it is while it can see a tag is not much use, and a robot that ignores the tag when it can see one is worse.

Task: fix your position from two tags

Tag 20 is at (40, 160) on the mat and tag 21 at (150, 140). Work out where the robot is, print it as my x: and my y:, and then drive to (130, 100) on the mat and stop there, refixing as you go. Neither position() nor heading() is allowed: both are the lab's truth, and the point is that two tags are enough.

from bugbot import *
import math
connect()

F = 92.4
TAGS = {20: (40.0, 160.0), 21: (150.0, 140.0)}
TX, TY = 130.0, 100.0
set_cv("apriltag")
wait(0.3)

Challenges

Start the iteration from a heading guess of 90 degrees. How many rounds does it take now?
Use only tag 20 and hold the heading at whatever your two-tag solve last said. How far has the fix drifted by the end of the run?
Corrupt one range by 20 percent before solving. Which of x, y and h suffers most, and does that match what the lesson said about where the strength lies?