Programming for Robotics 1, Chapter 1: Challenge 1 solution (jitter statistics, drift and the budget)

Formulas: mean = sum / n          s = sqrt( sum( (dt - mean)^2 ) / (n - 1) )          J(max) = max |dt - T|          f(effective) = 1 / mean
          drift: real period = T + w          U = C / T          headroom = T - C          f(max) = 1 / C
          latency: t(lat) = t(sensor) + T + t(compute) + t(actuator)          d(react) = v x t(lat)

(a) Eight passes of a 100 Hz loop (T = 10 ms): 10.1, 9.9, 10.0, 10.3, 14.2, 9.8, 10.1, 9.9
        sum = 84.3 ms                    mean = 84.3 / 8 = 10.5375 ms
        squared deviations from the mean:  0.1914, 0.4064, 0.2889, 0.0564, 13.4139, 0.5439, 0.1914, 0.4064   (sum = 15.4987)
        s = sqrt( 15.4987 / 7 ) = sqrt( 2.2141 ) = 1.488 ms
        largest dt = 14.2 ms             J(max) = 14.2 - 10.0 = 4.2 ms
        effective rate = 1000 / 10.5375 = 94.9 Hz        (not 100 Hz: the one slow pass pulls the mean up by 0.54 ms)

(b) The naive loop does 1.8 ms of work and then sleeps 10 ms, so each pass takes 11.8 ms.
        passes in 300 s = 300 / 0.0118 = 25 423.7, so 25 423 whole passes
        believed robot time = 25 423 x 10 ms = 254.23 s
        error = 300 - 254.23 = 45.77 s, which is 15.3 % of the real time
        (the loss is the same as the drift formula, n x w = 25 423 x 1.8 ms = 45.76 s, up to the rounding of the last pass.)
    Any integration that used the nominal 10 ms (odometry, an integral term) would be 15 % short. Measuring dt, and scheduling against deadlines, removes both faults.

(c) C = 1.1 + 0.4 + 0.3 + 0.7 + 1.0 = 3.5 ms
        U at 100 Hz = 3.5 / 10 = 0.35 = 35 %           headroom = 10 - 3.5 = 6.5 ms           f(max) = 1000 / 3.5 = 285.7 Hz
        U at 50 %:   T = C / 0.5 = 3.5 / 0.5 = 7.0 ms, so f = 1000 / 7.0 = 142.9 Hz
    So 100 Hz is comfortable (35 %), and the loop should not be pushed past about 140 Hz with this code. The numbers are for the average pass: measure the worst pass too.

(d) 100 Hz (T = 10 ms):   t(lat) = 60 + 10 + 5 + 30 = 105 ms        d(react) = 0.3 x 0.105 = 0.0315 m = 31.5 mm
    20 Hz (T = 50 ms):    t(lat) = 60 + 50 + 5 + 30 = 145 ms        d(react) = 0.3 x 0.145 = 0.0435 m = 43.5 mm
    The difference between the loops is 12 mm. The sensor delay and the motor delay (90 ms of the 105) are most of the chain, so a faster loop gives a small gain.

The same numbers recomputed in Python (the code was run, and the output below is what it printed):

CODE:
import statistics
dt_ms = [10.1, 9.9, 10.0, 10.3, 14.2, 9.8, 10.1, 9.9]
mean = statistics.mean(dt_ms)
print("(a) mean %.4f ms  std dev %.3f ms  max %.1f ms  max dev %.1f ms  rate %.1f Hz" % (
    mean, statistics.stdev(dt_ms), max(dt_ms), max(abs(x - 10.0) for x in dt_ms), 1000 / mean))
passes = int(300 / 0.0118)
print("(b) passes %d  believed %.2f s  error %.2f s (%.1f %% of 300 s)" % (passes, passes * 0.010, 300 - passes * 0.010, 100 * (300 - passes * 0.010) / 300))
cost_ms = 1.1 + 0.4 + 0.3 + 0.7 + 1.0
print("(c) C %.1f ms  U(100 Hz) %.0f %%  headroom %.1f ms  f(max) %.1f Hz  T at 50 %% %.1f ms = %.1f Hz" % (
    cost_ms, 100 * cost_ms / 10, 10 - cost_ms, 1000 / cost_ms, cost_ms / 0.5, 1000 / (cost_ms / 0.5)))
for hz in (100, 20):
    period_ms = 1000 / hz
    lat_ms = 60 + period_ms + 5 + 30
    print("(d) %3d Hz: latency %.0f ms, reaction distance %.1f mm" % (hz, lat_ms, 0.3 * lat_ms))
END CODE
OUTPUT:
(a) mean 10.5375 ms  std dev 1.488 ms  max 14.2 ms  max dev 4.2 ms  rate 94.9 Hz
(b) passes 25423  believed 254.23 s  error 45.77 s (15.3 % of 300 s)
(c) C 3.5 ms  U(100 Hz) 35 %  headroom 6.5 ms  f(max) 285.7 Hz  T at 50 % 7.0 ms = 142.9 Hz
(d) 100 Hz: latency 105 ms, reaction distance 31.5 mm
(d)  20 Hz: latency 145 ms, reaction distance 43.5 mm
END OUTPUT

Check on method: the sample standard deviation divides by n - 1 (here 7), not n. A common slip is to report the mean only: it hides the 14.2 ms pass, and for a robot the worst pass is the one that matters.