M12-T5 — From Model to Flight Software

Learning objectives

After this lesson, you should be able to:

• Assign rates to the GNC functions of a vehicle and total the latency of a loop from its stages.
• Compute the phase margin lost to sampling, hold and computing delay.
• List the numerical differences between a model and fixed-step flight code, and quantify the single-precision ones.
• Say what a log must contain to reconstruct a flight exactly, and what is lost if it does not.

The intuition

A control design is validated in the model, and then it is implemented as code that runs at a fixed rate on a processor. Software-in-the-loop simulation, covered in UAV M6-T14: Software-in-the-Loop simulation, runs that code against a simulated vehicle, and the timing of the code is the subject of UAV M7-T7: Jitter and deadline monitoring. This lesson is about what the GNC designer must hand to the software team, and what to check on its return.

Rates

Each function of Hopper runs at its own rate, set by the bandwidth of the loop it closes. The rule of thumb is that the loop runs at 10 to 20 times its bandwidth, so that the delay of sampling is small against the delay the loop can tolerate. The rates were set in the first module; here is the arrangement.

Latency

The delay in a loop is not one number but a sum of stages: the filter that smooths the gyro, the age of the sample when the loop reads it (half a period on average), the computing time, the hold of the command until the actuator reads it (half a period again), and the bus. Each one is small. Together they are 19 ms in the pitch loop at 100 Hz, and a delay of 19 ms at a crossover of 6 rad/s is 6.6° of phase.

What changes between model and code

Precision. A single-precision number has 24 bits of mantissa. At 300 m from the pad the spacing between representable numbers is 0.03 mm and nothing is lost. At an Earth-centred coordinate of 6.4 million metres the spacing is 0.5 m, larger than Hopper's whole error budget for position, so navigation in an Earth-centred frame needs double precision or a local frame. Clocks. Time accumulated as a floating-point sum of steps drifts: a 200 Hz clock in single precision lost 185 s in 10 000 s of running, because the increment of 0.005 s falls below the spacing once the sum is large. An integer tick counter has no such drift. Discretization. A derivative that is a filter in the model becomes a difference of samples in the code, and an integrator becomes a sum with a step. The step must be small enough, and the form must be the one the analysis used.

The model-to-code difference is a list: rates, a latency budget, number formats, a clock, discretization, and logging. Each item has a number, and none of them is large enough to ignore or small enough to guess.

Definitions

Loop rate $f_s$

The number of times per second a function reads its inputs and writes its outputs; the period is $T = 1/f_s$.

Latency budget

The list of delays between a physical quantity and the actuator response to it, with their sum.

Zero-order hold

The output of a sampled controller held constant until the next sample.

Single and double precision

24 and 53 bits of mantissa: relative spacing of about $6\times10^{-8}$ and $1.1\times10^{-16}$.

Replay

Running the code on logged inputs to reproduce its outputs.

Sampling with a zero-order hold behaves, near the crossover, like a delay of half a period; the sampling of the measurement adds another half. Together with the fixed stages, the delay of the loop is:

A delay costs phase in proportion to the crossover frequency, and the rate needed for a margin follows:

Derivation: sampling as a delay, and the single-precision spacing

A sampled controller with a hold is not a delay, but it behaves like one at frequencies well below the sampling frequency.

1. $$H_{\mathrm{ZOH}}(s) = \frac{1 - e^{-sT}}{sT}$$

   The hold holds the value for a period T. As a transfer function (with the sampler gain divided out) it is this expression.

2. $$H_{\mathrm{ZOH}}(j\omega) = \frac{\sin(\omega T/2)}{\omega T/2}\,e^{-j\omega T/2}$$

   Factor out $e^{-sT/2}$ on the imaginary axis: a magnitude, and a phase of exactly $-\omega T/2$.

3. $$\frac{\sin(\omega T/2)}{\omega T/2} \approx 1 - \frac{(\omega T)^2}{24}$$

   For $\omega T \ll 1$ the magnitude is nearly one: at 6 rad/s and 100 Hz it is 0.9998. What remains is a pure delay of half a period.

4. $$\tau_{\mathrm{ZOH}} = T/2,\quad \tau_{\mathrm{sample}} \approx T/2$$

   The sampler reads a value that is on average half a period old: another half. The two make one period, which is the first term of the delay formula.

The spacing of single-precision numbers follows from the format.

1. $$x = (1 + f)\,2^{e},\quad f = k\,2^{-23},\ k = 0,\dots,2^{23}-1$$

   A single-precision number has 23 bits of fraction $f$ and an exponent $e$: numbers between $2^e$ and $2^{e+1}$ are spaced by $2^{e-23}$.

2. $$\Delta x = 2^{e-23},\qquad 6.4\times10^{6} \in [2^{22}, 2^{23}) \Rightarrow \Delta x = 2^{-1} = 0.5\ \mathrm{m}$$

   The Earth-centred coordinate is between $2^{22}$ and $2^{23}$ metres, so $e = 22$ and the spacing is 0.5 m.

3. $$\text{an increment smaller than } \Delta x/2 \text{ added to } x \text{ is lost}$$

   The sum rounds back to $x$. A 1 mm increment added to 60 km (spacing 4 mm) is lost every time, which is why 1000 of them gave 0.000 m.

A sampled loop costs a period of delay, to first order. Single precision has 24 significant bits, which means a spacing that grows with the size of the number: use a local frame, integer clocks, and double precision where the numbers are large.

A worked example

The Hopper pitch loop ($K_P = 0.8405$, $K_D = 0.2854$, 40 rad/s servo, crossover 6.07 rad/s, 55.5° margin with no delay) as flight code at 100 Hz.

1. Latency budget at 100 Hz. Gyro filter 4.0 ms, sampling 5.0, computation 3.0, hold 5.0, bus 2.0: total 19.0 ms. The phase margin falls from 55.5° to 48.9°, a loss of 6.6°.
2. The rate. With 9 ms of fixed delay, the margin is 35.0° at 20 Hz, 45.4° at 50 Hz, 48.9° at 100 Hz, 50.6° at 200 Hz and 51.5° at 400 Hz. A 45° requirement allows 30 ms and needs at least 47 Hz.
3. Stable is not adequate. A simulation with the control law sampled with a hold keeps the loop stable up to a 400 ms period (2.5 Hz), where it rings; at 500 ms it is unstable. Stability survives a much slower rate than a 45° margin allows.
4. Single precision. At 300 m from the pad a coordinate is stored with an error of 0.00003 m. At 6 378 137 m the spacing is 0.5 m and the example value was rounded by 0.12 m. A clock accumulated as t += 0.005 for 2 000 000 steps reads 9815.4 s instead of 10 000 s. Adding 1 mm a thousand times to a coordinate of 60 km gains 1.000 m in double precision and nothing in single.
5. Reconstruction. Replaying 2000 logged altimeter samples through the same code gives an estimate identical to the flight (difference 0). A log written with two decimals gives estimates that differ by up to 1.9 mm. A log stored in single precision differs by 5e-7 m.

import math
import random
import struct

KP, KD, B, LAM = 0.8405, 0.2854, 19.62, 0.7
SERVO = 40.0

def f32(x):
    """Round to single precision, as a 32-bit flight computer would store the number."""
    return struct.unpack('f', struct.pack('f', x))[0]

# ---- 1. a latency budget for the pitch loop: every stage between the physical angle and the gimbal angle
def crossover(delay):
    lo, hi = 0.5, 60.0
    mag = lambda w: math.hypot(KP, KD * w) * B / (w * w + LAM ** 2) * SERVO / math.hypot(w, SERVO)
    for _ in range(80):
        mid = (lo + hi) / 2
        if mag(mid) > 1: lo = mid
        else: hi = mid
    return (lo + hi) / 2
def phase_margin(delay):
    wc = crossover(delay)
    return wc, math.degrees(math.atan(KD * wc / KP) - math.atan(wc / SERVO) - wc * delay)

rate = 100.0                                   # Hz, the pitch loop
T = 1.0 / rate
stages = [('gyro low-pass filter (group delay)', 0.004), ('sampling of the gyro by the loop (age, half a period)', T / 2),
          ('computation of the control law', 0.003), ('hold of the command until the next actuator update (half a period)', T / 2),
          ('bus and servo input latency', 0.002)]
total = sum(d for _, d in stages)
print(f"latency budget of the pitch loop at {rate:.0f} Hz:")
for n, d in stages: print(f"  {n:62s} {d * 1000:5.1f} ms")
wc0, pm0 = phase_margin(0.0)
wc1, pm1 = phase_margin(total)
print(f"  total {total * 1000:.1f} ms; phase margin {pm0:.1f} deg with no delay, {pm1:.1f} deg with the budget ({pm0 - pm1:.1f} deg lost at the {wc1:.2f} rad/s crossover)")
print("effect of the loop rate (sampling and hold give a delay of one period, plus the fixed 9 ms of the other stages):")
for r in (20.0, 50.0, 100.0, 200.0, 400.0):
    d = 1.0 / r + 0.009
    wc, pm = phase_margin(d)
    print(f"  {r:5.0f} Hz: delay {d * 1000:5.1f} ms, phase margin {pm:5.1f} deg")
d_max = math.radians(pm0 - 45.0) / wc1
print(f"delay allowed by a 45 degree phase margin: {d_max * 1000:.0f} ms, so a loop rate of at least {1 / (d_max - 0.009):.0f} Hz")

# ---- 2. what changes between the model and fixed-step code: single precision position
print("single-precision numbers:")
for name, x in (('local position 300 m from the pad', 300.123456789), ('geocentric x-coordinate, 6378137 m', 6378137.123456789)):
    e = f32(x) - x
    ulp = f32(x) - struct.unpack('f', struct.pack('I', struct.unpack('I', struct.pack('f', f32(x)))[0] - 1))[0]
    print(f"  {name}: stored as {f32(x):.4f}, rounding error {e:+.4f} m, spacing of representable numbers {ulp:.6f} m")
# accumulating time in single precision at 200 Hz
t32 = 0.0; k = 0
for k in range(1, 2000001):
    t32 = f32(t32 + 0.005)
print(f"clock accumulated as t += 0.005 in single precision for 2,000,000 steps: {t32:.3f} s (exact {2000000 * 0.005:.3f} s), error {t32 - 10000.0:+.3f} s; an integer tick count has none")
# position integrated from velocity: increments lost once the sum is large
x32 = f32(60000.0); x64 = 60000.0
for _ in range(1000):
    x32 = f32(x32 + f32(0.001)); x64 += 0.001
print(f"1000 increments of 1 mm added to a coordinate of 60 km: double precision gains {x64 - 60000.0:.3f} m, single precision {x32 - 60000.0:.3f} m")

# ---- 3. a discrete controller: critical sampling period of the PD loop (simulation with a zero-order hold)
def simulate(Ts, T_end=10.0, dt=0.0005, theta0=0.05):
    th, q, d = theta0, 0.0, 0.0
    u = -(KP * th + KD * q); next_s = 0.0
    n = int(T_end / dt); peak = 0.0
    for k in range(n):
        t = k * dt
        if t >= next_s - 1e-12:
            u = -(KP * th + KD * q); next_s += Ts
        d += SERVO * (u - d) * dt
        q += (LAM ** 2 * th + B * d) * dt
        th += q * dt
        peak = max(peak, abs(th))
        if abs(th) > 5.0: return None
    return abs(th), peak
print("closed loop with the control law sampled with a zero-order hold, initial tilt 0.05 rad, tilt after 10 s:")
for Ts in (0.01, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6):
    r = simulate(Ts)
    print(f"  period {Ts * 1000:4.0f} ms ({1 / Ts:5.1f} Hz): " + ("unstable" if r is None else f"stable, tilt {r[0]:.5f} rad, peak {r[1]:.3f} rad"))

# ---- 4. reconstructing a flight from its log
def fly(seed, log=None, replay=None):
    r = random.Random(seed)
    h, v = 50.0, 0.0
    he, ve = 50.0, 0.0
    out = []
    for k in range(2000):
        if replay is None:
            z = h + r.gauss(0, 0.3)
            if log is not None: log.append(z)
        else:
            z = replay[k]
        he += ve * 0.01; inn = z - he
        he += 0.8 * 0.01 * 10 * inn; ve += 0.5 * 0.01 * 10 * inn
        a_cmd = -(2.0 * (he - 10.0) + 3.0 * ve) / 10.0
        v += a_cmd * 0.01; h += v * 0.01
        out.append(he)
    return out, h
log = []
est, h_end = fly(77, log=log)
est2, h_end2 = fly(77, replay=log)
print(f"replay of the logged measurements through the same code: largest difference of the estimate {max(abs(a - b) for a, b in zip(est, est2)):.1e} m, final height difference {abs(h_end - h_end2):.1e} m")
rounded = [round(z, 2) for z in log]
est3, h_end3 = fly(77, replay=rounded)
print(f"the log written with two decimals only: largest difference of the estimate {max(abs(a - b) for a, b in zip(est, est3)):.4f} m, final height difference {abs(h_end - h_end3):.4f} m")
single = [f32(z) for z in log]
est4, h_end4 = fly(77, replay=single)
print(f"the log stored in single precision: largest difference of the estimate {max(abs(a - b) for a, b in zip(est, est4)):.1e} m")

latency budget of the pitch loop at 100 Hz:
  gyro low-pass filter (group delay)                               4.0 ms
  sampling of the gyro by the loop (age, half a period)            5.0 ms
  computation of the control law                                   3.0 ms
  hold of the command until the next actuator update (half a period)   5.0 ms
  bus and servo input latency                                      2.0 ms
  total 19.0 ms; phase margin 55.5 deg with no delay, 48.9 deg with the budget (6.6 deg lost at the 6.07 rad/s crossover)
effect of the loop rate (sampling and hold give a delay of one period, plus the fixed 9 ms of the other stages):
     20 Hz: delay  59.0 ms, phase margin  35.0 deg
     50 Hz: delay  29.0 ms, phase margin  45.4 deg
    100 Hz: delay  19.0 ms, phase margin  48.9 deg
    200 Hz: delay  14.0 ms, phase margin  50.6 deg
    400 Hz: delay  11.5 ms, phase margin  51.5 deg
delay allowed by a 45 degree phase margin: 30 ms, so a loop rate of at least 47 Hz
single-precision numbers:
  local position 300 m from the pad: stored as 300.1234, rounding error -0.0000 m, spacing of representable numbers 0.000031 m
  geocentric x-coordinate, 6378137 m: stored as 6378137.0000, rounding error -0.1235 m, spacing of representable numbers 0.500000 m
clock accumulated as t += 0.005 in single precision for 2,000,000 steps: 9815.425 s (exact 10000.000 s), error -184.575 s; an integer tick count has none
1000 increments of 1 mm added to a coordinate of 60 km: double precision gains 1.000 m, single precision 0.000 m
closed loop with the control law sampled with a zero-order hold, initial tilt 0.05 rad, tilt after 10 s:
  period   10 ms (100.0 Hz): stable, tilt 0.00000 rad, peak 0.050 rad
  period  100 ms ( 10.0 Hz): stable, tilt 0.00000 rad, peak 0.050 rad
  period  200 ms (  5.0 Hz): stable, tilt 0.00000 rad, peak 0.050 rad
  period  300 ms (  3.3 Hz): stable, tilt 0.00000 rad, peak 0.050 rad
  period  400 ms (  2.5 Hz): stable, tilt 0.00073 rad, peak 0.050 rad
  period  500 ms (  2.0 Hz): unstable
  period  600 ms (  1.7 Hz): unstable
replay of the logged measurements through the same code: largest difference of the estimate 0.0e+00 m, final height difference 0.0e+00 m
the log written with two decimals only: largest difference of the estimate 0.0019 m, final height difference 0.0012 m
the log stored in single precision: largest difference of the estimate 4.9e-07 m

In robotics

These checks are what a GNC engineer asks of the software on its first delivery.

• The timing table. Every function has a rate, a worst-case execution time and a latency from input to output, measured on the target. The latency budget is part of the policy of M12-T3: A Stability-Margin Policy: the delay of the real code is a dispersed parameter of the margin check.
• Frames and number formats. Navigation in an Earth-centred frame is done in double precision or by subtracting a reference first; the 0.5 m spacing of single precision is a navigation error that does not appear in a model run in double precision. The frame choices are in M2-T1: Frames for Flight: ECI, ECEF, Geodetic, NED, Body and Wind.
• Integer clocks and fixed steps. Time is a tick counter, and a fixed step is not a measured one: the filter of module 7 uses the actual time between samples, and a missed sample or a jitter of the schedule enters the model as a delay.
• Equivalence testing. The code and the model are run on the same inputs and their outputs compared: a difference of 1e-6 m is rounding, and one of millimetres means a different discretization. The comparison is what turns a model into a specification.
• Flight reconstruction. A flight is analysed from the log: the logged sensor inputs are replayed through the code, and the outputs must match the flown ones. When they do not, the log is incomplete or the code is not deterministic. A log written with too few digits is a lossy log: here 1.9 mm of estimate difference, small, but nonzero, so a bit-exact replay needs the original precision.

The tests that continue from here are in UAV M8-T4: SIL, HIL and ground testing.

Common misconceptions

Key takeaways

1. Each function runs at 10 to 20 times the bandwidth of its loop or more; Hopper's ratios are above 100 cycles.
2. The delay of a sampled loop is one period plus the fixed stages: 19 ms at 100 Hz, which costs 6.6° of margin.
3. The rate needed follows from the margin: 45° allows 30 ms and needs at least 47 Hz here; beyond 100 Hz the gain is small.
4. Single precision has a spacing of 0.5 m at an Earth-centred coordinate, loses 185 s in a 10 000 s accumulated 200 Hz clock, and drops 1 mm increments at 60 km.
5. Use a local frame, an integer tick counter and double precision where the numbers are large.
6. A flight replays exactly from a log of the inputs at the precision the code used.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• K. J. Åström and B. Wittenmark, Computer-Controlled Systems: Theory and Design, 3rd ed., Prentice-Hall, 1997 (Dover reprint), on sampling, aliasing and the choice of the sampling rate. Paid.
• D. Goldberg, "What every computer scientist should know about floating-point arithmetic", ACM Computing Surveys 23(1), 1991, on the spacing and rounding of floating-point numbers. Free to read from the Oracle documentation archive.
• G. F. Franklin, J. D. Powell and M. L. Workman, Digital Control of Dynamic Systems, 3rd ed., Addison-Wesley, 1998. Paid.
• MIT 16.30 Feedback Control Systems (OpenCourseWare) , for sampled-data control and delay. Free; licence CC BY-NC-SA 4.0.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Related content

Nothing below is required to finish this lesson.

• Before
  M12-T4: Modes, Redundancy and Fault Management in GNC

  Design the mode logic of a GNC system, compare voting with analytic redundancy, and define a safe mode and the conditions that enter it.

Other courses on this site treat neighbouring topics from their own angle:

• Probability M1-T3: Simulating Randomness: Sampling and Monte Carlo
• Probability M4-T4: Propagating Uncertainty Through Transformations
• Linear Algebra M9-T4: Covariance Propagation
• UAV M8-T9: Fault detection, isolation and recovery
• UAV M8-T7: FMEA
• UAV M8-T3: Verification and validation
• UAV M8-T4: SIL, HIL and ground testing
• UAV M6-T14: Software-in-the-Loop simulation
• UAV M7-T7: Jitter and deadline monitoring
• UAV M8-T1: System requirements engineering
• UAV M8-T5: Flight-test planning
• Navigation M10-T2: Engineering Trade-offs and Safety Cases
• Robot Learning M11-T2: Seeds, Variance and Statistics

This course leaves the following to the pages that already teach them:

• Requirements engineering, FMEA and fault-tree analysis as methods
• SIL and HIL test benches, real-time scheduling and time synchronization
• Flight-test planning and incident investigation