Fusing a fix

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

U6.6State estimationUniversity35 min

Do this lesson in the simulator

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

Guide: Kalman filters explained

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

In the simulator the camera reads tag 4 on every tick once it is close enough. Real fixes are rarer: a phone's GPS gives one a second, a ship's navigator one star sight a night. So this demo takes a fix only on the whole seconds, i % 10 == 0, and dead reckons in between.

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(90):
    est += flow()[1] * DT
    p += Q
    seen = [t for t in apriltags() if t[0] == 4]
    if seen and seen[0][3] < 130 and i % 10 == 0:     # a fix once a second
        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, by Q every tick, from 4.6 to 28.0 before the first fix. The tag comes within 130 cm at 3.2 seconds, and the first fix, at 4 seconds, drops the variance to 6.9. After that it climbs for a second and drops at each fix, settling into teeth that climb to about 10 and fall back to about 5. 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.

Take out and i % 10 == 0 and there is only one tooth: the variance climbs to 23.2, the first fix drops it to 6.5, and with a fix every tick it settles flat at 2.04.

The filter's variance against time, with a tag fix once a second and with one on every tick01234567890102030time, secondsvariance p, cm²tag within 130 cm from 3.2 s28.0once asecondevery tick
The solid line is the demo. The variance climbs by Q = 0.6 on every tick of dead reckoning, and drops at each fix, making the sawtooth. The dashed line is the same run with a fix on every tick the tag is near enough: after its first drop it has no time to climb, and it lies flat.

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.

R for a tag against its range: the fixed R_TAG of 9, and the model 4 + 0.002 d², which is lower close in and higher far out0255075100125010203040range to the tag d, cmR, cm²R_TAG = 937.8the modelequal at 50 cmtrusted more
The model agrees with the fixed R_TAG at 50 cm. Closer than that it gives an R below 9, so the filter leans on the tag more. At the demo's 130 cm limit it says 38, about four times as much, so a far fix only nudges the estimate.

The simulator's detector is kinder than a real one: it reports the range to the nearest millimetre at every distance. So R_TAG = 9 is cautious here, and a range model will change the estimate very little in the simulator. It matters on a real camera, where it is worth doing properly.

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.

The three sigma gate around the estimate, before the first fix and before a later one, with a fix 16 cm away that one gate lets in and the other turns awaybefore the first fix: p = 28.6−18.4+18.4estimateusedbefore a later fix: p = 11.0−13.4+13.4estimateturned awaya fix 16 cm out
The gate is three sigma of the innovation, 3 × √(p + R_TAG). Before the first fix p is 28.6 and the gate reaches ±18.4 cm, so a fix 16 cm out is believed. Once fixes have pulled p down to about 11, the gate is ±13.4 cm, and the same fix is refused.

Task: fuse a tag fix

Drive at least 80 cm towards tag 4, predicting with flow and correcting once a second while 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

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