M10-T2 — The Autopilot by Successive Loop Closure

Learning objectives

After this lesson, you should be able to:

• Close a pitch-rate loop, a pitch-angle loop and an altitude loop in order, and read the margin and bandwidth of each.
• Explain why the loops are closed from the inside out and why each should be at least three times slower than the one inside it.
• Add a yaw damper with a washout and say what the washout is for.
• Explain why an altitude loop needs the speed held and why elevator-to-height has a non-minimum-phase start.

The intuition

UAV M6-T6: Flight modes lists the modes of a flight stack (stabilize, altitude hold, position hold) as options for the pilot. This lesson shows the structure behind them: which loop each mode closes, and why the loops are nested as they are. The principle is the separation of timescales of M1-T2: Nested Loops and Separation of Timescales, applied to a specific aircraft with its own numbers.

Hopper's own lateral channel is built the same way: pitch rate at 12 rad/s, pitch angle at 4, lateral velocity at 1.2 and position at 0.4, ratios of 3, 3.3 and 3. The aircraft in this lesson uses the same logic with different numbers and one new complication, the non-minimum-phase altitude response.

From the inside out

The first loop feeds the pitch rate back to the elevator. It does what the aircraft's own damping cannot: it raises the short-period damping from 0.71 to 0.99 (with a gain of 0.5) at a frequency of 7.1 rad/s, and it makes the pitch response to elevator close to a single integrator with a predictable gain. The second loop closes on pitch angle, commanding the rate loop. With the rate loop inside it the plant seen by the angle loop is simple, and the angle loop crosses over at 2.2 rad/s with a phase margin of 86°. The third loop commands pitch angle from the altitude error: at 0.38 rad/s with a phase margin of 84°. The ratios are 3.2 and 5.8, which is why each can be designed as if the one inside it were instantaneous.

The speed has to be held

A pitch-hold loop that has no control of speed lets the speed drift with the aircraft's own slow dynamics: with the elevator loops closed, a root at about +0.02 per second remains, the speed (phugoid-like) mode, and the altitude drifts away from the command. The altitude loop in the figure has the speed held by an autothrottle (modelled as extra speed damping of 0.3 per second); the energy-based way to do that properly was the subject of M8-T2: Mission Guidance: Legs, Loiters and Energy. A second feature appears in the altitude loop: the elevator that pitches the nose up first pushes the aircraft down slightly, because the tail force that rotates it also reduces lift. This right half-plane zero limits how fast the altitude loop can be, and the loop crosses over at 0.38 rad/s, far below the pitch loop.

The lateral stack and the yaw damper

The Dutch roll of the aircraft has a damping ratio of 0.11. A yaw damper feeds the yaw rate to the rudder. With gains of 0.3, 0.6 and 1.0 the damping becomes 0.34, 0.58 and 0.99. But in a steady turn the aircraft has a constant yaw rate (0.196 rad/s in a 30° bank at 25 m/s), and a plain damper would command a rudder of 6.7° against the turn. The fix is a washout: the signal passes through a high-pass filter with a time constant of 2 s, so the damper responds to oscillations and lets steady yaw rate through. The result has damping 0.60, and the steady rudder is zero.

Close loops from the inside out, each at least three times slower than the one inside; hold the speed with the throttle; and put a washout in any damper that must not oppose a steady manoeuvre.

Definitions

Successive loop closure

The design of a multi-loop controller by closing the innermost loop first and treating it as part of the plant of the next loop.

Rate, attitude, path loops

The nested loops of an autopilot: angular rate to the surface, attitude to the rate command, path (altitude, heading) to the attitude command.

Yaw damper

A feedback of yaw rate to the rudder that adds Dutch-roll damping.

Washout filter

A high-pass filter $s/(s + 1/\tau_w)$ that passes oscillations and blocks steady values.

Non-minimum-phase response

A response that first moves the wrong way: the altitude to elevator response has a right half-plane zero.

Derivation: why the inner loop simplifies the outer, and what the washout does

The effect of the rate loop on the plant seen by the angle loop.

1. $$\dot{q} = M_ww + M_qq + M_\delta u,\quad u = u_c - K_qq$$

   The pitch moment equation with the rate loop closed: u_c is the command to the rate loop.

2. $$\dot{q} = M_ww + (M_q - M_\delta K_q)\,q + M_\delta u_c$$

   Collect: the rate gain adds to the pitch damping. With $M_\delta = 14$ and $K_q = 0.5$ the damping term goes from −3 to −10, which makes the short period overdamped and fast.

3. $$\dfrac{\theta}{u_c}(s) \approx \dfrac{M_\delta}{s\,(s + M_q - M_\delta K_q)}\quad(\text{short-period dynamics fast})$$

   For a stiff inner loop the pitch response to the command is an integrator followed by one lag: the angle loop sees a simple plant.

4. $$\omega_{c,\theta}\approx \dfrac{K_\theta M_\delta}{10},\quad \omega_{c,h}\approx K_h U_0\ \ (\text{with the pitch loop at unit gain})$$

   The crossover of each loop is set by its gain times the gain of what is inside: attitude near 2.2 rad/s and altitude near $0.018 \times 25 = 0.45$ rad/s (0.38 computed).

The washout

1. $$\dfrac{r - x_w}{r}(s) = \dfrac{s}{s + 1/\tau_w}$$

   Transfer function from yaw rate to the washed-out signal: a high-pass filter with the corner 1/τ_w = 0.5 rad/s.

2. $$\text{steady turn: } s \to 0 \Rightarrow r - x_w \to 0$$

   A constant yaw rate is a zero-frequency signal and is blocked: the rudder returns to zero, so the pilot or the turn-coordination loop does not have to overcome the damper.

3. $$\text{Dutch roll at } 3.6\ \mathrm{rad/s}\gg 0.5\ \mathrm{rad/s}: \left|\dfrac{j\omega}{j\omega + 0.5}\right| \approx 0.99$$

   At the Dutch-roll frequency the filter passes the signal almost unchanged, so the damping is added there: 0.60 with the washout against 0.58 without (gain 0.6).

Each inner loop turns the plant seen by the next into something close to an integrator, which is why successive closure works when the loops are separated in bandwidth. The washout moves the damper out of the frequency range of manoeuvres.

A worked example

The longitudinal autopilot of the aircraft of the last lesson (nose-up elevator input, $M_\delta = 14,\ Z_\delta = 9$), with an autothrottle holding the speed, and the lateral yaw damper (rudder $N_{\delta_r} = 6$).

1. Open loop. Short period 4.97 rad/s (damping 0.71) and phugoid 0.40 rad/s (0.02).
2. Loop 1, rate. With $K_q = 0.5$ the short period is at 7.10 rad/s with damping 0.99 and the phugoid damping rises to 0.60; the loop crossover is 7.10 rad/s.
3. Loop 2, attitude. With $K_\theta = 2.0$: crossover 2.22 rad/s, phase margin 86°; the closed-loop poles are 7.48 rad/s (damping 0.81) and two real poles at −1.77 and −0.41.
4. Loop 3, altitude. With $K_h = 0.018$ rad/m: crossover 0.382 rad/s, phase margin 84°. Bandwidth ratios: 3.2 (rate to attitude) and 5.8 (attitude to altitude).
5. Altitude step. A 20 m command: 90% in 9.7 s, no overshoot, peak pitch 6.6°, peak elevator 13.8°. A 120 m step with the pitch command limited to 6.9° takes 51.8 s to reach 90%: a climb rate set by the limit, 2.3 m/s.
6. Yaw damper. Dutch-roll damping goes from 0.112 (open loop) to 0.337, 0.576 and 0.988 for gains 0.3, 0.6 and 1.0, with the frequency falling from 3.63 to 3.60, 3.47 and 2.95 rad/s. With a 2 s washout and gain 0.6: damping 0.599 at 3.11 rad/s.
7. The turn. A 30° bank at 25 m/s gives a turn rate of 12.98°/s, a load factor of 1.155 and a yaw rate of 0.196 rad/s; an unwashed damper of gain 0.6 would command 6.7° of rudder against the turn, which the washout removes (6×10⁻¹³ degrees after 60 s).

import math
import gnckit as g

U0, G = 25.0, 9.81
Xu, Zu, Zw, Mw, Mq = -0.06, -0.80, -4.0, -0.5, -3.0
# longitudinal states (u, w, q, theta, h); input: nose-up elevator-equivalent deflection (rad)
A5 = [[Xu, 0.0, 0.0, -G, 0.0], [Zu, Zw, U0, 0.0, 0.0], [0.0, Mw, Mq, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0, 0.0], [0.0, -1.0, 0.0, U0, 0.0]]
B5 = [0.0, 9.0, 14.0, 0.0, 0.0]
A5[0][0] = Xu - 0.3                                # an autothrottle holds the speed: it adds -0.3 to the speed damping (the slow speed divergence of a pitch-hold loop is otherwise left to the pilot)
def closed(A, B, K):
    return [[A[i][j] - B[i] * K[j] for j in range(len(A))] for i in range(len(A))]

def pairs(A):
    out = []
    for e in g.mat_eigvals(A):
        e = complex(e)
        if e.imag > 1e-9: out.append(f'{abs(e):.2f} rad/s, zeta {-e.real / abs(e):.2f}')
        elif abs(e.imag) < 1e-9: out.append(f'real {e.real:.3f}')
    return out

def L_freq(A, B, C, K, w):
    """Frequency response K * C (jwI - A)^-1 B for the loop closed at the actuator, K scalar."""
    return K * g.ss_freq(A, B, C, 0.0, w)

def margins_of(A, B, C, K):
    ws = g.logspace(-3, 2, 4000)
    # crossover and phase margin by scanning
    prev = None; wc = None; pm = None; ph0 = None
    import cmath
    unwrapped = 0.0; last = None
    for w in ws:
        v = L_freq(A, B, C, K, w)
        a = math.degrees(cmath.phase(v))
        if last is not None:
            d = a - last
            while d > 180: d -= 360
            while d < -180: d += 360
            unwrapped += d
        else:
            unwrapped = a
        last = a
        if prev is not None and abs(prev) > 1 >= abs(v):
            wc = w; pm = 180 + unwrapped                  # the last crossing from above: the highest-frequency gain crossover
        prev = abs(v)
    return wc, pm

Iq = [0, 0, 1, 0, 0]; It = [0, 0, 0, 1, 0]; Ih = [0, 0, 0, 0, 1]
print('LONGITUDINAL AUTOPILOT, loops closed from the inside out (aircraft of the previous lesson, nose-up elevator input)')
print('open loop modes (no autothrottle):  ', '; '.join(pairs([[Xu, 0, 0, -G], [Zu, Zw, U0, 0], [0, Mw, Mq, 0], [0, 0, 1, 0]])))
Kq = 0.50
K1 = [0, 0, Kq, 0, 0]
A1 = closed(A5, B5, K1)
print(f'1. pitch-rate damper u = -{Kq} q:      ', '; '.join(pairs([r[:4] for r in A1[:4]])))
# the rate loop gain with the inner nothing: margins of Kq q / u
wc, pm = margins_of(A5, B5, Iq, Kq)
print(f'   rate loop: crossover {wc:.2f} rad/s, phase margin {pm:.0f} deg')
Kth = 2.0
# attitude loop: u = Kth (theta_c - theta) - Kq q ; loop gain Kth * theta / u_cmd with the rate loop closed
wc2, pm2 = margins_of(A1, B5, It, Kth)
K2 = [0, 0, Kq, Kth, 0]
A2 = closed(A5, B5, K2)
print(f'2. pitch-angle loop Kth = {Kth}:       ', '; '.join(pairs([r[:4] for r in A2[:4]])), f' | crossover {wc2:.2f} rad/s, phase margin {pm2:.0f} deg')
Kh = 0.018
# altitude loop: theta_c = Kh (h_c - h); loop gain Kh * h / theta_c with the pitch loop closed (the signal is u_cmd = Kth theta_c)
# h / theta_c for closed pitch loop: input matrix Kth * B5
B_theta_c = [Kth * b for b in B5]
wc3, pm3 = margins_of(A2, B_theta_c, Ih, Kh)
print(f'3. altitude loop Kh = {Kh} rad/m:   crossover {wc3:.3f} rad/s, phase margin {pm3:.0f} deg')
print(f'   bandwidth ratios: rate / attitude = {wc / wc2:.1f}, attitude / altitude = {wc2 / wc3:.1f}')

# step response of the complete longitudinal autopilot: 20 m altitude step
K3 = [0, 0, Kq, Kth + 0.0, 0]
def sim_alt(hstep=20.0, T=40.0, dt=0.01, limit=0.12):
    x = [0.0] * 5; hist = []
    for i in range(int(T / dt)):
        thc = max(-limit, min(limit, Kh * (hstep - x[4])))
        u = Kth * (thc - x[3]) - Kq * x[2]
        d = g.mat_vec(A5, x)
        x = [x[k] + dt * (d[k] + B5[k] * u) for k in range(5)]
        hist.append((i * dt, x[4], x[3], u))
    return hist
h = sim_alt()
t90 = next(t for t, hh, th, u in h if hh >= 18.0)
ov = max(hh for t, hh, th, u in h) - 20.0
print(f'   20 m altitude step: 90% in {t90:.1f} s, overshoot {max(ov, 0):.2f} m, peak pitch {max(th for t, hh, th, u in h) * 57.3:.1f} deg, peak elevator {max(abs(u) for t, hh, th, u in h) * 57.3:.1f} deg')
h2 = sim_alt(hstep=120.0, T=80.0)
print(f'   120 m step with the pitch command limited to {0.12 * 57.3:.1f} deg: peak pitch {max(th for t, hh, th, u in h2) * 57.3:.1f} deg, 90% in {next(t for t, hh, th, u in h2 if hh >= 108.0):.1f} s')

# LATERAL: yaw damper and washout
print('LATERAL: Dutch roll damping with a yaw damper, with and without a washout')
Yv, Lv, Lp, Lr, Nv, Np, Nr = -0.25, -1.0, -8.0, 1.0, 0.45, -0.3, -0.8
Alat = [[Yv, 0.0, -U0, G], [Lv, Lp, Lr, 0.0], [Nv, Np, Nr, 0.0], [0.0, 1.0, 0.0, 0.0]]
Br = [0.5, 0.3, 6.0, 0.0]                          # rudder: positive turns the nose to the right
def dr(A):
    best = None
    for e in g.mat_eigvals(A):
        e = complex(e)
        if e.imag > 1e-9 and (best is None or abs(e) > abs(best)): best = e
    return best
e0 = dr(Alat)
print(f'  open loop Dutch roll: {abs(e0):.2f} rad/s, damping ratio {-e0.real / abs(e0):.3f}')
for Kr in (0.3, 0.6, 1.0):
    Acl = closed(Alat, Br, [0, 0, Kr, 0])
    e = dr(Acl)
    print(f'  yaw damper delta_r = -{Kr} r: Dutch roll {abs(e):.2f} rad/s, damping ratio {-e.real / abs(e):.3f}')
tauw = 2.0
Kr = 0.6
Aw = [[Alat[i][j] if (i < 4 and j < 4) else 0.0 for j in range(5)] for i in range(5)]
Aw[4][2] = 1.0 / tauw; Aw[4][4] = -1.0 / tauw           # washout state: xw' = (r - xw) / tau
Bw = Br + [0.0]
Kw = [0, 0, Kr, 0, -Kr]                                # u = -Kr (r - xw)
Aclw = closed(Aw, Bw, Kw)
evs = g.mat_eigvals(Aclw)
dre = max((complex(e) for e in evs if complex(e).imag > 1e-9), key=lambda c: abs(c))
print(f'  with a washout of {tauw:.0f} s and gain {Kr}: Dutch roll {abs(dre):.2f} rad/s, damping ratio {-dre.real / abs(dre):.3f}; the washout slow pole {min((complex(e) for e in evs if abs(complex(e).imag) < 1e-9 and complex(e).real > -1.0), key=lambda c: abs(c)).real:.3f}')
phi = math.radians(30.0)
psid = G * math.tan(phi) / U0
r_ss = psid * math.cos(phi)
print(f'  steady 30 deg turn at {U0:.0f} m/s: turn rate {math.degrees(psid):.2f} deg/s, load factor {1 / math.cos(phi):.3f}, yaw rate {r_ss:.3f} rad/s')
print(f'  an unwashed damper (gain {Kr}) would command a rudder of {math.degrees(Kr * r_ss):.1f} deg against the turn; the washout passes it to {math.degrees(Kr * r_ss * math.exp(-60 / tauw)):.2g} deg after 60 s of turn')

LONGITUDINAL AUTOPILOT, loops closed from the inside out (aircraft of the previous lesson, nose-up elevator input)
open loop modes (no autothrottle):   4.97 rad/s, zeta 0.71; 0.40 rad/s, zeta 0.02
1. pitch-rate damper u = -0.5 q:       7.11 rad/s, zeta 0.99; 0.28 rad/s, zeta 0.60
   rate loop: crossover 7.10 rad/s, phase margin 125 deg
2. pitch-angle loop Kth = 2.0:        7.48 rad/s, zeta 0.81; real -1.771; real -0.414  | crossover 2.22 rad/s, phase margin 86 deg
3. altitude loop Kh = 0.018 rad/m:   crossover 0.382 rad/s, phase margin 84 deg
   bandwidth ratios: rate / attitude = 3.2, attitude / altitude = 5.8
   20 m altitude step: 90% in 9.7 s, overshoot 0.00 m, peak pitch 6.6 deg, peak elevator 13.8 deg
   120 m step with the pitch command limited to 6.9 deg: peak pitch 6.6 deg, 90% in 51.8 s
LATERAL: Dutch roll damping with a yaw damper, with and without a washout
  open loop Dutch roll: 3.63 rad/s, damping ratio 0.112
  yaw damper delta_r = -0.3 r: Dutch roll 3.60 rad/s, damping ratio 0.337
  yaw damper delta_r = -0.6 r: Dutch roll 3.47 rad/s, damping ratio 0.576
  yaw damper delta_r = -1.0 r: Dutch roll 2.95 rad/s, damping ratio 0.988
  with a washout of 2 s and gain 0.6: Dutch roll 3.11 rad/s, damping ratio 0.599; the washout slow pole -0.019
  steady 30 deg turn at 25 m/s: turn rate 12.98 deg/s, load factor 1.155, yaw rate 0.196 rad/s
  an unwashed damper (gain 0.6) would command a rudder of 6.7 deg against the turn; the washout passes it to 6.3e-13 deg after 60 s of turn

In robotics

The structure is the one of every conventional autopilot.

• Typical gains. Flight stacks expose the loop gains and time constants (rate P and D, attitude P, altitude P) and the default separation between them is around 3 to 5; changing one loop without regard to its neighbours is the usual way to lose margin.
• Autothrottle and total energy. The speed hold used here is the simplest form; the energy-based pairing of elevator and throttle of module 8 handles thrust limits properly, and the altitude loop of this lesson is the elevator half of it.
• Turn coordination. In a bank the rudder is used to keep the sideslip small, and the aileron-to-rudder interconnect (or the yaw damper with washout) does it so that the pilot or the heading loop need not. The load factor of 1.155 g in a 30° bank needs the pitch loop to increase the elevator by about 15% of its level-flight trim.
• Limits. The pitch command limit of 6.9° sets the climb rate for large steps (2.3 m/s here); the elevator limit and rate limit of the real servo limit the rate loop, and windup protection (module 3) applies to each integrator.
• The non-minimum-phase start. The initial dip in altitude after an up-elevator command is why altitude is the outermost loop and the slowest: a faster altitude loop would drive the zero into the loop and lose its phase margin.

The lab M10-L2: Close the Loops in Order builds this stack from the rate loop to the glide slope. The glide-slope loop of M10-T4: Approach and Automatic Landing sits outside the altitude loop; the margin requirements for the whole stack are in M10-T5: Handling Qualities, Margins and Envelope Protection.

Common misconceptions

Key takeaways

1. Close loops from the inside out: pitch rate (7.1 rad/s), pitch angle (2.2 rad/s), altitude (0.38 rad/s), ratios 3.2 and 5.8.
2. The rate loop raises the short-period damping from 0.71 to 0.99 and makes the plant seen by the next loop close to an integrator.
3. An altitude loop needs the speed held; otherwise a slow root at +0.02 per second remains.
4. A yaw damper with gain 0.6 raises the Dutch-roll damping from 0.11 to 0.58, and 0.60 with a 2 s washout.
5. An unwashed damper would command 6.7° of rudder against a 30° turn; the washout removes it.
6. The altitude step of 20 m takes 9.7 s to 90% with no overshoot, and the climb rate for large steps is set by the pitch limit.

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.

• B. L. Stevens, F. L. Lewis and E. N. Johnson, Aircraft Control and Simulation, 3rd edition, Wiley, 2015, the chapters on stability augmentation and autopilot design. Paid.
• R. W. Beard and T. W. McLain, Small Unmanned Aircraft: Theory and Practice, Princeton University Press, 2012, the chapter on autopilot design using successive loop closure. Paid.
• ArduPilot documentation, plane tuning quickstart , which describes the loops a working autopilot exposes. Free to read; licence CC BY-SA 3.0.
• MIT 16.30 Feedback Control Systems (OpenCourseWare) , for the nested-loop design method. 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
  M10-T1: Aircraft Dynamics and Their Five Modes

  Separate the longitudinal and lateral-directional equations, and relate the short period, phugoid, roll, spiral and Dutch roll to stability derivatives.

• Lab
  M10-L2: Close the Loops in Order

  Build the longitudinal autopilot of the survey aircraft from pitch rate to glide slope, one loop at a time, and land within the sink-rate limit in turbulence.

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

• UAV M1-T2: Multirotor, fixed-wing and VTOL architectures
• UAV M5-T1: UAV equations of motion (wind triangle)
• UAV M6-T6: Flight modes
• UAV M6-T3: PX4 architecture
• Navigation M9-T5: Other Platforms: Legged, Aerial, Marine and Off-Road

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

• Multirotor flight control
• The wind triangle
• PX4 and ArduPilot software, flight modes and tooling