Problem 650032 · medium · Level 06 Heuristics & Optimization

A Cyclist in a Tunnel: Wheel Count or GPS?

dead reckoning · sensor fusion · Kalman filter · control input · RMS error · uncertainty growth

A bike computer tracks how far a cyclist has ridden along a route (m). It has two sources:

  • a wheel sensor, which gives the speed speeds[k] (m/s) over each interval of dt seconds. Adding up speed * dt from the start is dead reckoning. It is smooth, but a slightly wrong tyre size makes every metre a little off, and the error keeps growing.
  • a GPS receiver, which gives a position fix fixes[k] with error variance r (m²) after each interval, or None when there is no signal (in a tunnel, under trees).

The fused estimate is a one-number Kalman filter that uses the wheel speed as a known input in its prediction. Both estimates start at the known position x0, and the fused one starts with variance p = 0. For each interval k = 0, 1, ..., in this order:

dead = dead + speeds[k] * dt                  dead reckoning
x = x + speeds[k] * dt                        fused: predict with the measured speed
p = p + q                                     ... which adds an error of variance q per interval
if fixes[k] is not None:                      fused: update with the GPS fix
    K = p / (p + r);  x = x + K * (fixes[k] - x);  p = (1 - K) * p

After interval k the true position is true_pos[k] (from a survey of the route). Write ride(speeds, fixes, true_pos, dt, x0, q, r) that returns a tuple of three numbers: the RMS error of the dead-reckoned positions, sqrt(mean((dead - true_pos[k]) ** 2)) over all intervals, the RMS error of the fused positions, and the largest value of p at the end of any interval.

Examples

Input:  speeds = [5.2, 5.2, 5.2, 5.2, 5.2, 5.2], fixes = [5.3, 9.6, None, None, None, 30.4],
        true_pos = [5, 10, 15, 20, 25, 30], dt = 1.0, x0 = 0.0, q = 0.5, r = 4.0
Output: (0.7788880963698617, 0.5920189331311924, 2.264044943820225)
Explanation: the wheel sensor reads 4 % high, so dead reckoning is 0.2 m further off every second
(1.2 m by the end). The fixes pull the fused estimate back. In the three seconds without GPS its
variance grows by 0.5 a second, to 2.26 m² just before the last fix.

Press Run with plot(true_pos) and the two estimates on one chart to see the drift and the corrections.

Constraints

  • speeds, fixes and true_pos have the same length, at least 1
  • answers are compared with a tolerance of 1e-6

Goals

  • Dead-reckon a position by adding up measured speed times time
  • Fuse the same speeds with occasional position fixes in a Kalman filter with a known input
  • Compare the two by their RMS error, and watch the variance grow while the fixes are missing
Starting Python…