Control experiments in SITL

Example 1 Comparing two tuning sets from a rate log

A simulated log of roll-rate response to a step command, recorded at 250 Hz. Set A has high P; set B has lower P and more D.

import math

def response(zeta, w0, rate_hz=250, t_end=2.0):
    t = [k / rate_hz for k in range(int(t_end * rate_hz))]
    wd = w0 * math.sqrt(1 - zeta ** 2)
    y = [1 - math.exp(-zeta * w0 * s) * (math.cos(wd * s) + zeta / math.sqrt(1 - zeta ** 2) * math.sin(wd * s)) for s in t]
    return t, y

def metrics(t, y, target=1.0):
    t10 = next(a for a, b in zip(t, y) if b >= 0.1 * target)
    t90 = next(a for a, b in zip(t, y) if b >= 0.9 * target)
    over = max(y) / target - 1
    ts = next(t[i] for i in range(len(y)) if all(abs(v - target) <= 0.02 * target for v in y[i:]))
    return t90 - t10, over, ts

for name, zeta, w0 in (("set A (high P)", 0.45, 6.0), ("set B (lower P, more D)", 0.7, 5.0)):
    tr, mp, ts = metrics(*response(zeta, w0))
    print(f"{name:<24} rise {tr * 1000:4.0f} ms, overshoot {mp:5.1%}, settling {ts:.2f} s")

set A (high P)           rise  256 ms, overshoot 20.5%, settling 1.39 s
set B (lower P, more D)  rise  424 ms, overshoot  4.6%, settling 1.20 s

Set A rises faster but overshoots by about a fifth and settles slowly. Set B rises more slowly but barely overshoots and settles sooner, closer to what the PX4 guide asks for. With a real log, trim the part before the command and use the actual final value instead of 1.0.

Example 2 Path-tracking error

Cross-track error along a straight 60 s path recorded at 10 Hz (hypothetical data).

import math

t = [k / 10 for k in range(600)]
calm = [0.15 * math.sin(0.8 * s) for s in t]                               # m no wind
windy = [0.8 * (1 - math.exp(-s / 5)) + 0.3 * math.sin(0.5 * s) for s in t]  # m crosswind

def summary(err):
    rms = math.sqrt(sum(e * e for e in err) / len(err))
    return rms, max(abs(e) for e in err), sum(err) / len(err)

for name, err in (("calm", calm), ("crosswind", windy)):
    rms, peak, bias = summary(err)
    print(f"{name:<10} RMS {rms:.2f} m, max {peak:.2f} m, mean offset {bias:+.2f} m")

calm       RMS 0.11 m, max 0.15 m, mean offset +0.01 m
crosswind  RMS 0.78 m, max 1.10 m, mean offset +0.74 m

A crosswind gives the error a mean offset to one side, showing that the velocity loop is not fully rejecting a constant disturbance. If raising the velocity loop’s I reduces the mean, that is the right fix. After tuning by hand, compare with PX4 Autotune or ArduPilot AutoTune, which the guides recommend running around hover.