Fusing a fix

Odometry prediction, a tag as the measurement, and a gain that rises as the estimate ages.

U6.6State estimationUniversity35 min

U3.5 took a fix by replacing the estimate. That was crude in two ways: it threw away good dead reckoning, and it trusted the tag completely. With a variance to hand, both are fixed.

The rule

Predict every tick. Correct only when a fix arrives, with the gain the variances give you.

# every tick
est += flow()[1] * DT
p += Q

# when a tag is seen
measured = TAG_Y - tag_distance
k = p / (p + R_TAG)
est += k * (measured - est)
p = (1 - k) * p

Nothing new. It is the U6.3 filter with the correction step made conditional, which is all that "fusing an intermittent measurement" means.

The sawtooth

from bugbot import *
connect()

DT, TAG_Y = 0.1, 165.0
Q, R_TAG = 0.6, 9.0
set_cv("apriltag")
est, p = 0.0, 4.0

forward(65)
for i in range(70):
    est += flow()[1] * DT
    p += Q
    seen = [t for t in apriltags() if t[0] == 4]
    if seen and seen[0][3] < 130:
        measured = TAG_Y - seen[0][3]
        k = p / (p + R_TAG)
        est += k * (measured - est)
        p = (1 - k) * p
    plot("estimate", est)
    plot("variance", p)
    wait(DT)
stop()

Run this in the simulator

The variance line is the one to watch. It climbs steadily while the robot is dead reckoning and drops the moment a fix arrives. That sawtooth is the picture of every navigation system ever built, from a ship's log and a star sight to a phone with GPS in a tunnel.

R for a tag

A tag's range error grows with distance, so a single R_TAG is a simplification. A better model:

R_tag = 4.0 + 0.002 * d * d

Near tags are trusted, far ones are not, automatically. The constants come from measuring the detector at several ranges, which is an afternoon well spent on any real system.

Gating, again

A fix that disagrees wildly with the estimate is more likely to be a mistake than a revelation. The filter has the numbers to judge:

if (measured - est) ** 2 < 9 * (p + R_TAG):      # inside three sigma
    ...correct...
else:
    ignore, and count it

The gate widens on its own when the filter is unsure, which is exactly when a surprising fix is most likely to be genuine. This is validation gating, and it is what stops one misread tag throwing a robot across the room.

Task: fuse a tag fix

Drive at least 80 cm towards tag 4, predicting with flow and correcting when the tag is in view. Plot estimate and variance, and print my y:. No position().

from bugbot import *
connect()

DT, TAG_Y = 0.1, 165.0
Q, R_TAG = 0.6, 9.0
set_cv("apriltag")
est, p = 0.0, 4.0

Challenges

Make R_TAG grow with range and compare the estimate.
Add a three sigma gate and count the fixes it refuses.
Print the innovation at each fix. Does it shrink as the run goes on?