Programming for Robotics 1, Chapter 4: Challenge 1 solution (choosing the filter for a worse sensor)

Idea: with a delay budget of 4 samples, the median takes (N - 1) / 2 of it and the exponential filter gets the rest, so (1 - alpha) / alpha = 4 - (N - 1) / 2, that is alpha = 1 / (1 + rest): N = 3 gives alpha = 0.25, N = 5 gives 1/3 and N = 7 gives 0.5 (the delays add, section 4). The chance that the median fails comes from the binomial formula of section 5 with p = 0.04. The noise left by the exponential filter alone is sigma x sqrt(alpha / (2 - alpha)) with sigma = 8 mm. The record is 6000 readings of a robot parked 0.93 m from the wall, through FaultyHardware with a 4 % rate of wrong echoes; the same record is scored for every filter, so the comparison is fair.

CODE:
SIGMA = 0.008
TRUE_M = 3.0 - 2.0 - 0.07                                # parked at x = 2.0, the sensor 0.07 m ahead, the wall at x = 3.0


def dirty_record(n=6000, seed=41):
    sim = Simulator(World(3.0, 2.0), seed=seed, pose=(2.0, 1.0, 0.0), sigma_m=SIGMA)
    hw = FaultyHardware(sim, random.Random(seed + 100), spike_rate=0.04)
    return [r for r in collect(hw, n) if r is not None]


record = dirty_record()
print("(a) prediction: budget 4 samples; the median of N costs (N - 1) / 2, the exponential filter gets the rest")
print("  N   median delay   rest   alpha     EMA delay   chance median fails   expected bad outputs (at most)   noise after EMA alone")
plans = []
for n in (3, 5, 7):
    rest = 4 - (n - 1) / 2
    alpha = 1.0 / (1.0 + rest)                           # (1 - alpha) / alpha = rest
    p_fail = median_failure_probability(n, 0.04)
    plans.append((n, alpha))
    print("  %d   %8.1f      %4.1f   %.4f   %8.1f      %12.5f         %14.1f                  %12.2f mm"
          % (n, (n - 1) / 2, rest, alpha, ema_delay_samples(alpha), p_fail, p_fail * len(record), SIGMA * ema_noise_ratio(alpha) * 1000))


def score(make_filter, skip=30):
    f = make_filter()
    errs = [(f.update(r) - TRUE_M) * 1000 for r in record][skip:]
    return math.sqrt(sum(e * e for e in errs) / len(errs)), sum(1 for e in errs if abs(e) > 25.0)


print()
print("(b) measured on %d readings: filter                      delay  rms (mm)  outputs > 25 mm off" % len(record))
rows = [("raw readings", lambda: Pipeline(), 0.0), ("moving average of 9", lambda: MovingAverage(9), 4.0)]
for n, alpha in plans:
    rows.append(("median %d + EMA %.4f" % (n, alpha), (lambda n=n, alpha=alpha: Pipeline(MedianFilter(n), ExpFilter(alpha))), 4.0))
results = {}
for name, make, delay in rows:
    rms, bad = score(make)
    results[name] = rms
    print("    %-32s              %4.1f   %8.2f   %10d" % (name, delay, rms, bad))
END CODE
OUTPUT:
(a) prediction: budget 4 samples; the median of N costs (N - 1) / 2, the exponential filter gets the rest
  N   median delay   rest   alpha     EMA delay   chance median fails   expected bad outputs (at most)   noise after EMA alone
  3        1.0       3.0   0.2500        3.0           0.00467                   28.0                          3.02 mm
  5        2.0       2.0   0.3333        2.0           0.00060                    3.6                          3.58 mm
  7        3.0       1.0   0.5000        1.0           0.00008                    0.5                          4.62 mm

(b) measured on 6000 readings: filter                      delay  rms (mm)  outputs > 25 mm off
    raw readings                                   0.0     222.45          265
    moving average of 9                            4.0      79.03         1660
    median 3 + EMA 0.2500                          4.0      17.18          114
    median 5 + EMA 0.3333                          4.0       6.72           13
    median 7 + EMA 0.5000                          4.0       3.38            0
END OUTPUT

Reading the output. (a) The prediction says: the longer the median, the safer it is (a failure chance of 0.47 %, 0.06 % and 0.008 %, so up to 28, 3.6 and 0.5 failures in the 6000 readings), and the more of the delay budget it uses, so the less smoothing the exponential filter can do (3.0, 3.6 and 4.6 mm of noise left by the EMA alone). (b) Measured, the pipelines rank in the order of the median length: median 7 with alpha = 0.5 has an rms error of 3.38 mm and no output more than 25 mm off, median 5 has 6.72 mm and 13 bad outputs, median 3 has 17.2 mm and 114. The unfiltered readings have 222 mm and 265 bad ones (4.4 %, the injected 4 % plus a little), and the 9-point moving average, with the same delay of 4 samples, is at 79 mm and 1660 bad outputs: each wild echo is smeared over nine outputs. (c) Choose the median of 7 with alpha = 0.5.

How close was the prediction? The noise: the measured 3.38 mm is below the 4.62 mm predicted for the exponential filter alone, because the median of 7 also smooths the ordinary noise a little, and the formula ignored that on purpose, so it is an upper bound here. The failures: 28 predicted for N = 3 and 114 measured, 3.6 predicted for N = 5 and 13 measured, 0.5 and 0 for N = 7. The prediction counts failures of the median; the exponential filter that follows spreads each failure over several outputs (a large error decays by the factor 1 - alpha per sample, so it stays above 25 mm for a number of samples), by a factor of about 4 in both cases where there are numbers to compare (114 / 28 = 4.1, 13 / 3.6 = 3.6). So the formula tells the order of the three choices and the scale of the failures, and it is not a count of bad outputs; to know that, measure.

Check on method: the three ways to get this wrong are (1) judging by the rms error alone, which a handful of failures ruins for N = 3 and N = 5, so look at the count of bad outputs next to it, (2) giving the delay budget to the exponential filter and using a median of 3 because it is cheapest: the smoothing is better, but the failures are far more numerous than for N = 7 (114 against 0), and (3) using one record only: a count as small as 13 changes from seed to seed, so repeat with a few seeds before trusting it.