Programming for Robotics 1, Chapter 4: Challenge 3 solution (fusion with a sensor that freezes)

Idea: the fusion weights are w_A = (1/25) / (1/25 + 1/4) = 0.138 and w_B = 0.862 (variances in mm squared). When B freezes, it keeps giving the range it had at t = 4 s, while the true range falls by v (t - 4) = 0.1 (t - 4) m, so the unchecked fusion lags the truth by w_B x 0.1 x (t - 4) = 0.0862 (t - 4) m: its error grows by 8.6 cm every second. The rms of a ramp w v s over s = s0 .. s1 is w v sqrt((s1^3 - s0^3) / (3 (s1 - s0))); for the window t >= 4.2 s that is s = 0.2 .. 4.0 s. The health-checked version wraps B in a Sensor and a Health monitor (stuck_n = 10) and passes None to fuse() as soon as the monitor has a fault; fuse() skips it, so the result is then A alone.

CODE:
VAR_A, VAR_B = 0.005 ** 2, 0.002 ** 2
sim = Simulator(World(3.0, 2.0), seed=7, pose=(1.3, 1.0, 0.0), sigma_m=0.005)       # 1.63 m from the wall
sim.add_range_sensor("b", sigma_m=0.002)
frozen_after_s = 4.0
last_b = [None]


def b_read():
    r = sim.range_of("b")
    if sim.now_s() >= frozen_after_s:
        return last_b[0]                                 # frozen on the last value before 4 s
    last_b[0] = r
    return r


sensor_b = Sensor(b_read, sim.now_s, 0.02, 4.0, 0.11, name="b")
health_b = Health(sensor_b, stuck_n=10)
sim.set_duty(0.25, 0.25)                                 # 0.1 m/s
rows = {"A alone": [], "fused, B unchecked": [], "fused, B health-checked": []}
when = []
detected = None
for _ in range(400):
    sim.advance(0.02)
    t = sim.now_s()
    truth = sim.range_sensors["front"].true_range(sim.pose())
    a = sim.range_m()
    vb = sensor_b.read()
    fault = health_b.update()
    if fault and detected is None:
        detected = t
    est_a = a
    est_naive = fuse([(a, VAR_A), (b_read(), VAR_B)])[0]
    est_checked = fuse([(a, VAR_A), (None if fault else vb, VAR_B)])[0]
    for k, e in zip(rows, (est_a, est_naive, est_checked)):
        rows[k].append((t, (e - truth) * 1000))
    when.append(t)


def rms(name, lo, hi):
    vals = [e for t, e in rows[name] if lo <= t < hi]
    return math.sqrt(sum(e * e for e in vals) / len(vals))


print("rms error against the true range (mm)       t < 4.0 s    4.0 <= t < 4.2 s    t >= 4.2 s")
for name in rows:
    print("  %-26s %14.2f %14.2f %17.2f" % (name, rms(name, 0.0, 4.0), rms(name, 4.0, 4.2), rms(name, 4.2, 99.0)))
w_b = (1 / VAR_B) / (1 / VAR_A + 1 / VAR_B)
print("weight of B: %.3f; after the freeze the unchecked fusion lags the truth by w_B x v x (t - 4) = %.4f x (t - 4) m"
      % (w_b, w_b * 0.1))
t0, t1 = 0.2, 4.0                                        # seconds after the freeze: the window t >= 4.2 s is s = 0.2 .. 4.0
pred = w_b * 0.1 * math.sqrt((t1 ** 3 - t0 ** 3) / (3 * (t1 - t0))) * 1000
print("predicted rms of the unchecked fusion for t >= 4.2 s: %.1f mm   (the rms of w_B x v x s over s = 0.2 .. 4.0 s)" % pred)
print("freeze noticed at t = %.2f s (stuck_n = 10 readings = 0.2 s); the error of the unchecked fusion then was %.1f mm"
      % (detected, [e for t, e in rows["fused, B unchecked"] if abs(t - detected) < 1e-9][0]))
END CODE
OUTPUT:
rms error against the true range (mm)       t < 4.0 s    4.0 <= t < 4.2 s    t >= 4.2 s
  A alone                              4.96           6.34              4.99
  fused, B unchecked                   1.79          10.30            205.35
  fused, B health-checked              1.79           8.92              4.99
weight of B: 0.862; after the freeze the unchecked fusion lags the truth by w_B x v x (t - 4) = 0.0862 x (t - 4) m
predicted rms of the unchecked fusion for t >= 4.2 s: 204.2 mm   (the rms of w_B x v x s over s = 0.2 .. 4.0 s)
freeze noticed at t = 4.18 s (stuck_n = 10 readings = 0.2 s); the error of the unchecked fusion then was 16.3 mm
END OUTPUT

Reading the output. Before the freeze (t < 4 s) both fusions agree and are better than A alone: 1.79 mm against 4.96 mm (the predicted fused value is 1.86 mm; the measured one comes from 200 passes, so the difference is within the scatter of the estimate). After the freeze the unchecked fusion is off by 205 mm rms for t >= 4.2 s, against the 204.2 mm predicted from the weights: it follows the frozen sensor, with a calm, confident, wrong number, and gets worse by 8.6 cm every second. The health-checked fusion goes back to A alone, 4.99 mm, the same as A alone. In the 0.2 s window around the freeze (4.0 to 4.2 s) the two are not yet different by much: the monitor needs ten readings that are exactly equal (it noticed at t = 4.18 s), and until then the frozen B is still in the fusion, at an error that has grown to 16.3 mm at that moment (the weights predict 0.0862 x 0.18 = 15.5 mm). That is the price of the check: it detects after stuck_n passes, so the error it permits is about w_B x v x stuck_n x dt, and a shorter stuck_n would detect sooner but would also reject a healthy sensor that happens to repeat a value.

Check on method: the three ways to get this wrong are (1) fusing first and checking the health of the fused number: it is a blend of a frozen and a live value and does not repeat exactly, so the stuck test never fires, which is why the health check must watch each sensor on its own, before the fusion, (2) comparing the three versions only over the whole run, where the average hides that they were identical before the freeze and very different after it (hence the windows of the table), and (3) giving a frozen sensor a smaller weight instead of removing it: the weights come from the variance, and a frozen sensor has no variance to speak of, only a bias that grows. A test for a failure has to inject it, as here.