Chapter 6

Successive Loop Closure

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 1/33
 Successive Loop Closure

Open-loop system

Closed-loop system

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 2/33
 SLC: Inner Loop Closed

At frequencies below
inner-loop bandwidth,
approximate CLTF as 1,
then design middle loop

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 3/33
 SLC: Two Loops Closed

At frequencies below
middle-loop bandwidth,
approximate CLTF as 1,
then design outer loop

Key idea: Each successive loop must be lower in bandwidth

--- typically by a factor of 5 to 10

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 4/33
 Saturation Limits
uunsat

The control signal u is largest immediately after a step on yc , at which point

the output of the di↵erentiator is essentially zeros. Therefore u ⇡ kp e. Let umax
be the input saturation limit, and emax , the largest expected step, then set
umax
kp = max .
e
The closed loop transfer function is

b0 k p c !n2 c
Y (s) = 2 Y (s) = 2 Y (s)
s + (a1 + b0 kd )s + (a0 + b0 kp ) s + 2⇣!n s + !n2

gives
2⇣!n a1 p
kd = , where !n = a 0 + b0 k p .
b0
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 5/33
 Lateral-directional Autopilot

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 6/33
 Roll Autopilot
max
a

kp a 2 !n2
H / c (s) = =
s2 + (a 1
+ a 2 kd )s + kp a 2
s2 + 2⇣ !n s + !n2

Closed loop TF Canonical 2nd-order TF

Design parameters are emax and ⇣

Gains are given by

max 2⇣ !n a
a 1
kp = kd =
emax a 2

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 7/33
 Linearization Effects and Disturbances

max
a

•  Zero steady-state error to step in roll angle

•  Non-zero steady-state error to step in disturbance
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 8/33
 Add Integrator to Reject Disturbances

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 9/33
 Add Integrator to Reject Disturbances

max
a

✓ ◆
s
= d
s3 + (a 1 + a 2 kd )s2 + a 2 kp s + a 2 ki 2

0 ⇣ ⌘ 1
ki
a 2 k p s + kp
B C c
+@ 3 A
s + (a 1 + a 2 kd )s2 + a 2 kp s + a 2 ki

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 10/33
 Roll loop – Integrator – Root Locus
25

20

15
Axis

10
imaginary axis

5
Imaginary

−5

−10

−15

−20

−25
−30 −20 −10 0 10 20
real axis
Real Axis

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 11/33
 Course Hold Loop

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 12/33
 TF Zero Affects Response

10 1.4

−10 1.2
magnitude (dB)

−20

−30 1

−40

−50 0.8

−60
0
0.6
phase (deg)

−30
0.4

−60 0.2

−90 0
−2 −1 0 1 2 3
10 10 10 10 10 10 0 2 4 6 8 10 12
frequency (rad/sec) (sec)

2⇣!n s + !n2
H= 2 Canonical 2nd-order TF
s + 2⇣!n s + !n2

Note that ⇣ has a di↵erent e↵ect when the zero is present.

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 13/33
 Course Hold Loop

(g/Vg )s (kp g/Vg )s + (ki g/Vg ) c

= d +
s2 + (kp g/Vg )s + (ki g/Vg ) s2 + (kp g/Vg )s + (ki g/Vg )

Equating coefficients gives: !n2 = g/Vg ki and 2⇣ !n = g/Vg kp .

Design parameters are bandwidth separation W and damping ratio ⇣

1
!n = !n kp = 2⇣ !n Vg /g ki = !n2 Vg /g
W

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 14/33
 Sideslip Hold

max
r
e

Design parameters are bandwidth separation emax and damping ratio ⇣

Equating coefficients gives
max
r a 1
+ a 2 kp !n2
kp = !n = ki =
emax 2⇣ a 2

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 15/33
 Lateral Autopilot - Summary
If model is known, the the design parameters are
Inner Loop (roll attitude hold)
• emax - Error in roll when aileron just saturates.
• ⇣ - Damping ratio for roll attitude loop.

• ki - Integrator in roll loop to reject disturbances.

Outer Loop (course hold)
• W > 1 - Bandwidth separation between roll and course loops.

• ⇣ - Damping ratio for course hold loop.

Sideslip hold (if rudder is available)
• emax - Error in sideslip when rudder just saturates.
• ⇣ - Damping ratio for sideslip loop.

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 16/33
 Lateral Autopilot – In Flight Tuning
If model is not known, and autopilot must be tuned in flight, then the
following gains are tuned one at a time, in this specific order:
Inner Loop (roll attitude hold)
• kd - Increase kd until onset of instability, and the back o↵ by 20%.

• kp - Tune kp to get acceptable step response.

• ki - Tune ki to remove steady state error.
Outer Loop (course hold)
• kp - Tune kp to get acceptable step response.

• ki - Tune ki to remove steady state error.

Sideslip hold (if rudder is available)
• kp - Tune kp to get acceptable step response.

• ki - Tune ki to remove steady state error.

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 17/33
 Longitudinal Flight Regimes

Descend zone

Altitude hold zone

hc

Climb zone

Take-off zone
✓c

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 18/33
 Pitch Attitude Hold
max
e

k p ✓ a ✓3 K✓DC !n2 ✓
H✓/✓c (s) = = 2
s2 + (a✓1 + kd✓ a✓3 )s + (a✓2 + kp✓ a✓3 ) s + 2⇣✓ !n✓ s + !n2 ✓

Closed loop TF Note: Non-unity DC gain

k a
K✓DC = athetap✓+k✓p3 a✓ !
Design parameters are emax
✓ and ⇣✓ 2 ✓ 3

Equating coefficients, the gains are given by

max
e
p 2⇣✓ !n✓ a✓1
kp✓ = sign(a✓3 ) ! n✓ = a ✓2 + k p ✓ a ✓ 3 kd =
emax
✓ a ✓3

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 19/33
 Altitude Hold Using Commanded Pitch

Provided pitch loop functions as intended, we can simplify the

inner-loop dynamics to

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 20/33
 Altitude from Pitch – Simplified

Tracks commands and rejects disturbances with zero steady-state error

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 21/33
 Altitude from Pitch Gain Calculations

Comparing coefficients to the canonical second order transfer function

2⇣!n s + !n2
H= 2
s + 2⇣!n s + !n2

give the coefficients

!n2 h 2⇣h !nh

k ih = kph =
K✓DC Va K✓DC Va

where bandwidth separation is achieved by selecting

1
! nh = !n
Wh ✓

Design parameters are bandwidth separation Wh and damping ratio ⇣✓

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 22/33
 Airspeed Hold Using Commanded Pitch

Tracks commands and rejects disturbances with zero steady-state error

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 23/33
 Airspeed from Pitch Gain Calculations
0 ✓ ◆ 1
ki V
K✓DC gkpV2 s + 2 !
B kpV C c s
V̄a (s) = B C V̄ (s)+
2
@ s2 + (aV dV (s)
1
K✓ gkp )s
DC V2
K✓DC gkiV2 A a s2 + (aV1 K✓DC gkpV2 )s K✓DC gkiV2

Comparing coefficients to the canonical second order transfer function

2⇣!n s + !n2
H= 2
s + 2⇣!n s + !n2

give the coefficients

!n2 V2 aV1 2⇣V2 !nV2

k iV 2 = kp V2 =
K✓DC g K✓DC g

where bandwidth separation is achieved by selecting

1
! nV 2 = !n
WV2 ✓

Design parameters are bandwidth separation WV2 and damping ratio ⇣V2

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 24/33
 Airspeed Hold Using Throttle

kiV
s
V̄ac ¯t V̄a
kpV

✓ ◆ ✓ ◆
aV2 (kpV s + kiV ) s
V̄a = V̄ac + dV
s2 + (aV1 + aV2 kpV )s + aV2 kiV s2 + (aV1 + aV2 kpV )s + aV2 kiV

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 25/33
 Airspeed Hold Using Throttle
✓ ◆ ✓ ◆
aV2 (kpV s + kiV ) s
V̄a = V̄ac + dV
s2 + (aV1 + aV2 kpV )s + aV2 kiV s2 + (aV1 + aV2 kpV )s + aV2 kiV

Comparing coefficients to the canonical second order transfer function give

the coefficients
!n2 V 2⇣V !nV aV1
k iV = kpV =
a V2 aV2

Design parameters are natural frequency !nV and damping ratio ⇣V .

The control signal is

⇤ k iV
t = t + kpV (Vac Va ) + (Vac Va )
s

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 26/33
 we have described four different longitudinal autopilot modes: (1) pitch atti-
tude hold, (2) altitude hold using commanded pitch, (3) airspeed hold using
commanded pitch, and (4) airspeed hold using throttle. These longitudinal
control modes can be combined to create the altitude control state machine
Altitude Control State Machine
shown in Figure 6.20. In the climb zone, the throttle is set to its maximum

Beard & McLain, “Small Figure

Unmanned
6.20:Aircraft,” Princeton
Altitude-control University
state machine.Press, 2012, Chapter 6: 27/33

value ( t = 1) and the airspeed hold from commanded pitch mode is used to
 Longitudinal Autopilot - Summary
If model is known, the the design parameters are
Inner Loop (pitch attitude hold)
• emax
✓ - Error in pitch when elevator just saturates.
• ⇣✓ - Damping ratio for pitch attitude loop.

Altitude Hold Outer Loop

• Wh > 1 - Bandwidth separation between pitch and altitude loops.
• ⇣h - Damping ratio for altitude hold loop.

Airspeed Hold Outer Loop

• WV2 > 1 - Bandwidth separation between pitch and airspeed loops.
• ⇣V2 - Damping ratio for airspeed hold loop.
Throttle hold (inner loop)

• !nV - Natural frequency for throttle loop.

• ⇣V - Damping ratio for throttle loop.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 28/33
 Longitudinal Autopilot – In Flight Tuning
If model is not known, and autopilot must be tuned in flight, then the
following gains are tuned one at a time, in this specific order:
Inner Loop (pitch attitude hold)
• kd✓ - Increase kd✓ until onset of instability, and the back o↵ by 20%.

• kp✓ - Tune kp✓ to get acceptable step response.

Altitude Hold Outer Loop
• kph - Tune kph to get acceptable step response.
• kih - Tune kih to remove steady state error.

Airspeed Hold Outer Loop

• kpV2 - Tune kpV2 to get acceptable step response.
• kiV2 - Tune kiV2 to remove steady state error.

Throttle hold (inner loop)

• kpV - Tune kpV to get acceptable step response.
• kiV - Tune kiV to remove steady state error.

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 29/33
 PID Loop Implementation

PID continuous time

Taking Laplace transform…

Use band limited-differentiator to reduce noise

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 30/33
 PID Loop Implementation
Tustin’s rule or trapezoidal rule

Integrator term

Differentiator term

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 31/33
 Integrator Anti-wind-up
control before anti-wind-up update

control after anti-wind-up update

is anti-wind-up update

subtracting top two equations

Let value of control after saturation is applied

solving…

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 32/33
 Transforming to the time domain, we have
# $ # $
2τ − Ts 2
D[n] = D[n − 1] + (E [n] − E [n − 1]) . (6.30)
2τ + Ts 2τ + Ts

PID Implementation
Matlab code that implements a general PID loop is shown below.

1 function u = pidloop(y_c, y, flag, kp, ki, kd, limit, Ts, tau)

2 persistent integrator;
3 persistent differentiator;
4 persistent error_d1;
5 if flag==1, % reset (initialize) persistent variables
6 % when flag==1
7 integrator = 0;
8 differentiator = 0;
9 error_d1 = 0; % _d1 means delayed by one time step
10 end
11 error = y_c − y; % compute the current error
12 integrator = integrator + (Ts/2)*(error + error_d1);
13 % update integrator
14 differentiator = (2*tau−Ts)/(2*tau+Ts)*differentiator...
15 + 2/(2*tau+Ts)*(error − error_d1);
16 % update differentiator
17 error_d1 = error; % update the error for next time through
18 % the loop
19 u = sat(... % implement PID control
20 kp * error +... % proportional term
21 ki * integrator +... % integral term
22 kd * differentiator,... % derivative term
23 limit... % ensure abs(u)<=limit
24 );
25 % implement integrator anti−windup
26 if ki~=0
27 u_unsat = kp*error + ki*integrator + kd*differentiator;
28 integrator = integrator + Ts/ki * (u − u_unsat);
29 end
30

31 function out = sat(in, limit)

32 if in > limit, out = limit;
33 elseif in < −limit; out = −limit;
34 else out = in;
35 end

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: 33/33