A tag as a fix

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

U11.4VisionUniversity40 min

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

Guide: SLAM explained: simultaneous localisation and mapping

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

It converges, but slowly. Each round here removes only about a fifth of the heading error, because the two tags are only 55 degrees apart as the robot sees them, so a wrong heading moves both implied positions in nearly the same direction and the bearings barely disagree. It also needs a sensible first guess: within about 75 degrees of the truth it finds the robot, but from 90 degrees it settles on a false answer at (130, 250), off the mat and facing backwards. A Gauss-Newton solve is the same idea with each step scaled by how strongly the bearings depend on the heading, and from a guess of 30 degrees that finishes in three rounds. A full bundle adjustment is Gauss-Newton again 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 = 30.0                     # a deliberately wrong guess
for step in range(24):
    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
    if step % 4 == 0 or step == 23:
        print("round", step, "->", round(x, 1), round(y, 1), "facing", round(h, 2))

Run this in the simulator

Watch it settle. The guess of 30 degrees puts the first fix at (14.8, 80.7), about 55 cm from the truth. By round 23 it reads (59.7, 50.1) facing 0.25 degrees, against a true (60, 50) facing 0. Once the robot is running you rarely pay that price, because each solve can start from the heading the last one found, which is already close.

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

1. Start the iteration from a heading guess of 90 degrees. Where does it end up, and what check would catch that answer before the robot drove on it?
2. 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?
3. 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?