Uavbook Supplement

uavbook March 18, 2020
Small Unmanned Aircraft

Small Unmanned Aircraft
Theory and Practice

Supplement
Randal W. Beard
Timothy W. McLain
PRINCETON UNIVERSITY PRESS

PRINCETON AND OXFORD
Contents
1. Introduction 1
1.1 System Architecture 1
1.2 Design Models 1
1.3 Design Project 1
2. Coordinate Frames 3
2.1 Rotation Matrices 3
2.2 MAV Coordinate Frames 3
2.3 Airspeed, Wind Speed, and Ground Speed 3
2.4 The Wind Triangle 3
2.5 Differentiation of a Vector 4
2.6 Chapter Summary 4
3. Kinematics and Dynamics 5

3.1 State Variables 5
3.2 Kinematics 5
3.3 Rigid-body Dynamics 5
4. Forces and Moments 7

4.1 Gravitational Forces 7
4.2 Aerodynamic Forces and Moments 7
4.3 Propulsion Forces and Moments 8
4.4 Atmospheric Disturbances 13
5. Linear Design Models 21

5.1 Summary of Nonlinear Equations of Motion 21
5.2 Coordinated Turn 23
5.3 Trim Conditions 23
5.4 Transfer Function Models 23
5.5 Linear State-space Models 26
vi CONTENTS
6. Autopilot Design 33
6.1 Successive Loop Closure 33
6.2 Total Energy Control 49
6.3 LQR Control 51
7. Sensors for MAVs 57

7.1 Accelerometers 57
7.2 Rate Gyros 57
7.3 Pressure Sensors 57
7.4 Digital Compasses 57
7.5 Global Positioning System 57
8. State Estimation 61
8.1 Benchmark Maneuver 61
8.2 Low-pass Filters 61
8.3 State Estimation by Inverting the Sensor Model 61
8.4 Complementary Filter 61
8.5 Dynamic-observer Theory 70
8.6 Derivation of the Continuous-discrete Kalman Filter 70
8.7 Covariance Update Equations 71
8.8 Measurement Gating 72
8.9 Attitude Estimation 75
8.10 GPS Smoothing 75
8.11 Full State EKF 75
9. Design Models for Guidance 87

9.1 Autopilot Model 87
9.2 Kinematic Model of Controlled Flight 87
9.3 Kinematic Guidance Models 88
9.4 Dynamic Guidance Model 88
9.5 The Dubins Airplane Model 88
10. Straight-line and Orbit Following 93

10.1 Straight-line Path Following 93
10.2 Orbit Following 94
10.3 3D Vector-field Path Following 96
11. Path Manager 103

CONTENTS vii
11.1 Transitions Between Waypoints 103

11.2 Dubins Paths 104
12. Path Planning 107

12.1 Point-to-Point Algorithms 107
13. Vision-guided Navigation 109

13.1 Gimbal and Camera Frames and Projective Geometry 109
13.2 Gimbal Pointing 109
13.3 Geolocation 110
13.4 Estimating Target Motion in the Image Plane 110
13.5 Time to Collision 110
13.6 Precision Landing 110
A. Nomenclature and Notation 113
B. Quaternions 115
B.1 Another look at complex numbers 115
B.2 Quaternion Rotations 115
B.3 Conversion Between Euler Angles and Quaternions 115
B.4 Conversion Between Quaternions and
Rotation Matrices 115
B.5 Quaternion Kinematics 115
B.6 Aircraft Kinematic and Dynamic Equations 115
C. Simulation Using Object Oriented Programming 117

C.1 Introduction 117
C.2 Numerical Solutions to Differential Equations 117
D. Animation 125
D.1 3D Animation Using Python 125
D.2 3D Animation Using Matlab 125
D.3 3D Animation Using Simulink 125
D.4 Handle Graphics in Matlab 125
D.5 Animation Example: Inverted Pendulum 125
D.6 Animation Example: Spacecraft using Lines 126
D.7 Animation Example: Spacecraft Using Vertices and Faces 128
E. Airframe Parameters 131
F. Trim and Linearization 133

F.1 Numerical Computation of the Jacobian 133
viii CONTENTS
F.2 Trim and Linearization in Simulink 135

F.3 Trim and Linearization in Matlab 137
F.4 Trim and Linearization in Python 137
G. Essentials from Probability Theory 139
H. Sensor Parameters 141

H.1 Rate Gyros 141
H.2 Accelerometers 141
H.3 Pressure Sensors 141
H.4 Digital Compass/Magnetometer 141
H.5 GPS 141
I. Useful Formulas and other Information 143

I.1 Conversion from knots to mph 143
I.2 Density of Air 143
Bibliography 145
Index 147
Chapter One
Introduction
1.1 SYSTEM ARCHITECTURE
1.2 DESIGN MODELS
1.3 DESIGN PROJECT

Chapter Two
Coordinate Frames
2.1 ROTATION MATRICES
2.2 MAV COORDINATE FRAMES
2.2.1 The inertial frame F i

2.2.2 The vehicle frame F v
2.2.3 The vehicle-1 frame F v1
2.2.4 The vehicle-2 frame F v2
2.2.5 The body frame F b
2.2.6 The stability frame F s
2.2.7 The wind frame F w
2.3 AIRSPEED, WIND SPEED, AND GROUND SPEED
MODIFIED MATERIAL: Combining expressions, we can express the air-

speed vector body-frame components in terms of the airspeed magnitude,
angle of attack, and sideslip angle as
   
ur Va
Vab =  vr  = Rbw  0 
wr 0
  
cos β cos α − sin β cos α − sin α Va
=  sin β cos β 0  0 ,
cos β sin α − sin β sin α cos α 0
2.4 THE WIND TRIANGLE

4 COORDINATE FRAMES
MODIFIED MATERIAL: Similarly, the airspeed vector in the inertial frame

can be expressed as
 
cos(ψ + β) cos γa
Vai = Va  sin(ψ + β) cos γa  ,
− sin γa
where Va = kVa k. Under the assumption that the sideslip angle is zero, the
wind triangle can be expressed in inertial coordinates as
     
cos χ cos γ cos(ψ + β) cos γa wn
Vg  sin χ cos γ  = Va  sin(ψ + β) cos γa  +  we  . (2.1)
− sin γ − sin γa wd
2.5 DIFFERENTIATION OF A VECTOR
2.6 CHAPTER SUMMARY
NOTES AND REFERENCES
2.7 DESIGN PROJECT

Chapter Three
Kinematics and Dynamics
3.1 STATE VARIABLES
3.2 KINEMATICS
3.3 RIGID-BODY DYNAMICS
3.3.1 Rotational Motion
3.4 CHAPTER SUMMARY
3.5 DESIGN PROJECT
MODIFIED MATERIAL:
3.1 Implement the MAV equations of motion given in equations (??)

through (??). Assume that the inputs to the block are the forces and
moments applied to the MAV in the body frame. Changeable param-
eters should include the mass, the moments and products of inertia,
and the initial conditions for each state. Use the parameters given in
Appendix E.
3.2 Verify that the equations of motion are correct by individually set-
ting the forces and moments along each axis to a nonzero value and
convincing yourself that the motion is appropriate.
3.3 Since Jxz is non-zero, there is gyroscopic coupling between roll and
yaw. To test your simulation, set Jxz to zero and place nonzero mo-
ments on l and n and verify that there is no coupling between the
roll and yaw axes. Verify that when Jxz is not zero, there is coupling
between the roll and yaw axes.
Chapter Four
Forces and Moments
4.1 GRAVITATIONAL FORCES
NEW MATERIAL: Using a unit quaternion to represent attitude instead of

Euler angles, the gravity force can be expressed as
 
2(ex ez − ey e0 )
fgb = mg  2(ey ez + ex e0 )  .
e2z + e20 − e2x − e2y
4.2 AERODYNAMIC FORCES AND MOMENTS
4.2.1 Control Surfaces
MODIFIED MATERIAL: Mathematically, we can convert between rudder-

vators and rudder-elevator signals as

δe 1 1 1 δrr
= .
δr 2 1 −1 δrl
MODIFIED MATERIAL: Mathematically, we can convert between elevons
and aileron-elevator signals with

δe 1 1 1 δer
= .
δa 2 −1 1 δel
4.2.2 Longitudinal Aerodynamics
MODIFIED MATERIAL: The parameter eos is the Oswald efficiency fac-

tor, which ranges between 0.8 and 1.0 [1].
4.2.3 Lateral Aerodynamics
MODIFIED MATERIAL: The coefficient CY0 is the value of the lateral

force coefficient CY when β = p = r = δa = δr = 0.
8 FORCES AND MOMENTS
4.2.4 Aerodynamic Coefficients
4.3 PROPULSION FORCES AND MOMENTS
NEW MATERIAL:
This section describes a model for the thrust and torque produced by a
motor/propeller pair. The inputs to the thrust and torque model will be the
airspeed of the aircraft Va and the throttle setting δt ∈ [0, 1]. We assume
that the thrust and torque vectors produced by the propeller/motor is aligned
with the rotation axes of the motor and denote the magnitude of the thrust
by Tp and the magnitude of the torque by Qp .
Based on propeller theory [], the standard model for the thrust and torque
produced by a propeller is given by
ρD4 2
Tp (Ωp , CT ) = Ω CT
4π 2 p
ρD5 2
Qp (Ωp , CQ ) = Ω CQ ,
4π 2 p
where ρ is the density of air, D is the propeller diameter, Ωp is the propeller

speed in radians per second, and CT and CQ are non-dimensional aerody-
namic coefficients. The aerodynamic coefficients are found experimentally
and typical plots are shown in figures 4.1 and 4.2, where CT and CQ are
plotted as a function of the nondimensional advanced ratio
2πVa
J(Ωp , Va ) = .
Ωp D
As shown in figures 4.1 and 4.2, aerodynamic coefficients can be approxi-

mated as quadratic functions of J. Accordingly we have
CT (J) ≈ CT 2 J 2 + CT 1 J + CT 0
CQ (J) ≈ CQ2 J 2 + CQ1 J + CQ0 ,
where the coefficients CT ∗ and CQ∗ are unitless coefficients that are deter-
mined experimentally from data. The quadratic approximations are shown
as solid lines in figures 4.1 and 4.2. Combining these equations, the thrust
Forces and Moments 9
0.12
0.1
0.08
CT , data and 2nd-order curve fit
0.06
0.04
0.02
C T = (-0.047394) J2 + (-0.13803) J + (0.11221)

0
-0.02
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7
advance ratio - J (1/rev)
Figure 4.1: Thrust coefficient versus advance ratio for APC Thin Electric 10x5
propeller [?].
and torque produced by the propeller are given by

ρD4 2
Ωp CT 2 J 2 (Ωp , Va ) + CT 1 J(Ωp , Va ) + CT 0

Tp (Ωp , Va ) = 2
4π 4 3
ρD CT 0 2 ρD CT 1 Va
= Ωp + Ωp + (ρD2 CT 2 Va2 )
4π 2 2π
(4.1)
5
ρD 2
Ωp CQ2 J 2 (Ωp , Va ) + CQ1 J(Ωp , Va ) + CQ0

Qp (Ωp , Va ) = 2
4π 5 4
ρD CQ0 2 ρD CQ1 Va
= Ωp + Ωp + (ρD3 CQ2 Va2 ).
4π 2 2π
(4.2)
The speed of the propeller Ωp will be determined by the torque applied to
the propeller by the motor. For a DC motor, the steady-state torque gener-
ated for a given input voltage Vin is given by

1
Qm = KQ (Vin − KV Ωm ) − i0 , (4.3)
R
where KQ is the motor torque constant, R is the resistance of the motor
windings, KV is the back-emf voltage constant, Ωm is the angular speed of
the motor, and i0 is the zero-torque or no-load current. Both KQ and KV
represent how power is transformed between the mechanical and electrical
10 -3
7
CQ, data and 2nd-order curve fit
2 C Q = (-0.015729) J2 + (0.0031409) J + (0.006199)
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7
advance ratio - J (1/rev)
Figure 4.2: Torque coefficient versus advance ratio for APC Thin Electric 10x5
propeller [?].
energy domains. If consistent units (such as SI units) are utilized, KQ and

KT are identical in value.1 It should be noted that when a voltage change
is applied to the motor, the motor changes speed according to the dynamic
behavior of the motor. For a fixed-wing aircraft, these transients are signif-
icantly faster than the aircraft dynamics and we can neglect them without
negatively effecting the accuracy of the model.
When a DC motor drives the propeller we have that Ωp = Ωm and Qm =
Qp . Therefore, setting equation (4.2) equal to equation (4.3) and grouping
like terms gives
!
2
KQ
ρD5 ρD4

2
C Q0 Ωp + CQ1 Va + Ωp
(2π)2 2π R

3 KQ
+ ρD CQ2 Va2 − Vin + KQ i0 = 0. (4.4)
R
1
We assume KQ has units of N-m/A and KV has units of V-sec/rad. In RC aircraft motor
datasheets, it is common for KV to be specified in units of rpm/V.
Denoting the coefficients of this quadratic equation as
ρD5
a= CQ0
(2π)2
KQ2
ρD4
b= CQ1 Va +
2π R
KQ
c = ρD3 CQ2 Va2 − Vin + KQ i0 ,
R
the positive root given by
√
−b + b2 − 4ac
Ωp (Vin , Va ) = (4.5)
2a
defines the operating speed of the propeller as a function of airspeed Va and
motor voltage Vin .
Using the approach above to find the operating speed, the thrust and
torque characteristics of the motor/propeller combination over the full range
of voltage and airspeed inputs can be quantified. A family of curves, one for
each level of voltage input, is plotted versus airspeed and shows the thrust
and torque generated by a specific motor/propeller pair in figures 4.3 and
4.4. Although the voltages are plotted at discrete intervals, the voltage input
is assumed to be continuously variable. Figure 4.3 in particular illustrates
the full range of thrust performance for the selected motor/propeller pair.
As expected, the maximum thrust for a given voltage setting occurs at zero
airspeed and the thrust produced goes down as the airspeed increases.
Note that these plots model the windmilling effect that can occur at ex-
cessively high advance ratios, where both the thrust and torque become neg-
ative and the propeller produces drag instead of thrust. This condition can
occur when an aircraft is gliding and descending in a motor-off state and the
airspeed is being controlled using the pitch angle of the aircraft.
In this book, we will approximate the input voltage as a linear function of
the throttle command, which is roughly true for PWM commands applied to
an RC speed controller. Accordingly,
Vin = Vmax δt , (4.6)
where Vmax is the maximum voltage that can be supplied by the battery.
In summary, the magnitude of the thrust and torque produced by a pro-
peller/motor pair is computed using Algorithm 4.1.
Assuming that the center of the propeller is on the body frame x-axis and
that the direction of the propeller is also aligned with the body frame x-axis,
15
10
thrust (N)
12 V
10 V
5
8V
6V
4V
2V
0
0 5 10 15 20 25 30
airspeed (m/s)
Figure 4.3: Propeller thrust versus airspeed for various motor voltage settings.
0.2
12 V
0.15
10 V
torque (N-m)
0.1 8V
6V
0.05
4V
2V
0
0 5 10 15 20 25 30
airspeed (m/s)
Figure 4.4: Motor torque versus airspeed for various motor voltage settings.
Algorithm 1 Calculate Tp (δt , Va ) and Qp (δt , Va )

Compute Vin using equation (4.6)
Compute Ωp using equation (4.5)
Compute Tp using equation (4.1)
Compute Qp using equation (4.2)
then the force and torque vectors due to the propeller are given by
>
fp = Tp , 0, 0
>
mp = Qp , 0, 0 .
4.4 ATMOSPHERIC DISTURBANCES
MODIFIED MATERIAL: In this section, we will discuss atmospheric dis-

turbances, such as wind, and describe how these disturbances enter into the
dynamics of the aircraft. In chapter 2, we defined Vg as the velocity of the
airframe relative to the ground, Va as the velocity of the airframe relative to
the surrounding air mass, and Vw as the velocity of the air mass relative to
the ground, or in other words, the wind velocity. As shown in equation (??),
the relationship between ground velocity, air velocity, and wind velocity is
given by
Vg = Va + Vw . (4.7)
For simulation purposes, we will assume that the total wind vector can be
represented as
Vw = Vws + Vwg ,
where Vws is a constant vector that represents a steady ambient wind, and
Vwg is a stochastic process that represents wind gusts and other atmospheric
disturbances. The ambient (steady) wind is typically expressed in the iner-
tial frame as
 
wns
i
Vw s
=  wes  ,
wds
where wns is the speed of the steady wind in the north direction, wes is
the speed of the steady wind in the east direction, and wds is the speed of
the steady wind in the down direction. The stochastic (gust) component
of the wind is typically expressed in the aircraft body frame because the
atmospheric effects experienced by the aircraft in the direction of its forward
motion occur at a higher frequency than do those in the lateral and down
directions. The gust portion of the wind can be written in terms of its body-
Table 4.1: Dryden gust model parameters [?].
altitude Lu = Lv Lw σu = σv σw
gust description (m) (m) (m) (m/s) (m/s)
low altitude, light turbulence 50 200 50 1.06 0.7
low altitude, moderate turbulence 50 200 50 2.12 1.4
medium altitude, light turbulence 600 533 533 1.5 1.5
medium altitude, moderate turbulence 600 533 533 3.0 3.0
frame components as
 
uwg
b
Vw g
=  vwg  .
wwg
Experimental results indicate that a good model for the non-steady gust
portion of the wind model is obtained by passing white noise through a
linear time-invariant filter given by the von Karmen turbulence spectrum
in [2]. Unfortunately, the von Karmen spectrum does not result in a rational
transfer function. A suitable approximation of the von Karmen model is
given by the Dryden transfer functions
r
2Va 1
Hu (s) = σu
πLu s + LVa
u

r s + √Va
3Va 3Lv
Hv (s) = σv 2
πLv

s + LVav

r s + √Va
3Va 3Lw
Hw (s) = σw 2 ,
πLw

s + LVwa
where σu , σv , and σw are the intensities of the turbulence along the vehi-
cle frame axes; and Lu , Lv , and Lw are spatial wavelengths; and Va is the
airspeed of the vehicle. The Dryden models are typically implemented as-
suming a constant nominal airspeed Va0 . The parameters for the Dryden
gust model are defined in MIL-F-8785C. Suitable parameters for low and
medium altitudes and light and moderate turbulence were presented in [?]
and are shown in Table 4.1.
Figure 4.5 shows how the steady wind and atmospheric disturbance com-
ponents enter into the equations of motion. White noise is passed
through the Dryden filters to produce the gust components expressed in the
vehicle frame. The steady components of the wind are rotated from the iner-
tial frame into the body frame and added to the gust components to produce
the total wind in the body frame. The combination of steady and gust terms
can be expressed mathematically as
     
uw wn s uwg
b
Vw =  vw  = Rbv (φ, θ, ψ)  wes  +  vwg  ,
ww wds wwg
where Rbv is the rotation matrix from the vehicle to the body frame given
in equation (??). From the components of the wind velocity Vw b and the
Figure 4.5: The wind is modeled as a constant wind field plus turbulence. The
turbulence is generated by filtering white noise with a Dryden model.
ground velocity Vgb , we can calculate the body-frame components of the

airspeed vector as
   
ur u − uw
Vab =  vr  =  v − vw  .
wr w − ww
From the body-frame components of the airspeed vector, we can calculate

the airspeed magnitude, the angle of attack, and the sideslip angle according
to equation (??) as
p
Va = u2r + vr2 + wr2

−1 wr
α = tan
ur
!
vr
β = sin−1 p .
u2r + vr2 + wr2
These expressions for Va , α, and β are used to calculate the aerodynamic

forces and moments acting on the vehicle. The key point to understand is
that wind and atmospheric disturbances affect the airspeed, the angle of at-
tack, and the sideslip angle. It is through these parameters that wind and
atmospheric effects enter the calculation of the aerodynamic forces and mo-
ments and thereby influence the motion of the aircraft.
Note that to implement the system
as + b
Y (s) = U (s),
s2 + cs + d
first put the system into control canonical form

−c −d 1
ẋ = x+ u
1 0 0

y = a b x,
and then convert to discrete time using
xk+1 = xk + Ts (Axk + Buk )

yk = Cxk
where Ts is the sample rate, to get

1 − Ts c −Ts d Ts
xk+1 = xk + uk
Ts 1 0

yk = a b x k .
For the Dryden model gust models, the input uk will be zero mean Gaussian
noise with unity variance.
Python code that implements the transfer function above is given as fol-
lows:
import numpy a s np
class transfer function :

def init ( s e l f , Ts ) :
s e l f . t s = Ts
# set i n i t i a l conditions
s e l f . s t a t e = np . a r r a y ( [ [ 0 . 0 ] , [ 0 . 0 ] ] )
# d e f i n e s t a t e s p a c e model
s e l f . A = np . a r r a y ([[1 − Ts ∗c , −Ts ∗d ] , [ Ts , 1 ] ] )
s e l f . B = np . a r r a y ( [ [ Ts ] , [ 0 ] ] )
s e l f . C = np . a r r a y ( [ [ a , b ] ] )
def update ( s e l f , u ) :
’ ’ ’ Update s t a t e s p a c e model ’ ’ ’
self . state = self . A @ self . state + self . B ∗ u
y = self . C @ self . state
return y
# i n i t i a l i z e the system
Ts = 0 . 0 1 # s i m u l a t i o n s t e p s i z e
s y s t e m = t r a n s f e r f u n c t i o n ( Ts )
# main s i m u l a t i o n l o o p
sim time = 0.0
while sim time < 1 0 . 0 :
u=np . random . r a n d n ( ) # ( w h i t e n o i s e )
y = s y s t e m . u p d a t e ( u ) # u p d a t e b a s e d on c u r r e n t i n p u t
s i m t i m e += Ts # increment the simulation time
4.5 CHAPTER SUMMARY
MODIFIED MATERIAL: The total forces on the MAV can be summarized

as follows:
     
fx −mg sin θ Tp
fy  =  mg cos θ sin φ  +  0 
fz mg cos θ cos φ 0
CX (α) + CXq (α) 2Vc a q
 
1 2 
+ ρVa S CY0 + CYβ β + CYp 2Vb a p + CYr 2Vb a r
2
CZ (α) + CZq (α) 2Vc a q
 
CXδe (α)δe
1
+ ρVa2 S CYδa δa + CYδr δr  , (4.8)
2 C (α)δ Zδe e
where
4
CX (α) = −CD (α) cos α + CL (α) sin α
4
CXq (α) = −CDq cos α + CLq sin α
4
CXδe (α) = −CDδe cos α + CLδe sin α (4.9)
4
CZ (α) = −CD (α) sin α − CL (α) cos α
4
CZq (α) = −CDq sin α − CLq cos α
4
CZδe (α) = −CDδe sin α − CLδe cos α,
and where CL (α) is given by equation (??) and CD (α) is given by equa-
tion (??). The subscripts X and Z denote that the forces act in the X and
Z directions in the body frame, which correspond to the directions of the ib
and the kb vectors.
The total torques on the MAV can be summarized as follows:
 h i 
  b Cl0 + Clβ β + Clp 2Vb a p + Clr 2Vb a r
l
m = 1 ρVa2 S 
 h i 
c Cm0 + Cmα α + Cmq 2Vc a q

2
 
n  h i 
b Cn0 + Cnβ β + Cnp 2Vb a p + Cnr 2Vb a r
   
b Clδa δa + Clδr δr Qp
1
+ ρVa2 S  c Cmδe δe  +  0  . (4.10)
2
0
b Cnδa δa + Cnδr δr
4.6 DESIGN PROJECT
MODIFIED MATERIAL:
4.1 Add simulation of the wind to the mavsim simulator. The wind ele-
ment should produce wind gust along the body axes, and steady state
wind along the NED inertial axes.
4.2 Add forces and moments to the dynamics of the MAV. The inputs to
the MAV should now be elevator, throttle, aileron, and rudder. The
aerodynamic coefficients are given in Appendix E.
4.3 Verify your simulation by setting the control surface deflections to

different values. Observe the response of the MAV. Does it behave as
you think it should?
Chapter Five
Linear Design Models
5.1 SUMMARY OF NONLINEAR EQUATIONS OF MOTION
MODIFIED MATERIAL: A variety of models for the aerodynamic forces

and moments appear in the literature ranging from linear, uncoupled models
to highly nonlinear models with significant cross coupling. In this section,
we summarize the six-degree-of-freedom, 12-state equations of motion with
the quasi-linear aerodynamic and propulsion models developed in chapter 4.
We characterize them as quasi-linear because the lift and drag terms are
nonlinear in the angle of attack, and the propeller thrust is nonlinear in the
throttle command. For completeness, we will also present the linear models
for lift and drag that are commonly used. Incorporating the aerodynamic
and propulsion models described in chapter 4 into equations (??)-(??), we
22 LINEAR DESIGN MODELS
get the following equations of motion:
ṗn = (cos θ cos ψ)u + (sin φ sin θ cos ψ − cos φ sin ψ)v
+ (cos φ sin θ cos ψ + sin φ sin ψ)w (5.1)
ṗe = (cos θ sin ψ)u + (sin φ sin θ sin ψ + cos φ cos ψ)v
+ (cos φ sin θ sin ψ − sin φ cos ψ)w (5.2)
ḣ = u sin θ − v sin φ cos θ − w cos φ cos θ (5.3)
ρV 2 S h cq
u̇ = rv − qw − g sin θ + a CX (α) + CXq (α)
2m 2Va
i T (δ , V )
p t a
+ CXδe (α)δe + (5.4)
m
ρV 2 S h
v̇ = pw − ru + g cos θ sin φ + a CY0 + CYβ β
2m
bp br i
+ C Yp + CYr + CYδa δa + CYδr δr (5.5)
2Va 2Va
ρV 2 S h
ẇ = qu − pv + g cos θ cos φ + a CZ (α)
2mi
cq
+ CZq (α) + CZδe (α)δe (5.6)
2Va
φ̇ = p + q sin φ tan θ + r cos φ tan θ (5.7)
θ̇ = q cos φ − r sin φ (5.8)
ψ̇ = q sin φ sec θ + r cos φ sec θ (5.9)
1 h bp
ṗ = Γ1 pq − Γ2 qr + ρVa2 Sb Cp0 + Cpβ β + Cpp
2 2Va
br i
+ Cp r + Cpδa δa + Cpδr δr (5.10)
2Va
ρV 2 Sc h
q̇ = Γ5 pr − Γ6 (p2 − r2 ) + a Cm 0 + Cm α α
2Jy
cq i
+ Cm q + Cmδe δe (5.11)
2Va
1 h bp
ṙ = Γ7 pq − Γ1 qr + ρVa2 Sb Cr0 + Crβ β + Crp
2 2Va
br i
+ Crr + Crδa δa + Crδr δr , (5.12)
2Va
MODIFIED MATERIAL: Further, it is common to model drag as a nonlin-

Linear Design Models 23
ear quadratic function of the lift as

(CL0 + CLα α)2
CD (α) = CDp + ,
πeos AR
where eos is the Oswald efficiency factor and AR is the aspect ratio of the
wing.
5.2 COORDINATED TURN
MODIFIED MATERIAL: The centrifugal force is calculated using the an-

gular rate χ̇ about the inertial frame ki axis and the horizontal component
of the groundspeed, Vg cos γ.
5.3 TRIM CONDITIONS
MODIFIED MATERIAL: For fixed-wing aircraft, the states are given by

4
x = (pn , pe , pd , u, v, w, φ, θ, ψ, p, q, r)> (5.13)
and the inputs are given by
4
u = (δe , δa , δr , δt )> , (5.14)
5.4 TRANSFER FUNCTION MODELS
5.4.1 Lateral Transfer Functions

5.4.2 Longitudinal Transfer Functions
MODIFIED MATERIAL: To complete the longitudinal models, we derive

the transfer functions from throttle and pitch angle to√airspeed. Toward that
objective, note that if wind speed is zero, then Va = u2 + v 2 + w2 , which
implies that
uu̇ + v v̇ + wẇ
V̇a = .
Va
Using equation (??), we get
V̇a = u̇ cos α cos β + v̇ sin β + ẇ sin α cos β
= u̇ cos α + ẇ sin α + dV1 , (5.15)
where
dV1 = −u̇(1 − cos β) cos α − ẇ(1 − cos β) sin α + v̇ sin β.
Note that when β = 0, we have dV 1 = 0. Substituting equations (5.4)

and (5.6) in equation (5.15), we obtain
(
V̇a = cos α rv − qw + r − g sin θ
ρVa2 S h cq
+ − CD (α) cos α + CL (α) sin α + (−CDq cos α + CLq sin α)
2m 2Va
)
i 1
+ (−CDδe cos α + CLδe sin α)δe + Tp (δt , Va )
m
(
+ sin α qur − pvr + g cos θ cos φ
ρVa2 S h cq
+ − CD (α) sin α − CL (α) cos α + (−CDq sin α − CLq cos α)
2m 2Va
)
i
+ (−CDδe sin α − CLδe cos α)δe + dV1
where Tp (δt , Va ) is the thrust produced by the motor. Using equations (??)
and the linear approximation CD (α) ≈ CD0 + CDα α, and simplifying, we
get
V̇a = rVa cos α sin β − pVa sin α sin β

− g cos α sin θ + g sin α cos θ cos φ
ρVa2 S

cq
+ −CD0 − CDα α − CDq − CDδe δe
2m 2Va
1
+ Tp (δt , Va ) cos α + dV1
m
= (rVa cos α − pVa sin α) sin β
− g sin(θ − α) − g sin α cos θ(1 − cos φ)
ρVa2 S

cq
+ −CD0 − CDα α − CDq − CDδe δe
2m 2Va
1
+ Tp (δt , Va ) cos α + dV1
m
ρVa2 S

cq
= −g sin γ + −CD0 − CDα α − CDq − CDδe δe
2m 2Va
1
+ Tp (δt , Va ) + dV2 , (5.16)
m
where
dV2 = (rVa cos α − pVa sin α) sin β − g sin α cos θ(1 − cos φ)
1
+ Tp (δt , Va )(cos α − 1) + dV1 .
m
Again note that in level flight dV2 ≈ 0.

When considering airspeed Va , there are two inputs of interest: the throt-
tle setting δt and the pitch angle θ. Since equation (5.16) is nonlinear in Va
and δt , we must first linearize before we can find the desired transfer func-
tions. Following the approach outlined in Section 5.5.1, we can linearize
4
equation (5.16) by letting V̄a = Va − Va∗ be the deviation of Va from trim,
4 4
θ̄ = θ − θ∗ be the deviation of θ from trim, and δ̄t = δt − δt∗ be the deviation
of the throttle from trim. Equation (5.16) can then be linearized around the
wings-level, constant-altitude (γ ∗ = 0) trim condition to give
V̄˙ a = −g cos(θ∗ − α∗ )θ̄ (5.17)

∗
ρVa S 1 ∂Tp ∗ ∗
+ −CD0 − CDα α∗ − CDδe δe∗ + (δ , V ) V̄a
m m ∂Va t a
1 ∂Tp ∗ ∗
+ (δ , V )δ̄t + dV
m ∂δt t a
= −aV1 V̄a + aV2 δ̄t − aV3 θ̄ + dV , (5.18)
where
ρVa∗ S 1 ∂Tp ∗ ∗
aV1 = CD0 + CDα α∗ + CDδe δe∗ − (δ , V )
m m ∂Va t a
1 ∂Tp ∗ ∗
aV2 = (δ , V ),
m ∂δt t a
aV3 = g cos(θ∗ − α∗ ),
and dV includes dV2 as well as the linearization error.
5.5 LINEAR STATE-SPACE MODELS
5.5.1 Linearization
5.5.2 Lateral State-Space Equations
MODIFIED MATERIAL:
5.5.3 Longitudinal State-Space Equations
MODIFIED MATERIAL: Assuming that the lateral states are zero (i.e.,
φ = p = r = β = v = 0) and the windspeed is zero and substituting
w
α = tan−1
p u
Va = u + w 2
2
Table 5.1: Lateral State-Space Model Coefficients.

Lateral h Formula i
ρSbv ∗
Yv 4mVa∗ CYp p∗ + CYr r∗
∗
h i
+ ρSv
m CY0 + CYβ β ∗ + CYδa δa∗ + CYδr δr∗
ρSCY √
+ 2m β u∗2 + w∗2
∗
Yp w∗ + ρV4m a Sb
C Yp
∗ ρVa∗ Sb
Yr −u + 4m CYr
ρVa∗2 S
Yδ a 2m CYδa
ρVa∗2 S
Yδr h 2m CYδr i
ρSb2 v ∗
Lv Cpp p∗ + Cpr r∗
4Va∗
h i
+ρSbv ∗ Cp0 + Cpβ β ∗ + Cpδa δa∗ + Cpδr δr∗
ρSbCp √
+ 2 β u∗2 + w∗2
∗ 2
Lp Γ1 q ∗ + ρVa4Sb Cpp
∗ 2
Lr −Γ2 q ∗ + ρVa4Sb Cpr
ρVa∗2 Sb
Lδa 2 Cpδa
ρVa∗2 Sb
Lδr h 2 Cpδr i
ρSb2 v ∗
Nv Crp p∗ + Crr r∗
4Va∗
h i
+ρSbv ∗ Cr0 + Crβ β ∗ + Crδa δa∗ + Crδr δr∗
ρSbCr √
+ 2 β u∗2 + w∗2
∗ 2
Np Γ7 q ∗ + ρVa4Sb Crp
∗ 2
Nr −Γ1 q ∗ + ρVa4Sb Crr
ρVa∗2 Sb
Nδa 2 Crδa
ρVa∗2 Sb
Nδr 2 Crδr
A14 g cos θ∗ cos φ∗
A43 cos φ∗ tan θ∗
A44 q cos φ tan θ∗ − r∗ sin φ∗ tan θ∗
∗ ∗
A53 cos φ∗ sec θ∗

A54 p cos φ sec θ∗ − r∗ sin φ∗ sec θ∗
∗ ∗
from equation (??) gives
ρ(u2 + w2 )S h w i
u̇ = −qw − g sin θ + CX0 + CXα tan−1 + CXδe δe
√ 2m u
ρ u2 + w2 S p
+ CXq cq + Tp (δt , u2 + w2 ) (5.19)
4m
ρ(u2 + w2 )S h w i
ẇ = qu + g cos θ + CZ0 + CZα tan−1 + CZδe δe
√ 2m u
ρ u2 + w2 S
+ CZq cq (5.20)
4m
1 h w i
q̇ = ρ(u2 + w2 )cS Cm0 + Cmα tan−1 + Cmδe δe
2Jy u
1 p 2
+ ρ u + w2 SCmq c2 q (5.21)
4Jy
θ̇ = q (5.22)
ḣ = u sin θ − w cos θ. (5.23)
MODIFIED MATERIAL:
5.5.4 Reduced-order Modes

5.5.4.1 Short-period Mode
5.5.4.2 Phugoid Mode
5.5.4.3 Roll Mode
5.5.4.4 Spiral-divergence Mode
5.5.4.5 Dutch-roll Mode
NEW MATERIAL: Using the fact that for the state-space equations
ẋ = Ax + Bu
y = Cx
the associated transfer function is
Y (s) = C(sI − A)−1 BU (s),

Table 5.2: Longitudinal State-Space Model Coefficients.

Longitudinal Formula
u∗ ρS
CX0 + CXα α∗ + CXδe δe∗

Xu m
ρSw∗ C ρScCXq u∗ q ∗ ∂T
− 2m Xα + 4mVa∗ + ∂up (δt∗ , Va∗ )
∗
−q ∗ + w mρS CX0 + CXα α∗ + CXδe δe∗

Xw
ρScCX w∗ q ∗ ρSCXα u∗ ∂T
+ 4mVq ∗ + 2m + ∂wp (δt∗ , Va∗ )
a
ρV ∗ SCX c
Xq −w∗ + a 4m q
ρVa∗2 SCXδ
Xδe 2m
e
∂Tp ∗ ∗
Xδt ∂δt (δt , Va )
∗
q ∗ + u mρS CZ0 + CZα α∗ + CZδe δe∗

Zu
ρSCZα w∗ u∗ ρSCZ cq ∗
− 2m + 4mV ∗q
a
w∗ ρS
CZ0 + CZα α∗ + CZδe δe∗

Zw m
ρSCZα u∗ ρw∗ ScCZq q ∗
+ 2m + 4mVa∗
∗ ρVa∗ SCZq c
Zq u + 4m
ρVa∗2 SCZδ
Zδ e 2m
e
u∗ ρSc
Cm0 + Cmα α∗ + Cmδe δe∗

Mu Jy
∗ ρSc2 Cmq q ∗ u∗
− ρScC2Jmyα w + 4Jy Va∗
w∗ ρSc
Cm0 + Cmα α + Cmδe δe∗
∗

Mw Jy
∗ ρSc2 Cmq q ∗ w∗
+ ρScC mα u
2Jy + 4Jy Va∗
ρVa∗ Sc2 Cmq
Mq 4Jy
ρVa∗2 ScCmδ
M δe 2Jy
e

and letting C = 0, 1 , the transfer function from δ̄r to r̄ is given by
Yr −1 Yδr
!
Yv Va∗ cos β ∗ Va∗ cos β ∗
r̄(s) = 0 1 sI − δ̄r (s)
Nv Va∗ cos β ∗ Nr Nδr
Nδr s + (Yδr Nv − Nδr Yv )

= δ̄r (s). (5.24)
s2 − (Yv + Nr )s + (Yv Nr − Yr Nv )
5.6 CHAPTER SUMMARY
5.7 DESIGN PROJECT
MODIFIED MATERIAL:
5.1 Create a function that computes the trim state and the trim inputs for
a desired airspeed of Va and a desired flight path angle of ±γ. Set the
initial condition in your simulation to the trim state and trim input,
and verify that the aircraft maintains trim until numerical errors cause
it to drift.
5.2 Create a function that computes the transfer function models
described in this chapter, linearized about the trim state and trim in-
puts.
5.3 Create a function that computes the longitudinal and lateral state
space models described in this chapter, linearized around trim.
5.4 Compute eigenvalues of A lon and notice that one of the eigenvalues
will be zero and that there are two complex conjugate pairs. Using the
formula
(s + λ)(s + λ∗ ) = s2 + 2<λs + |λ|2 = s2 + 2ζωn s + ωn2 ,
extract ωn and ζ from the two complex conjugate pairs of poles. The
pair with the larger ωn correspond to the short-period mode, and
the pair with the smaller ωn correspond to the phugoid mode. The
phugoid and short-period modes can be excited by starting the simu-
lation in a wings-level, constant-altitude trim condition, and placing
an impulse on the elevator. By placing an impulse on the elevator,
convince yourself that the eigenvalues of A lon adequately predict
the short period and phugoid modes.
5.5 Compute eigenvalues of A lat and notice that there is an eigenvalue

at zero, a real eigenvalue in the right half plane, a real eigenvalue in
the left half plane, and a complex conjugate pair. The real eigenvalue
in the right half plane is the spiral-divergence mode, the real eigen-
value in the left half plane is the roll mode, and the complex eigen-
values are the dutch-roll mode. The lateral modes can be excited by
starting the simulation in a wings-level, constant-altitude trim con-
dition, and placing a unit doublet on the aileron or on the rudder.
By simulating the doublet, convince yourself that the eigenvalues of
A lat adequately predict the roll, spiral-divergence, and dutch-roll
modes.
Chapter Six
Autopilot Design
6.1 SUCCESSIVE LOOP CLOSURE
MODIFIED MATERIAL: The primary goal in autopilot design is to con-

trol the inertial position (pn , pe , h) and attitude (φ, θ, χ) of the MAV. For
most flight maneuvers of interest, autopilots designed on the assumption of
decoupled dynamics yield good performance. In the discussion that fol-
lows, we will assume that the longitudinal dynamics (forward speed, pitch-
ing, climbing/descending motions) are decoupled from the lateral dynamics
(rolling, yawing motions). This simplifies the development of the autopilot
significantly and allows us to utilize a technique commonly used for autopi-
lot design called successive loop closure.
The basic idea behind successive loop closure is to close several simple
feedback loops in succession around the open-loop plant dynamics rather
then designing a single (presumably more complicated) control system. To
illustrate how this approach can be applied, consider the open-loop system
shown in figure 6.1. The open-loop dynamics are given by the product of
three transfer functions in series: P (s) = P1 (s)P2 (s)P3 (s). Each of the
transfer functions has an output (y1 , y2 , y3 ) that can be measured and used
for feedback. Typically, each of the transfer functions, P1 (s), P2 (s), P3 (s),
is of relatively low order — usually first or second order. In this case, we are
interested in controlling the output y3 . Instead of closing a single feedback
loop with y3 , we will instead close feedback loops around y1 , y2 , and y3 in
succession, as shown in figure 6.2. We will design the compensators C1 (s),
C2 (s), and C3 (s) in succession. A necessary condition in the design process
is that the inner loop has the highest bandwidth, with each successive loop
bandwidth a factor of 5 to 10 times smaller in frequency.
Figure 6.1: Open-loop transfer function modeled as a cascade of three transfer

functions.
Examining the inner loop shown in figure 6.2, the goal is to design a
closed-loop system from r1 to y1 having a bandwidth ωBW1 . The key as-
34 AUTOPILOT DESIGN
Figure 6.2: Three-stage successive loop closure design.
sumption we make is that for frequencies well below ωBW1 , the closed-loop
transfer function y1 (s)/r1 (s) can be modeled as a gain of 1. This is depicted
schematically in figure 6.3. With the inner-loop transfer function modeled
as a gain of 1, design of the second loop is simplified because it includes
only the plant transfer function P2 (s) and the compensator C2 (s). The crit-
ical step in closing the loops successively is to design the bandwidth of the
next loop so that it is a factor of W smaller than the preceding loop, where
1
S is typically in the range of 5-10. In this case, we require ωBW2 < W ωBW1
thus ensuring that the unity gain assumption on the inner loop is not violated
over the range of frequencies that the middle loop operates.
Figure 6.3: Successive loop closure design with inner loop modeled as a unity
gain.
With the two inner loops operating as designed, y2 (s)/r2 (s) ≈ 1 and the
transfer function from r2 (s) to y2 (s) can be replaced with a gain of 1 for
the design of the outermost loop, as shown in figure 6.4. Again, there is a
bandwidth constraint on the design of the outer loop: ωBW3 < W12 ωBW2 .
Because each of the plant models P1 (s), P2 (s), and P3 (s) is first or second
order, conventional PID or lead-lag compensators can be employed effec-
tively. Transfer-function-based design methods such as root-locus or loop-
shaping approaches are commonly used.
The following sections discuss the design of a lateral autopilot and a lon-
gitudinal autopilot. Transfer functions modeling the lateral and longitudinal
dynamics were developed in Section 5.4 and will be used to design the au-
topilots in this chapter.
Autopilot Design 35
Figure 6.4: Successive-loop-closure design with two inner loops modeled as a

unity gain.
6.1.1 Lateral-directional Autopilot
MODIFIED MATERIAL:
Figure 6.5 shows the block diagram for a lateral autopilot using succes-
sive loop closure. There are five gains associated with the lateral autopilot.
The derivative gain kdφ provides roll rate damping for the innermost loop.
The roll attitude is regulated with the proportional gain kpφ . The course an-
gle is regulated with the proportional and integral gains kpχ and kiχ . And
the dutch-roll mode is effectively damped using the yaw damper with gain
kr . The idea with successive loop closure is that the gains are successively
chosen beginning with the inner loop and working outward. In particular,
kdφ and kpφ are usually selected first, and kpχ and kiχ are usually chosen
second. The gain kr is selected independently of the other gains.
rc = 0 Cr (s2 + 2⇣!n s + !n2 ) <latexit sha1_base64="6OOY3nqChLZrJnTzIcHQr3tHOk4=">AAAB6XicbZBNS8NAEIYn9avWr6pHL4tF8FSSXuyx4MVjFfsBbSib7aRdutmE3Y1QQv+BFw+KePUfefPfuGlz0NYXFh7emWFn3iARXBvX/XZKW9s7u3vl/crB4dHxSfX0rKvjVDHssFjEqh9QjYJL7BhuBPYThTQKBPaC2W1e7z2h0jyWj2aeoB/RieQhZ9RY60FVRtWaW3eXIpvgFVCDQu1R9Ws4jlkaoTRMUK0HnpsYP6PKcCZwURmmGhPKZnSCA4uSRqj9bLnpglxZZ0zCWNknDVm6vycyGmk9jwLbGVEz1eu13PyvNkhN2PQzLpPUoGSrj8JUEBOT/Gwy5gqZEXMLlCludyVsShVlxoaTh+Ctn7wJ3Ubds3zv1lqNIo4yXMAlXIMHN9CCO2hDBxiE8Ayv8ObMnBfn3flYtZacYuYc/sj5/AELoIz2</latexit>

<latexit sha1_base64="7VmVkfJ34GzMAfPk2ppqwcHbw3E=">AAAB6XicbZC7SgNBFIbPeo3rLWppMxgEq7CbRhsxYGMZxVwgWcLsZDYZMju7zJwVwhLwAWwsFLH1IXwPO9/GyaXQxB8GPv7/HOacE6ZSGPS8b2dldW19Y7Ow5W7v7O7tFw8OGybJNON1lshEt0JquBSK11Gg5K1UcxqHkjfD4fUkbz5wbUSi7nGU8iCmfSUiwSha60673WLJK3tTkWXw51C6+nQvHwGg1i1+dXoJy2KukElqTNv3UgxyqlEwycduJzM8pWxI+7xtUdGYmyCfTjomp9bpkSjR9ikkU/d3R05jY0ZxaCtjigOzmE3M/7J2htFFkAuVZsgVm30UZZJgQiZrk57QnKEcWaBMCzsrYQOqKUN7nMkR/MWVl6FRKfuWb71StQIzFeAYTuAMfDiHKtxADerAIIIneIFXZ+g8O2/O+6x0xZn3HMEfOR8/ZSiOsw==</latexit>
sha1_base64="E0THBuggfHWUnJr9vp8B8bg4diM=">AAAB6XicbZDLSgMxFIZP6q2Ot6pLN8EiuCoz3ehGLLhxWcVeoB1KJs20oZnMkGSEMvQN3LhQxG0fwvdwI76NmbYLbf0h8PH/55BzTpAIro3rfqPC2vrG5lZx29nZ3ds/KB0eNXWcKsoaNBaxagdEM8ElaxhuBGsnipEoEKwVjG7yvPXIlOaxfDDjhPkRGUgeckqMte6V0yuV3Yo7E14FbwHl6w/nKpl+OfVe6bPbj2kaMWmoIFp3PDcxfkaU4VSwidNNNUsIHZEB61iUJGLaz2aTTvCZdfo4jJV90uCZ+7sjI5HW4yiwlRExQ72c5eZ/WSc14aWfcZmkhkk6/yhMBTYxztfGfa4YNWJsgVDF7ayYDoki1Njj5EfwlldehWa14lm+c8u1KsxVhBM4hXPw4AJqcAt1aACFEJ7gBV7RCD2jN/Q+Ly2gRc8x/BGa/gBWt5An</latexit>
r
1
<latexit sha1_base64="HVFfYrvNas4GrfXg18Rb57AAalI=">AAAB7nicbZBNS8NAEIYn9avWr6pHL4tF8FSSXvQiFLx4rGA/oI1ls920SzebsDsRSuiP8OJBEa/+Hm/+G7dpDtr6wsLDOzPszBskUhh03W+ntLG5tb1T3q3s7R8cHlWPTzomTjXjbRbLWPcCargUirdRoOS9RHMaBZJ3g+ntot594tqIWD3gLOF+RMdKhIJRtFZXPzJyQ9xhtebW3VxkHbwCalCoNax+DUYxSyOukElqTN9zE/QzqlEwyeeVQWp4QtmUjnnfoqIRN36WrzsnF9YZkTDW9ikkuft7IqORMbMosJ0RxYlZrS3M/2r9FMNrPxMqSZErtvwoTCXBmCxuJyOhOUM5s0CZFnZXwiZUU4Y2oYoNwVs9eR06jbpn+d6tNRtFHGU4g3O4BA+uoAl30II2MJjCM7zCm5M4L86787FsLTnFzCn8kfP5A+Z2jow=</latexit>
sha1_base64="sY/S3B+9UzeqCPkQe8fnoqpU4Zs=">AAAB7nicbZC7SgNBFIbPxltcb1FLm8EgWIXdNNoEAzaWEcwFkhhmJ7PJkNnZZeasEJaAr2BjoYitb+B72Pk2Ti6FJv4w8PH/5zDnnCCRwqDnfTu5tfWNza38truzu7d/UDg8apg41YzXWSxj3Qqo4VIoXkeBkrcSzWkUSN4MRtfTvPnAtRGxusNxwrsRHSgRCkbRWk19z0iFeL1C0St5M5FV8BdQvPp0K48AUOsVvjr9mKURV8gkNabtewl2M6pRMMknbic1PKFsRAe8bVHRiJtuNht3Qs6s0ydhrO1TSGbu746MRsaMo8BWRhSHZjmbmv9l7RTDy24mVJIiV2z+UZhKgjGZ7k76QnOGcmyBMi3srIQNqaYM7YVcewR/eeVVaJRLvuVbr1gtw1x5OIFTOAcfLqAKN1CDOjAYwRO8wKuTOM/Om/M+L805i55j+CPn4wdADZBJ</latexit>
sha1_base64="z/2dypsx4m/Lm2xSi8WWnVAGI8I=">AAAB7nicbZDLSgMxFIbP1Fsdb1WXboJFcFVmurGbYsGNywr2Au1YMmmmDc1kQpIRytCHcONCERdufAPfw434NqaXhbb+EPj4/3PIOSeUnGnjed9Obm19Y3Mrv+3u7O7tHxQOj5o6SRWhDZLwRLVDrClngjYMM5y2paI4DjlthaOrad66p0qzRNyasaRBjAeCRYxgY62WuiOoirxeoeiVvJnQKvgLKF5+uFX59uXWe4XPbj8haUyFIRxr3fE9aYIMK8MIpxO3m2oqMRnhAe1YFDimOshm407QmXX6KEqUfcKgmfu7I8Ox1uM4tJUxNkO9nE3N/7JOaqJKkDEhU0MFmX8UpRyZBE13R32mKDF8bAETxeysiAyxwsTYC7n2CP7yyqvQLJd8yzdesVaGufJwAqdwDj5cQA2uoQ4NIDCCB3iCZ0c6j86L8zovzTmLnmP4I+f9BzGckb0=</latexit>
(s + psp )(s2 + 2⇣dr !ndr s + !n2 dr )

<latexit sha1_base64="nCNoHcQNYdPwoNpMmEigAYg9mcE=">AAAB6XicbZBNT8JAEIan+IX4hXr0spGYeJG0XPRI4sUjGgsk0JDtsoUN222zOzUhDf/AiweN8eo/8ua/cYEeFHyTTZ68M5OdecNUCoOu++2UNja3tnfKu5W9/YPDo+rxSdskmWbcZ4lMdDekhkuhuI8CJe+mmtM4lLwTTm7n9c4T10Yk6hGnKQ9iOlIiEoyitR6uvEG15tbdhcg6eAXUoFBrUP3qDxOWxVwhk9SYnuemGORUo2CSzyr9zPCUsgkd8Z5FRWNugnyx6YxcWGdIokTbp5As3N8TOY2Nmcah7Ywpjs1qbW7+V+tlGN0EuVBphlyx5UdRJgkmZH42GQrNGcqpBcq0sLsSNqaaMrThVGwI3urJ69Bu1D3L926t2SjiKMMZnMMleHANTbiDFvjAIIJneIU3Z+K8OO/Ox7K15BQzp/BHzucP3dSM2A==</latexit>
sha1_base64="7BARNB3AEjf56WHCl1/2ub4je0g=">AAAB6XicbZC7SgNBFIbPxluMt6ilzWAQbAy7abQzYGMZxVwgWcLs5GwyZHZ2mZkVwpI3EMFCEVvfyM7O17Bzcik08YeBj/8/hznnBIng2rjup5NbWV1b38hvFra2d3b3ivsHDR2nimGdxSJWrYBqFFxi3XAjsJUopFEgsBkMryZ58x6V5rG8M6ME/Yj2JQ85o8Zat2det1hyy+5UZBm8OZQuv5PHLwCodYsfnV7M0gilYYJq3fbcxPgZVYYzgeNCJ9WYUDakfWxblDRC7WfTScfkxDo9EsbKPmnI1P3dkdFI61EU2MqImoFezCbmf1k7NeGFn3GZpAYlm30UpoKYmEzWJj2ukBkxskCZ4nZWwgZUUWbscQr2CN7iysvQqJQ9yzduqVqBmfJwBMdwCh6cQxWuoQZ1YBDCAzzDizN0npxX521WmnPmPYfwR877DwkEj+4=</latexit>
sha1_base64="IJU2sXXlCh+hIwHh8Vb0L3ln+Mg=">AAAB6XicbZC7SgNBFIbPeo3xFrW0GQyCjWE3jXYGbCyj5AZJCLOTs8mQ2dllZlYIS95ABQtFbH0Fn8TOxvewc3IpNPGHgY//P4c55/ix4Nq47qeztLyyurae2chubm3v7Ob29ms6ShTDKotEpBo+1Si4xKrhRmAjVkhDX2DdH1yO8/otKs0jWTHDGNsh7UkecEaNtW5OvU4u7xbcicgieDPIX3zHD1+V9/tyJ/fR6kYsCVEaJqjWTc+NTTulynAmcJRtJRpjyga0h02Lkoao2+lk0hE5tk6XBJGyTxoycX93pDTUehj6tjKkpq/ns7H5X9ZMTHDeTrmME4OSTT8KEkFMRMZrky5XyIwYWqBMcTsrYX2qKDP2OFl7BG9+5UWoFQue5Ws3XyrCVBk4hCM4AQ/OoARXUIYqMAjgDp7g2Rk4j86L8zotXXJmPQfwR87bDyNHkYE=</latexit>
<latexit sha1_base64="HfiHpXm+daNHKCHEUixBAvsqjhw=">AAACcnicbVHRTtswFHXCGNDBKEy8bNJmqJCKCijJC3tE4oVHkCggNSVy3JvWwnEi+wZRonwAv8fbvmIv+4A5JRoFdiVLx+f4XNvnxrkUBj3vl+MufFj8uLS80vq0uvZ5vb2xeWmyQnPo80xm+jpmBqRQ0EeBEq5zDSyNJVzFtye1fnUH2ohMXeA0h2HKxkokgjO0VNR+DBPNeHlwEmnaNTdBLwgfAFmYpTBmUakqQ3v0ZXcT7FVl1/TyKES4x9Lk1d6crWFHuvpneaFet5rj66a0FbU73qE3K/oe+A3okKbOovZTOMp4kYJCLpkxA9/LcVgyjYJLqFphYSBn/JaNYWChYimYYTmLrKK7lhnRJNN2KaQzdt5RstSYaRrbkynDiXmr1eT/tEGByc9hKVReICj+fFFSSIoZrfOnI6GBo5xawLgW9q2UT5idAdop1SH4b7/8HlwGh77F517nOGjiWCbfyA7pEp8ckWNySs5In3Dy29lyvjs/nD/uV3fbbbJzncbzhbwqd/8vgyO+FQ==</latexit>
sha1_base64="j5D8D/+EaeWxfLPHBFL9qI42GRY=">AAACcnicbVFNT9tAEF27lEKgkBZxKRIsjSoFBZDtSzkicekRJAJIcbDWm3FYsV5bu+OqqWWJK/+NEzd+BZf+gK6TCMLHSCu9eTNvdvdNnEth0PMeHPfD3Mf5TwuLjaXlzyurzS9fz0xWaA5dnslMX8TMgBQKuihQwkWugaWxhPP4+qiun/8GbUSmTnGUQz9lQyUSwRlaKmreholmvNw7ijRtm8ugE4R/AVmYpTBkUakqQzv0ObsMdqqybTp5FCL8wdLk1c6MbMoOdPUkeaZejprh66G0ETVb3r43DvoW+FPQOgzulm4IIcdR8z4cZLxIQSGXzJie7+XYL5lGwSVUjbAwkDN+zYbQs1CxFEy/HFtW0R+WGdAk0/YopGN2VlGy1JhRGtvOlOGVeV2ryfdqvQKTg34pVF4gKD65KCkkxYzW/tOB0MBRjixgXAv7VsqvmN0B2i3VJvivv/wWnAX7vsUntRtkEgtkg3wnbeKTn+SQ/CLHpEs4eXTWnU1ny/nnfnO33dak1XWmmjXyItzd/+ySv94=</latexit>
sha1_base64="B9iIZqcWVF0RhjclwGCybakLn/g=">AAACcnicbVHBTttAEF07baGhLWlRL63UbhtVCgog2xc4IiGhHqnUAFIcrPVmnKxYr63dcZXU8gfwb5y4IfEPXPgA1klUAnSkld68mTe7+ybOpTDoedeO23jx8tXK6uvm2pu379Zb7z8cm6zQHHo8k5k+jZkBKRT0UKCE01wDS2MJJ/H5QV0/+QPaiEz9xmkOg5SNlEgEZ2ipqHURJprxcvsg0rRjzoJuEP4FZGGWwohFpaoM7dKH7CzYrMqO6eZRiDDB0uTV5pJswQ519U/yQD0etcTXQ2kzarW9HW8W9DnwF6C9H1yuTRqHN0dR6yocZrxIQSGXzJi+7+U4KJlGwSVUzbAwkDN+zkbQt1CxFMygnFlW0R+WGdIk0/YopDN2WVGy1JhpGtvOlOHYPK3V5P9q/QKTvUEpVF4gKD6/KCkkxYzW/tOh0MBRTi1gXAv7VsrHzO4A7ZZqE/ynX34OjoMd3+JftRtkHqvkM/lOOsQnu2Sf/CRHpEc4uXU+Ol+cr86d+8n95rbnra6z0GyQR+Fu3QNohcD7</latexit>
⌧r s
kr
<latexit sha1_base64="U0a30+4YvkzOVSFcqtGZHPhgSIw=">AAAB7HicbZBNS8NAEIYn9avWr6pHL4tF8FSSXvRY8OKxgmkLbSib7aZdutmE3YlQQn+DFw+KePUHefPfuGlz0NYXFh7emWFn3jCVwqDrfjuVre2d3b3qfu3g8Oj4pH561jVJphn3WSIT3Q+p4VIo7qNAyfup5jQOJe+Fs7ui3nvi2ohEPeI85UFMJ0pEglG0lj8b5XoxqjfcprsU2QSvhAaU6ozqX8NxwrKYK2SSGjPw3BSDnGoUTPJFbZgZnlI2oxM+sKhozE2QL5ddkCvrjEmUaPsUkqX7eyKnsTHzOLSdMcWpWa8V5n+1QYbRbZALlWbIFVt9FGWSYEKKy8lYaM5Qzi1QpoXdlbAp1ZShzadmQ/DWT96EbqvpWX5wG+1WGUcVLuASrsGDG2jDPXTABwYCnuEV3hzlvDjvzseqteKUM+fwR87nDxkkjsw=</latexit>
sha1_base64="gDdB5A2KY4anfCNAK37VFc74rgA=">AAAB7HicbZDPSgMxEMZnq9Za/1U9egkWwVPZ7UWPBS8eW3DbQruUbDrbhmazS5IVytJn8OJBEa8+h8/gTXwZ0z8Hbf0g8OObGTLzhang2rjul1PY2t4p7pb2yvsHh0fHlZPTtk4yxdBniUhUN6QaBZfoG24EdlOFNA4FdsLJ7bzeeUCleSLvzTTFIKYjySPOqLGWPxnkajaoVN2auxDZBG8F1Uax9f0BAM1B5bM/TFgWozRMUK17npuaIKfKcCZwVu5nGlPKJnSEPYuSxqiDfLHsjFxaZ0iiRNknDVm4vydyGms9jUPbGVMz1uu1uflfrZeZ6CbIuUwzg5ItP4oyQUxC5peTIVfIjJhaoExxuythY6ooMzafsg3BWz95E9r1mme5ZdOow1IlOIcLuAIPrqEBd9AEHxhweIRneHGk8+S8Om/L1oKzmjmDP3LefwAPEJD+</latexit>
sha1_base64="RGvNkIiSrkKJODxAw8/FonS73aU=">AAAB7HicbZBNSwMxEIZn/ai1flU9egkWwVPZ7UWPBS8eW3DbQltKNp1tQ7PZJckKZelvEEFEEa/6Xzx6E/+M6cdBW18IPLwzQ2beIBFcG9f9ctbWNzZzW/ntws7u3v5B8fCooeNUMfRZLGLVCqhGwSX6hhuBrUQhjQKBzWB0Na03b1FpHssbM06wG9GB5CFn1FjLH/UyNekVS27ZnYmsgreAUjVX//54vH+v9YqfnX7M0gilYYJq3fbcxHQzqgxnAieFTqoxoWxEB9i2KGmEupvNlp2QM+v0SRgr+6QhM/f3REYjrcdRYDsjaoZ6uTY1/6u1UxNedjMuk9SgZPOPwlQQE5Pp5aTPFTIjxhYoU9zuStiQKsqMzadgQ/CWT16FRqXsWa7bNCowVx5O4BTOwYMLqMI11MAHBhzu4AmeHek8OC/O67x1zVnMHMMfOW8/hcGS1g==</latexit>
⌧r s + 1
<latexit sha1_base64="lJlMqHzIwMl/9rWXK/4QEudtzoI=">AAACBnicbZDLSsNAFIZPvNZ6i7oUYbAIglCSbnRZcOOygr1AE8JkOmmHTiZhZiKUkJUbX8WNC0Xc+gzufBunbRBt/WHg4z/ncOb8YcqZ0o7zZa2srq1vbFa2qts7u3v79sFhRyWZJLRNEp7IXogV5UzQtmaa014qKY5DTrvh+Hpa795TqVgi7vQkpX6Mh4JFjGBtrMA+8SKJSe5pnAUSqeKH0AVyi8CuOXVnJrQMbgk1KNUK7E9vkJAspkITjpXqu06q/RxLzQinRdXLFE0xGeMh7RsUOKbKz2dnFOjMOAMUJdI8odHM/T2R41ipSRyazhjrkVqsTc3/av1MR1d+zkSaaSrIfFGUcaQTNM0EDZikRPOJAUwkM39FZIRNLtokVzUhuIsnL0OnUXcN3zq1ZqOMowLHcArn4MIlNOEGWtAGAg/wBC/waj1az9ab9T5vXbHKmSP4I+vjG17gmFE=</latexit>
sha1_base64="nDdTjYfytUOD/ZXe2e1MvdjBApo=">AAACBnicbZDLSsNAFIZP6q3WW9SlCINFEISSFEGXBUFcVrAXaEOYTCft0MkkzEyEErJy42O4deNCEbc+gzvfxukF0dYfBj7+cw5nzh8knCntOF9WYWl5ZXWtuF7a2Nza3rF395oqTiWhDRLzWLYDrChngjY005y2E0lxFHDaCoaX43rrjkrFYnGrRwn1ItwXLGQEa2P59mE3lJhkXY1TXyKV/xA6RW7u22Wn4kyEFsGdQblWuTp7BIC6b392ezFJIyo04Vipjusk2suw1Ixwmpe6qaIJJkPcpx2DAkdUednkjBwdG6eHwliaJzSauL8nMhwpNYoC0xlhPVDztbH5X62T6vDCy5hIUk0FmS4KU450jMaZoB6TlGg+MoCJZOaviAywyUWb5EomBHf+5EVoViuu4RuTRhWmKsIBHMEJuHAONbiGOjSAwD08wQu8Wg/Ws/VmvU9bC9ZsZh/+yPr4BnAImdk=</latexit>
sha1_base64="p7FlzB8RlrkinhcGs17cUmdW4Lw=">AAACBnicbZDLSsNAFIZPvNZ6i7oUYbAIglCSIuiyIoi4qmAv0IQwmU7aoZMLMxOhhKzc+AY+gxsXinTrM7jzbZxeEG39YeDjP+dw5vx+wplUlvVlLCwuLa+sFtaK6xubW9vmzm5DxqkgtE5iHouWjyXlLKJ1xRSnrURQHPqcNv3+5ajevKdCsji6U4OEuiHuRixgBCtteeaBEwhMMkfh1BNI5j+ETpCde2bJKltjoXmwp1Cqlq9On4YXNzXP/HQ6MUlDGinCsZRt20qUm2GhGOE0LzqppAkmfdylbY0RDql0s/EZOTrSTgcFsdAvUmjs/p7IcCjlIPR1Z4hVT87WRuZ/tXaqgnM3Y1GSKhqRyaIg5UjFaJQJ6jBBieIDDZgIpv+KSA/rXJROrqhDsGdPnodGpWxrvtVpVGCiAuzDIRyDDWdQhWuoQR0IPMAzvMKb8Wi8GO/GcNK6YExn9uCPjI9vEqebEg==</latexit>
Figure 6.5: Autopilot for lateral control using successive loop closure.
The following sections describe the design of the lateral autopilot using
successive loop closure.
36 AUTOPILOT DESIGN
6.1.1.1 Roll Hold using the Aileron
The inner loop of the lateral autopilot is used to control roll angle and roll
rate, as shown in figure 6.6.
MODIFIED MATERIAL:
If the transfer function coefficients aφ1 and aφ2 are known, then there is
a systematic method for selecting the control gains kdφ and kpφ based on
the desired response of closed-loop dynamics. From figure 6.6, the transfer
Figure 6.6: Roll attitude hold control loops.
function from φc to φ is given by

kpφ aφ2
Hφ/φc (s) = .
s2 + (aφ1 + aφ2 kdφ )s + kpφ aφ2
Note that the DC gain is equal to one. If the desired response is given by the
canonical second-order transfer function
d
ωn2 φ
Hφ/φc (s) = ,
s2 + 2ζφ ωnφ s + ωn2 φ
then equating denominator polynomial coefficients, we get
ωn2 φ = kpφ aφ2 (6.1)
2ζφ ωnφ = aφ1 + aφ2 kdφ . (6.2)
Accordingly, the proportional and derivative gains are given by
ωn2 φ
kpφ =
aφ2
2ζφ ωnφ − aφ1
kdφ = .
aφ2
where the natural frequency ωnφ and the damping ratio ζφ is a design pa-
rameter.
The output of the roll attitude hold loop is
δa (t) = kpφ (φc (t) − φ(t)) − kdφ p(t).
Autopilot Design 37
6.1.1.2 Course Hold using Commanded Roll
MODIFIED MATERIAL:
The next step in the successive-loop-closure design of the lateral autopilot
is to design the course-hold outer loop. If the inner loop from φc to φ has
been adequately tuned, then Hφ/φc ≈ 1 over the range of frequencies from
0 to ωnφ . Under this condition, the block diagram of figure 6.5 can be
simplified to the block diagram in figure 6.7 for the purposes of designing
the outer loop.
Figure 6.7: Course hold outer feedback loop.
The objective of the course hold design is to select kpχ and kiχ in fig-
ure 6.5 so that the course χ asymptotically tracks steps in the commanded
course χc . From the simplified block diagram, the transfer functions from
the inputs χc and dχ to the output χ are given by
g/Vg s kp g/Vg s + kiχ g/Vg

χ= dχ + 2 χ χc . (6.3)
s2 + kpχ g/Vg s + kiχ g/Vg s + kpχ g/Vg s + kiχ g/Vg
Note that if dχ and χc are constants, then the final value theorem implies
that χ → χc . The transfer function from χc to χ has the form
2ζχ ωnχ s + ωn2 χ

Hχ = . (6.4)
s2 + 2ζχ ωnχ s + ωn2 χ
As with the inner feedback loops, we can choose the natural frequency and
damping of the outer loop and from those values calculate the feedback
gains kpχ and kiχ . Figure 6.8 shows the frequency response and the step
response for Hχ . Note that because of the numerator zero, the standard
intuition for the selection of ζ does not hold for this transfer function. Larger
ζ results in larger bandwidth and smaller overshoot.
Comparing coefficients in equations (6.3) and (6.4), we find
ωn2 χ = g/Vg kiχ

2ζχ ωnχ = g/Vg kpχ .
38 AUTOPILOT DESIGN
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)
Figure 6.8: Frequency and step response for a second-order system with a transfer
function zero for ζ = 0.5, 0.7, 1, 2, 3, 5.
Solving these expressions for kpχ and kiχ , we get

kpχ = 2ζχ ωnχ Vg /g (6.5)
k iχ = ωn2 χ Vg /g. (6.6)
To ensure proper function of this successive-loop-closure design, it is es-
sential that there be sufficient bandwidth separation between the inner and
outer feedback loops. Adequate separation can be achieved by letting
1
ωnχ = ωn ,
Wχ φ
where the separation Wχ is a design parameter that is usually chosen to
be greater than five. Generally, more bandwidth separation is better. More
bandwidth separation requires either slower response in the χ loop (lower
ωnχ ), or faster response in the φ loop (higher ωnφ ). Faster response usually
comes at the cost of requiring more actuator control authority, which may
not be possible given the physical constraints of the actuators.
The output of the course hold loop is
Z t
c c
φ (t) = kpχ (χ (t) − χ(t)) + kiχ (χc (τ ) − χ(τ ))dτ.
−∞
6.1.1.3 Yaw Damper using the Rudder
NEW MATERIAL: As discussed in the previous section, the aileron is used

to control the roll angle, and subsequently the course angle. In deriving the
transfer function from the roll angle to the course angle we assumed that
the side-slip angle is zero. While the rudder could be used to regulate the
Autopilot Design 39
side-slip angle, this is impractical for small aircraft since the side-slip angle
is not measured directly. An alternative is to use the rudder to regulate the
yaw rate to zero. The difficulty with doing so, however, is that in a turn we
do not desire the yaw rate to be zero and so a direct control implementation
would create competing objectives between the course controller and the
yaw rate controller. The solution to this dilemma is to employ a so-called
washout filter to only allow feedback from yaw rate to the rudder for high
frequency yaw rate. A wash-out filter is simply a high-pass filter.
Recall from Equation (5.24) that the transfer function from the rudder to
the yaw rate is given by
Nδr s + (Yδr Nv − Nδr Yv )

r(s) = δr (s). (6.7)
s2 − (Yv + Nr )s + (Yv Nr − Yr Nv )
The yaw-damper control loop is shown in Figure 6.9, where pwo is the pole
of the washout-filter, and kr > 0 is the control gain from yaw-rate to rudder.
The effect of the washout filter is to remove the feedback signal over the
Figure 6.9: Yaw damper with washout filter.
frequency band below pwo and to pass with unity gain the feedback signal
for frequencies above pwo . The negative sign in the top path is due to the
fact that negative δr results in a postive yaw rate (i.e., Nδr < 0).
The selection of pwo and kr can be found from physical considerations.
As discussed in Chapter 5, the poles of the transfer function from δr to r
shown in Equation 6.7 correspond to the lightly-damped dutch roll mode.
Essentially, the objective of the yaw damper is damp out the dutch-roll
mode. Therefore, the pole of the washout filter should be selected to be be-
low the natural frequency of the dutch-roll mode. The canonical expression
s2 + 2ζdr ωndr s + ωn2 dr = s2 − (Yv + Nr )s + (Yv Nr − Yr Nv ) approximates
the natural frequency of the dutch-roll mode as
p
ωndr ≈ Yv Nr − Yr Nv .
A rule-of-thumb is to select pwo = ωndr /10.

40 AUTOPILOT DESIGN
For frequencies over pwo , when the washout filter is active, the closed-
loop transfer function from rc to r is given by
−(Nδr s + (Yδr Nv − Nδr Yv ))

r(s) = rc (s),
s2 + (−Yv − Nr − kr Nδr )s + (Yv Nr − Yr Nv − kr (Yδr Nv − Nδr Yv ))
where the closed-loop poles are given by

Yv + Nr + kr Nδr
pyaw-damper =
2
s 2
Yv + Nr + kr Nδr
±j (Yv Nr − Yr Nv − kr (Yδr − Nv Nδr Yv ) − .
2
A reasonable approach is to select kr so that the damping ratio of the closed-

loop system is ζ = 0.707. This occurs when the real and imaginary parts of
closed-loop poles have the same magnitude, i.e., when
2
Yv + Nr + kr Nδr
2
2
Yv + Nr + kr Nδr
= (Yv Nr − Yr Nv − kr (Yδr − Nv Nδr Yv ) − .
2
Simplifying, we get that kr is the positive root of the quadratic equation
Nδ2r kr2 + 2(Nr Nδr + Yδr Nv )kr + (Yv2 + Nr2 + 2Yr Nv ) = 0,
resulting in
Nr Nδr + Yδr Nv
kr = −
Nδ2r
v
u N N +Y N 2
u ! !
r δr δr v Yv2 + Nr2 + 2Yr Nv
+ t − .
Nδ2r Nδ2r
For the Aerosonde model given in the appendix, we get pwo = 0.45 and
kr = 0.196.
Figure 6.10 shows the lateral-directional performance of the Aerosonde
aircraft in response to a doublet roll command, both without the yaw damper
and with the yaw damper. The upper plot shows the roll response of the
aircraft to the doublet roll command. Without the yaw damper, the lightly
damped dutch-roll dynamics couple into the closed-loop roll dynamics caus-
ing the roll dynamics to be less-damped than anticipated. The lower plot
shows the yaw (heading) angle of the aircraft with and without the yaw
damper. The improved response resulting from the yaw damper is clear.
This is particularly true for the Aerosonde aircraft model used, which has
Autopilot Design 41
an influential non-minimum-phase zero in the transfer function from aileron

deflection to yaw angle. This can be seen at time t = 1 s where the pos-
itive roll response (caused by a positive aileron deflection) causes the air-
craft to yaw negatively initially. This is commonly referred to as adverse
yaw and is caused by the negative yawing moment produced by the posi-
tive aileron deflection as modeled by the Nδr aerodynamic coefficient. This
non-minimum phase zero further aggravates the negative effect of the lightly
damped dutch-roll mode, making the implementation of the yaw damper a
necessity for well-behaved flight.
30
desired
w/o yaw damper
20 with yaw damper
roll angle (deg)
10
-10
-20
-30
0 1 2 3 4 5 6 7 8 9 10
time (s)
40
w/o yaw damper
with yaw damper
30
yaw angle (deg)
20
10
-10
0 1 2 3 4 5 6 7 8 9 10
time (s)
Figure 6.10: Turning performance with and without yaw damper.
A final comment on the effect of the dutch-roll mode on the design of the
lateral-directional control of the aircraft is that it may be necessary to take
the speed of the dutch-roll mode into consideration when using successive-
loop closure to set the bandwidth of the outer-loop course control. Rather
than keeping the bandwidth of the outer course loop at least a factor of ten
below the bandwidth of the roll loop. Better performance may result from
setting the outer course-loop bandwidth at least a factor of ten slower than
the frequency of the dutch-roll mode.
42 AUTOPILOT DESIGN
6.1.1.4 Yaw Damper Implementation
NEW MATERIAL:
The yaw damper in the Laplace domain is given by

s
δr (s) = kr r(s).
s + pwo
Converting to the time domain results in the differential equation
δ̇r = −pwo δr + kr ṙ.
Integrating once gives
Z t
δr (t) = −pwo δr (σ)dσ + kr r(t). (6.8)
−∞
A block diagram that corresponds to equation (6.8) is shown in figure 6.11.
Figure 6.11: Block diagram for yaw damper transfer function.
If the output of the integrator is ξ, then the block diagram corresponds to

the state-space equations
ξ˙ = −pwo ξ + kr r
δr = −pwo ξ + kr r.
Using a Euler approximation for the derivative, the discrete-time equations
are
ξk = −pwo ξk−1 + kr rk−1
δrk = −pwo ξk + kr rk .
The corresponding Python code is shown below.
c l a s s yawDamper :
def init ( s e l f , k r , p wo , Ts ) :
s e l f . xi = 0.
s e l f . Ts = Ts
self . k r = k r
s e l f . p wo = p wo
Autopilot Design 43
s e l f . xi = s e l f . xi
+ s e l f . Ts ∗ (− s e l f . p wo ∗ s e l f . x i
+ self . k r ∗ r )
d e l t a r = −p wo ∗ s e l f . x i + s e l f . k r ∗ r
return d e l t a r
6.1.2 Longitudinal Autopilot
MODIFIED MATERIAL: Figure 6.12 shows the block diagram for the lon-
gitudinal autopilot using successive loop closure. There are six gains associ-
ated with the longitudinal autopilot. The derivative gain kdθ provides pitch
rate damping for the innermost loop. The pitch attitude is regulated with the
proportional gain kpθ . The altitude is regulated with the proportional and
integral gains kph and kih . The airpseed is regulated using the proportional
and integral gains kpVa and kiVa . The idea with successive loop closure is
that the gains are successively chosen beginning with the inner loop and
working outward. In particular, kdθ and kpθ are usually selected first, and
kph and kih are usually chosen second. The gains kpVa and kiVa are selected
independently of the other gains.
kiV
s
V̄ac ¯t V̄a
kpV
Figure 6.12: Autopilot for longitudinal control using successive loop closure.
The following sections describe the design of the longitudinal autopilot

using successive loop closure.
6.1.2.1 Pitch Hold using the Elevator
MODIFIED MATERIAL: The pitch attitude hold loop is similar to the

roll attitude hold loop, and we will follow a similar line of reasoning in its
44 AUTOPILOT DESIGN
development. From figure 6.13, the transfer function from θc to θ is given

by
kpθ aθ3
Hθ/θc (s) = . (6.9)
s2 + (aθ1 + kdθ aθ3 )s + (aθ2 + kpθ aθ3 )
Note that in this case, the DC gain is not equal to one.
Figure 6.13: Pitch attitude hold feedback loops.
If the desired response is given by the canonical second-order transfer

function
d KθDC ωn2 θ
Hθ/θ c = ,
s2 + 2ζθ ωnθ s + ωn2 θ
then, equating denominator coefficients, we get
ωn2 θ = aθ2 + kpθ aθ3 (6.10)

2ζθ ωnθ = aθ1 + kdθ aθ3 (6.11)
KθDC ωn2 θ = kpθ aθ3 . (6.12)
Solving for kpθ , kdθ , and KθDC we get
ωn2 θ − aθ2
kpθ =
aθ3
2ζ ωn − aθ1
kdθ = θ θ
aθ3
kp aθ
KθDC = θ2 3 ,
ωnθ
where ωnθ and ζθ are design parameters.
An integral feedback term could be employed to ensure unity DC gain on
the inner loop. The addition of an integral term, however, can severely limit
the bandwidth of the inner loop, and therefore is not used. Note however,
that in the design project, the actual pitch angle will not converge to the com-
manded pitch angle. This fact will be taken into account in the development
of the altitude hold loop.
Autopilot Design 45
The output of the pitch loop is therefore
δe (t) = kpθ (θc (t) − θ(t)) − kdθ q(t).
6.1.2.2 Altitude Hold using Commanded Pitch
MODIFIED MATERIAL: The altitude-hold autopilot utilizes a successive-

loop-closure strategy with the pitch attitude hold autopilot as an inner loop,
as shown in figure 6.12. Assuming that the pitch loop functions as designed
and that θ ≈ KθDC θc , the altitude hold loop using the commanded pitch can
be approximated by the block diagram shown in figure 6.14.
Figure 6.14: The altitude hold loop using the commanded pitch angle.
In the Laplace domain, we have

 kih

KθDC Va kph s + kph
h(s) =   hd (s)
s2 + KθDC Va kph s + KθDC Va kih

s
+ dh (s),
s2 + KθDC Va kph s + KθDC Va kih
where again we see that the DC gain is equal to one, and constant distur-
bances are rejected. The closed-loop transfer function is again independent
of aircraft parameters and is dependent only on the known airspeed. The
gains kph and kih should be chosen such that the bandwidth of the altitude-
from-pitch loop is less than the bandwidth of the pitch-attitude-hold loop.
Similar to the course loop, let
1
ωnh = ωn ,
Wh θ
where the bandwidth separation Wh is a design parameter that is usually
between five and fifteen. If the desired response of the altitude hold loop is
46 AUTOPILOT DESIGN
given by the canonical second-order transfer function
d ωn2 h
Hh/hc = ,
s2 + 2ζh ωnh s + ωn2 h
then, equating denominator coefficients, we get
ωn2 h = KθDC Va kih
2ζh ωnh = KθDC Va kph .
Solving these expressions for kih and kph , we get
ωn2 h
kih = (6.13)
KθDC Va
2ζ ωn
kph = h h. (6.14)
KθDC Va
Therefore, selecting the desired damping ratio ζh and the bandwidth sepa-
ration Wh fixes the value for kph and kih .
The commanded pitch angle is therefore
Z t
θc (t) = kph (hc (t) − h(t)) + kih (hc (τ ) − h(τ ))dτ.
−∞
6.1.2.3 Airspeed Hold using Throttle
MODIFIED MATERIAL: Using PI control for the airspeed loop results

in the closed-loop system is shown in figure 6.15. If we use proportional
kiV
s
V̄ac ¯t V̄a
kpV
Figure 6.15: Airspeed hold using throttle.
control, then

aV2 kpV 1
V̄a (s) = V̄ac (s) + dV (s).
s + (aV1 + aV2 kpV ) s + (aV1 + aV2 kpV )
Autopilot Design 47
Note that the DC gain is not equal to one and that step disturbances are not
rejected. If, on the other hand, we use proportional-integral control, then

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

1
+ dV .
s2 + (aV1 + aV2 kpV )s + aV2 kiV
It is clear that using a PI controller results in a DC gain of one, with step dis-
turbance rejection. If aV1 and aV2 are known, then the gains kpV and kiV can
be determined using the same technique we have used previously. Equating
the closed-loop transfer function denominator coefficients with those of a
canonical second-order transfer function, we get
ωn2 V = aV2 kiV
2ζV ωnV = aV1 + aV2 kpV .
Inverting these expressions gives the control gains
ωn2 V
kiV = (6.15)
aV2
2ζV ωnV − aV1
kpV = . (6.16)
aV2
The design parameters for this loop are the damping coefficient ζV and the
natural frequency ωnV .
Note that since V̄ac = Vac − Va∗ and V̄a = Va − Va∗ , the error signal in
figure 6.15 is
e = V̄ac − V̄a = Vac − Va .
Therefore, the control loop shown in figure 6.15 can be implemented with-
out knowledge of the trim velocity Va∗ . If the throttle trim value δt∗ is known,
then the throttle command is
δt = δt∗ + δ̄t .
However, if δt∗ is not precisely known, then the error in δt∗ can be thought
of as a step disturbance, and the integrator will wind up to reject the distur-
bances.
The throttle command is therefore
Z t
δt (t) = kpV (Vac (t) − Va (t)) + kiV (Vac (τ ) − Va (τ ))dτ.
−∞
48 AUTOPILOT DESIGN
6.1.3 Digital Implementation of PID Loops
NEW MATERIAL:
A Python class that implements a general PID loop is shown below.
import numpy a s np
class pid control :

def init ( s e l f , kp = 0 . 0 , k i = 0 . 0 , kd = 0 . 0 , Ts = 0 . 0 1 ,
sigma =0.05 , l i m i t = 1 . 0 ) :
s e l f . kp = kp
s e l f . ki = ki
s e l f . kd = kd
s e l f . Ts = Ts
self . limit = limit
s e l f . i n t e g r a t o r = 0.0
s e l f . error delay 1 = 0.0
s e l f . e r r o r d o t d e l a y 1 = 0.0
# gains for d i f f e r e n t i a t o r
s e l f . a1 = ( 2 . 0 ∗ s i g m a − Ts ) / ( 2 . 0 ∗ s i g m a + Ts )
s e l f . a2 = 2 . 0 / ( 2 . 0 ∗ s i g m a + Ts )
def update ( s e l f , y ref , y , r e s e t f l a g = False ) :

i f r e s e t f l a g == T r u e :
s e l f . y dot = 0.0
s e l f . y delay 1 = 0.0
s e l f . y dot delay 1 = 0.0
# compute t h e e r r o r
error = y ref − y
# update the i n t e g r a t o r using t r a p a z o i d a l rule
self . integrator = self . integrator \
+ ( s e l f . Ts / 2 ) ∗ ( e r r o r + s e l f . e r r o r d e l a y 1 )
# update the d i f f e r e n t i a t o r
e r r o r d o t = s e l f . a1 ∗ s e l f . e r r o r d o t d e l a y 1 \
+ s e l f . a2 ∗ ( e r r o r − s e l f . e r r o r d e l a y 1 )
# PID c o n t r o l
u = s e l f . kp ∗ e r r o r \
+ s e l f . ki ∗ s e l f . i n t e g r a t o r \
+ s e l f . kd ∗ e r r o r d o t
# s a t u r a t e PID c o n t r o l a t l i m i t
u sat = self . saturate (u)
# i n t e g r a l a n t i −windup
# a d j u s t i n t e g r a t o r to keep u out of s a t u r a t i o n
i f np . abs ( s e l f . k i ) > 0 . 0 0 0 1 :
+ (1.0 / s e l f . ki ) ∗ ( u sat − u )
# update the delayed v a r i a b l e s
self . error delay 1 = error
Autopilot Design 49
self . error dot delay 1 = error dot

return u s a t
def u p d a t e w i t h r a t e ( s e l f , y r e f , y , ydot ,
r e s e t f l a g =False ) :
i f r e s e t f l a g == T r u e :
# compute t h e e r r o r
error = y ref − y
# update the i n t e g r a t o r using t r a p a z o i d a l rule
+ ( s e l f . Ts / 2 ) ∗ ( e r r o r + s e l f . e r r o r d e l a y 1 )
# PID c o n t r o l
u = s e l f . kp ∗ e r r o r \
+ s e l f . ki ∗ s e l f . i n t e g r a t o r \
− s e l f . kd ∗ y d o t
# s a t u r a t e PID c o n t r o l a t l i m i t
u sat = self . saturate (u)
# i n t e g r a l a n t i −windup
# a d j u s t i n t e g r a t o r to keep u out of s a t u r a t i o n
i f np . abs ( s e l f . k i ) > 0 . 0 0 0 1 :
+ (1.0 / s e l f . ki ) ∗ ( u sat − u )
self . error delay 1 = error
return u s a t
def saturate ( self , u ):

# s a t u r a t e u a t +− s e l f . l i m i t
i f u >= s e l f . l i m i t :
u sat = self . limit
e l i f u <= − s e l f . l i m i t :
u s a t = −s e l f . l i m i t
else :
u sat = u
return u s a t
STOP NEW MATERIAL:

NEW MATERIAL:
6.2 TOTAL ENERGY CONTROL
One of the disadvantages of the longitudinal autopilot discussed in the book

is the number of required loops and the state machine for altitude control
shown in page 113. In this note we will describe a simplier scheme based on
the energy states of the system. The ideas in this supplement are motivated
by [?, ?].
50 AUTOPILOT DESIGN
The longitudinal control system is complicated by the fact that both alti-
tude and airspeed need to be regulated, but that these quantities are strongly
coupled. If we assume a low level autopilot on the pitch angle, then the pri-
mary control signals are the throttle and the commanded pitch angle. Both
of these quantities have a significant effect on both altitude and airspeed.
Rather than attempt to decouple these effects by operating different loops in
different flight regimes, the total energy control method changes the regu-
lated outputs from altitude and airspeed to total energy and energy balance,
which produces a natural decoupling in the longitudinal motion.
The kinetic energy of a body in motion is given by K = 12 m kvk2 . If we
use the velocity of the aircraft relative to the air mass, then K = 21 mVa2 .
The reference kinetic energy is given by Kref = 12 m(Vac )2 . Therefore, the
error in kinetic energy is given by
4 1
Kerror = Kref − K = m((Vac )2 − Va2 ).
2
The potential energy of a body with mass m is given by U = U0 + mgh
where U0 is the potential of ground level when the altitude h = 0. The
reference potential energy is given by Uref = U0 + mghc . Therefore, the
error in potential energy is given by
Uerror = mg(hc − h). (6.17)
The total energy (error) is given by
E = Uerror + Kerror .
The energy (error) balance is given by
B = Uerror − Kerror .
As mentioned previously, the throttle is used to control the total energy using
a PI controller:
Z t
δt (t) = kpE E(t) + kiE E(τ ) dτ.
−∞
The pitch command is used to regulate the energy balance using a PI con-
troller:
Z t
c
θ (t) = kpB B(t) + kiB B(τ ) dτ.
−∞
As a matter of practical consideration, the TECS scheme works better for

large deviations in altitude if the altitude error in (6.17) is saturated as
Uerror = mg sath̄e (hc − h),
Autopilot Design 51
where h̄e > 0 is the largest altitude error used to compute Uerror , and sat is
the saturation function.
NEW MATERIAL:
6.3 LQR CONTROL
Given the state space equation
ẋ = Ax + Bu
and the symmetric positive semi-definite matrix Q, and the symmetric pos-
itive definite matrix R, the LQR problem is to minimize the cost index
Z ∞
J(x0 ) = min x> (τ )Qx(τ ) + u> (τ )Ru(τ )dτ.
u(t),t≥0 0
If (A, B) is controllable, and (A, Q1/2 ) is observable, then a unique optimal

control exists and is given in linear feedback form as
ulqr (t) = −Klqr x(t),
where the LQR gain is given by
Klqr = R−1 B > P,
and where P is the symmetric positive definite solution of the Algebraic

Riccati Equation
P A + A> P + Q − P BR−1 B > P = 0.
Note that Klqr are the optimal feedback gains given Q and R. The con-
troller is tuned by changing Q and R. Typically we choose Q and R to be
diagonal matrices
   
q1 0 . . . 0 r1 0 . . . 0
 0 q2 . . . 0   0 r2 . . . 0 
Q =  .. R =  .. ..  ,
   
.. ..  ..
. . .   . . . 
0 0 ... qn 0 0 ... rm
where n is the number of states, m is the number of inputs, and qi ≥ 0

ensures Q is positive semi-definite, and ri > 0 ensure R is positive definite.
For the lateral and longitudinal autopilots, we have seen that we need
integral control for course, altitude, and airspeed. Given the state space
52 AUTOPILOT DESIGN
system
ẋ = Ax + Bu
z = Hx
where z represents the controlled output. Suppose that the objective is to

drive z to a reference signal z c and further suppose that z c is a step, i.e.,
ż c = 0. The first step is to augment the state with the integrator
Z t
xI = (z(τ ) − z c ) dτ.
−∞
Defining the augmented state as ξ = (x> , x> >

I ) , results in the augmented
state space equations
ξ˙ = Āξ + B̄u,
where

A 0 B
Ā = B̄ = .
H 0 0
The LQR design process then proceeds as normal.
6.3.1 Lateral Autopilot using LQR
As derived in chapter 5, the state space equations for the lateral equation of
motion are given by
ẋlat = Alat xlat + Blat ulat ,
where xlat = (v, p, r, φ, ψ)> and ulat = (δa , δr )> . The objective of the
lateral autopilot is to drive the course χ to the commanded course χc . There-
fore, we augment the state with
Z
xI = (χ − χc )dt.
Since χ ≈ ψ, we approximate xI as
Z
xI = (Hlat xlat − χc )dt,
where Hlat = (0, 0, 0, 0, 1).

The augmented lateral state equations are therefore
ξ˙lat = Ālat ξlat + B̄lat ulat ,

Autopilot Design 53
where

Alat 0 Blat
Ālat = B̄lat =
Hlat 0 0
The LQR controller is designed using
Q = diag ([qv , qp , qr , qφ , qχ , qI ])
R = diag ([rδa , rδr ]) .
6.3.2 Longitudinal Autopilot using LQR
As derived in chapter 5, the state space equations for the longitudinal equa-
tions of motion are given by
ẋlon = Alon xlon + Blon ulon ,
where xlon = (u, w, q, θ, h)> and ulat = (δe , δt )> . The objective of the
longitudinal autopilot is to drive the altitude h to the commanded altitude
hc , and the airspeed Va to commanded airspeed Vac . Therefore, we augment
the state with
c )dt
R
(h − h
xI = R
(Va − Vac )dt
Z c
h
= Hlon xlon − dt,
Vac
where

0 0 0 0 1
Hlon = u∗ w∗ .
Va Va 0 0 0
The augmented longitudinal state equations are therefore
ξ˙lon = Ālon ξlon + B̄lon ulon ,
where

Alon 0 Blon
Ālon = B̄lon =
Hlon 0 0
The LQR controller is designed using
Q = diag ([qu , qw , qq , qθ , qh , qI ])
R = diag ([rδe , rδt ]) .
54 AUTOPILOT DESIGN
6.4 CHAPTER SUMMARY

Autopilot Design 55
6.5 DESIGN PROJECT
The objective of this assignment is to implement the lateral and longitudinal

autopilots, as described in this chapter. You can use either successive loop
closure, total energy control, or LQR.
6.1 Implement the longitudinal autopilot and tune the gains. The input the
longitudinal autopilot is the commanded airspeed and the commanded
altitude. To make the process easier, you may want to initialize the
system to trim inputs and then tune airspeed control first, followed by
the pitch loop, and then the altitude loop.
6.2 Implement the lateral autopilot. The input to the lateral autopilot is
the commanded course angle.
6.3 Test the autopilot using a variety of different step inputs on the com-
manded airspeed, altitude, and course angle. Be sure to command
course angles that are greater than ±180 degrees.
Chapter Seven
Sensors for MAVs
7.1 ACCELEROMETERS
MODIFIED MATERIAL: As an alternative, we can substitute from equa-

tions (5.4), (5.5), and (5.6) to obtain
ρVa2 S

c̄q
yaccel,x = CX (α) + CXq (α) + CXδe (α)δe
2m 2Va
+ Tp (δt , Va ) + ηaccel,x
ρV S2 h bp br
yaccel,y = a CY0 + CYβ β + CYp + C Yr
2m 2Va 2Va
i
+ CYδa δa + CYδr δr + ηaccel,y (7.1)
ρV 2 S

c̄q
yaccel,z = a CZ (α) + CZq (α) + CZδe (α)δe + ηaccel,z .
2m 2Va
7.2 RATE GYROS
7.3 PRESSURE SENSORS
7.3.1 Altitude Measurement

7.3.2 Airspeed Sensor
7.4 DIGITAL COMPASSES
7.5 GLOBAL POSITIONING SYSTEM
MODIFIED MATERIAL:
Because of clock synchronization errors between the satellites (whose
atomic clocks are almost perfectly synchronized) and the receiver, four in-
dependent pseudo-range measurements are required to trilaterate the posi-
tion of the receiver as depicted in figure 7.1. Why are four pseudo-range
58 SENSORS FOR MAVS
measurements required? One range measurement narrows the receiver po-

sition to a sphere around the satellite. A second measurement from another
satellite narrows the possible positions to the the intersection of the the two
spheres around the satellites, which is a circle. A third measurement nar-
rows the possible positions to the intersection of the three spheres around
the satellites, which is two points. The point closest to the surface of the
earth is taken as the position. To resolve position in three dimensions with
a receiver clock offset error, at least four measurements are needed. The ge-
ometry associated with the pseudo-range measurements from four different
satellites form a system of four nonlinear algebraic equations in four un-
knowns: latitude, longitude, and altitude of the GPS receiver, and receiver
clock time offset [?].
satellite 3
satellite 2
satellite 1 satellite 4
r2 r3
r4
r1
receiver
Figure 7.1: Pseudo-range measurements from four satellites used to trilaterate the
position of a receiver.
7.5.1 GPS Measurement Error

7.5.2 Transient Characteristics of GPS Positioning Error
7.5.3 GPS Velocity Measurements
Sensors for MAVs 59
MODIFIED MATERIAL: Horizontal ground speed and course are calcu-

lated from the North and East velocity components from GPS as
p
Vg = Vn2 + Ve2 (7.2)
χ = atan2 (Ve , Vn ) , (7.3)
where Vn = Va cos ψ + wn and Ve = Va sin ψ + we .
7.6 CHAPTER SUMMARY
7.7 DESIGN PROJECT

Chapter Eight
State Estimation
8.1 BENCHMARK MANEUVER
8.2 LOW-PASS FILTERS
8.3 STATE ESTIMATION BY INVERTING THE SENSOR MODEL
8.3.1 Angular Rates

8.3.2 Altitude
8.3.3 Airspeed
8.3.4 Roll and Pitch Angles
8.3.5 Position, Course, and Groundspeed
8.4 COMPLEMENTARY FILTER
8.4.1 Model Free Complementary Filter
NEW MATERIAL: The objective of the complementary filter described in

this section is to produce estimates of the Euler angles φ, θ, and ψ, when
they are small. The complementary filter fuses two types of measurements.
The first measurement for each angle comes from the rate gyros which mea-
sure the angular rates plus a bias. From equation (??), we see that when φ
and θ are small that
φ̇ ≈ p
θ̇ ≈ q
ψ̇ ≈ r,
62 STATE ESTIMATION
where ω bb/i = (p, q, r)> is the angular rate of the vehicle resolved in the
body frame. Resolving equation (??) along each body axes we get
ygyro,x = p + bp + ηp
ygyro,y = q + bq + ηq
ygyro,z = r + br + ηr ,
where b∗ is a slowly varying bias term, and η∗ is a zero mean Gaussian

random variable with known variance. Integrating the output of the rate
gyros gives
Z t Z t
4
φ̂gyro (t) = ygyro,x (τ )dτ = φ(t) + (bp (τ ) + ηp (τ )) dτ
−∞ −∞
= φ(t) + βφ (t)
Z t Z t
4
θ̂gyro (t) = ygyro,y (τ )dτ = θ(t) + (bq (τ ) + ηq (τ )) dτ
−∞ −∞
= θ(t) + βθ (t)
Z t Z t
4
ψ̂gyro (t) = ygyro,z (τ )dτ = ψ(t) + (br (τ ) + ηr (τ )) dτ
−∞ −∞
= ψ(t) + βψ (t),
where β∗ (t) are slowly varying signals, or signals with low frequency con-
tent.
The second measurement that will be used in the model-free complemen-
tary filter either comes from the accelerometers in the case of the roll and
pitch angles, or from a magnetometer, in the case of the yaw angle. Letting
vb = (u, v, w)> , using equation (??) for the rotation matrix in terms of the
Euler angles, and resolving equation (??) along each body axis, we get
yaccel,x = u̇ + qw − rv + g sin θ + bx + ηx
yaccel,y = v̇ + ru − pw + g cos θ sin φ + by + ηy
yaccel,z = ẇ + pv − qu + g cos θ cos φ + bz + ηz ,
where b∗ are slowly varying biases, and η∗ represent zero mean Gaussian
noise. If the bias terms are known, for example through pre-flight calibra-
State Estimation 63
tion, then the roll and pitch angles can be approximated as
−1 yaccel,y − by

4
φ̂accel (t) = tan
yaccel,z − bz
v̇ + ru − pw + g cos θ sin φ + ηy

−1
= tan
ẇ + pv − qu + g cos θ cos φ + ηz
= φ(t) + νφ (t) + ηφ
−1 yaccel,x − bx

4
θ̂accel (t) = sin
g
u̇ + qw − rv + g sin θ + ηx

= sin−1
g
= θ(t) + νθ (t) + ηθ .
The approximation of φ and θ given by the accelerometers will be most
accurate when the vehicle is not accelerating, i.e., when u̇ + qw − rv =
v̇+ru−pw = ẇ+pv−qu = 0. Since these terms are high frequency signals
that are linked through the dynamics to the true roll and pitch angles, φaccel
and θaccel are only good approximations at low frequencies. Therefore, we
assume that νφ and νθ are signals with high frequency content. We note that
this assumption is violated in many flight scenarios for fixed wing aircraft
like when the vehicle is in a fixed loiter configuration where νφ and νθ are
constant.
In the case of the magnetometer, the measurement is first processed by
subtracting any known biases, rotating to remove the inclination and decli-
nation angles, and then rotating to the body level frame as
mv1 = Rbv1 (φ, θ)Rz> (δ)Ry> (ι)(ymag − bmag ) (8.1)

= Rbv1 (φ, θ)Ry (ι)Rz (δ) mb + ν mag + η mag (8.2)
   
cθ sθ sφ sθ cφ cι 0 sι cδ sδ 0
= 0 cφ −sφ   0 1 0  −sδ cδ 0 (8.3)
−sθ cθ sφ cθ cφ −sι 0 cι 0 0 1

· mb + ν mag + η mag , (8.4)
where δ is the declination angle, ι is the inclination angle, η is zero mean

Gaussian noise, and ν represents all other magnetic interference that comes
from, for example, the motor of the vehicle or flying over power lines, etc.
The estimate of the heading ψ is therefore given by
ψ̂mag (t) = −atan2(mv1 v1
y , mx )
= ψ(t) + νψ (t) + ηψ .
64 STATE ESTIMATION
We will assume that νψ is a high frequency signal, and therefore, that a low
pass filtered version of ψ̂mag is essentially ψ over low frequencies.
We will present the derivation of the simple complementary filter for the
roll angle φ. The derivation for the pitch and yaw angles is similar. The
intuitive idea of the complementary filter is to estimate the roll angle by
blending a high pass version of φ̂gyro and a low pass version of φ̂accel as
φ̂ = HHP F (s)φ̂gyro + HLP F (s)φ̂accel

where HHP F (s) is a high pass filter and HLP F (s) is a low pass filter. Since
φ̂ = HHP F (s)φ̂gyro + HLP F (s)φ̂accel
= HHP F (s) [φ + βφ ] + HLP F (s) [φ + νφ + ηφ ]
= [HHP F (s) + HLP F (s)] φ + HHP F (s)βφ + HLP F (s) [νφ + ηφ ] ,
if the frequency content of βφ is below the cut-off frequency of HHP F and
the frequency content of νφ + ηφ is above the cut-off frequency of HLP F
then
φ̂ = [HHP F (s) + HLP F (s)] φ,
which implies that the filters HHP F and HLP F need to be selected so that
HHP F (s) + HLP F (s) = 1.
k s
For example, if HLP F (s) = s+kp p then we need to select HHP F (s) = s+k p
.
The block diagram for a naive implementation of the complementary filter
is shown in figure 8.1.
Figure 8.1: Naive complementary filter for roll angle estimation
The implementation of the complementary filter shown in figure 8.1 has

several drawbacks that include the need to implement two filters and also the
fact that bias rejection properties are not obvious. A better implementation
strategy is to use a feedback configuration, as explained below. Consider
the feedback loop show in figure 8.2. Following standard block diagram
manipulation we get that

1 P PC
y(s) = do (s) + di (s) + r(s)
1 + PC 1 + PC 1 + PC
State Estimation 65
Figure 8.2: Standard feedback loop
where y is the output, r is the reference input, do is an output disturbance,

and di is an input disturbance. The transfer function
1
S=
1 + PC
is called the sensitivity function, and the tranfer function
PC
T =
1 + PC
is called the complementary sensitivity function. Note that
S(s) + T (s) = 1.
If P (s)C(s) is a standard loopshape that is large (>> 1) at low frequency
and small (<< 1) at high frequency, then the sensitivity function S(s) is
a high pass filter and the complementary sensitivity function T (s) is a low
pass filter. Therefore, the feedback structure can be used to implement a
complementary filter for the roll angle as shown in figure 8.3.
Figure 8.3: Feedback loop implementation of the complementary filter.
In order to get a first order filter where

1 s 1
S(s) = = = k
1 + PC s + kp 1 + sp
we set P (s) = 1/s and C(s) = kp as shown in figure 8.4. Figure 8.4 also
indicates that φ̂gyro is the integral of the measurement ygyro,x . Clearly, the
output disturbance of φ̂gyro is equivalent to an input disturbance of ygyro,x
as shown in figure 8.5.
66 STATE ESTIMATION
As mentioned above, the gyro measurement contains the true roll rate
p plus a nearly constant bias bφ . We can use the final value theorem to
determine the response of the feedback system shown above to a constant
bφ as the input disturbance. The relevant transfer function is
P
φ̂(s) = bφ (s).
1 + PC
The final value theorem gives
φ̂ss = lim φ̂(t)
t→∞

P b
= lim s
s→0 1 + PC s
bP
= lim .
s→0 1 + P C
1
When P = s and C = kp we get
b
s
φ̂ss = lim k
s→01 + sp
b
= lim
s→0 s + kp
b
= .
kp
State Estimation 67
Therefore, the effect of the bias can be reduced by increasing kp but cannot
be eliminated. However, using a PI structure for C(s) we can completely re-
move the effect of the bias. The resulting architecture is shown in figure 8.6.
When C(s) = kp + ki /s the steady state response to a constant bias is
b
s
φ̂ss = lim kp +ki /s
s→0 1+ s
bs
= lim
s→0 ss + kp s + ki
= 0.
From the block diagram, we see that the differential equations that describe
the complementary filter are given by
˙
b̂φ = −ki (φ̂accel − φ̂) (8.5)
˙
φ̂ = (ygyro,x − b̂φ ) + kp (φ̂accel − φ̂).
Note that we have introduced negative signs in the implementation of the

integrator to emphasize the fact that the role of the integrator is to estimate
the bias and to subtract the bias from the gyro measurement.
We also note that a Lyapunov argument can be used to prove the stability
of the complementary filter given in equation (8.5) in the absence of noise.
Indeed, consider the Lyapunov function candidate
1 1
V = (φ − φ̂)2 + (bφ − b̂φ )2 .
2 2ki
68 STATE ESTIMATION
Differentiating and using the system and filter dynamics gives

˙ 1 ˙
V̇ = (φ − φ̂)(φ̇ − φ̂) + (bφ − b̂φ )(ḃφ − b̂φ )
ki
= (φ − φ̂)(p − (ygyro,x − b̂) − kp (φ̂accel − φ̂))
1
− (bφ − b̂φ )(−ki )(φ̂accel − φ̂)
ki
= (φ − φ̂)((ygyro,x − b) − (ygyro,x − b̂) − kp (φ + νφ − φ̂))
+ (bφ − b̂φ )(φ + νφ − φ̂)

2
φ − φ̂
≤ −kp (φ − φ̂) + γ|νφ |
.
b − b̂
If νφ = 0 then LaSalle’s invariance principle can be used to show asymptotic
convergence of the complementary filter. For non-zero νφ , the filter error is
bounded by a function of the size of νφ .
We note also that for Euler angle representation for pitch and yaw, a sim-
ilar derivation results in the complementary filters
˙
b̂θ = −ki (θ̂accel − θ̂) (8.6)
˙
θ̂ = (ygyro,y − b̂θ ) + kp (θ̂accel − θ̂),
˙
b̂ψ = −ki (ψ̂mag − ψ̂) (8.7)
˙
ψ̂ = (ygyro,z − b̂ψ ) + kp (ψ̂mag − ψ̂).
8.4.1.1 Digital Implementation of the Simple Complementary Filter
NEW MATERIAL: Using the Euler approximation

z(t) − z(t − Ts )
ż(t) ≈
Ts
where Ts is the sample rate, the simple complementary filter can be imple-
mented using the following pseudo-code.
Inputs:
• The rate gyro measurements ygyro,x , ygyro,y , ygyro,z .
• The accelerometer measurements yaccel,x , yaccel,y , and yaccel,z .
• The (processed) magnetometer measurement ymag,ψ .
State Estimation 69
• Accelerometer and magnetometer biases bx , by , bz , bψ .
• The sample rate Ts .
Initialization:
• Initialize the estimate of the biases b̂φ [0], b̂θ [0], b̂ψ [0].
• Initialize the estimate of the Euler angles φ̂[0], θ̂[0], ψ̂[0].
Step 1: Process the accelerometers and magnetometer:

y [n]−by
• φ̂accel [n] = tan−1 yaccel,y
accel,z [n]−bz

yaccel,x [n]−bx
• θ̂accel [n] = sin−1 g
• ψ̂mag [n] = ymag,ψ [n] − bψ .
Step 2: Compute the errors
• eφ [n] = φ̂accel [n] − φ̂[n − 1]
• eθ [n] = θ̂accel [n] − θ̂[n − 1]
• eψ [n] = ψ̂mag [n] − ψ̂[n − 1]
Step 3: Update the bias estimates:
• b̂φ [n] = b̂φ [n − 1] − Ts ki eφ [n]
• b̂θ [n] = b̂θ [n − 1] − Ts ki eθ [n]
• b̂ψ [n] = b̂ψ [n − 1] − Ts ki eψ [n]
Step 4: Update Euler angle estimates:

• φ̂[n] = φ̂[n − 1] = Ts (ygyro,x [n] − b̂φ [n]) + kp eφ [n]

• θ̂[n] = θ̂[n − 1] = Ts (ygyro,y [n] − b̂θ [n]) + kp eθ [n]

• ψ̂[n] = ψ̂[n − 1] = Ts (ygyro,z [n] − b̂ψ [n]) + kp eψ [n]
RWB: Change the previous into a python implementation.

70 STATE ESTIMATION
Algorithm 2 Continuous-discrete Observer

1: Initialize: x̂ = 0.
2: Pick an output sample rate Tout that is less than the sample rates of the
sensors.
3: At each sample time Tout :
4: for i = 1 to N do {Propagate the state equation.}
5: x̂ = x̂ + TN out
f (x̂, u)
6: end for
7: if A measurement has been received from sensor i then {Measurement
Update}
8: x̂ = x̂ + Li (yi − hi (x̂))
9: end if
8.5 DYNAMIC-OBSERVER THEORY
MODIFIED MATERIAL:
8.6 DERIVATION OF THE CONTINUOUS-DISCRETE KALMAN FILTER
MODIFIED MATERIAL: ξ is a zero-mean Gaussian random process with

covariance E{ξ(t)ξ(τ )} = Qδ(t, τ ) where δ(t, τ ) is the Dirac delta func-
tion, and η[n] is a zero-mean Gaussian random variable with covariance R.
8.6.0.1 Between Measurements:
MODIFIED MATERIAL: We can compute E{x̃ξ > } as

n Z t
> >
E{x̃ξ } = E e (A−LC)t
x̃0 ξ (t) + e(A−LC)(t−τ ) ξ(τ )ξ > (τ ) dτ
0
Z t o
− e(A−LC)(t−τ ) Lη(τ )ξ > (τ ) dτ
0
Z t
= e(A−LC)(t−τ ) Qδ(t − τ ) dτ
0
1
= Q,
2
where the 12 is because we only use half of the area inside the delta function.
Therefore, since Q is symmetric we have that P evolves between measure-
State Estimation 71
ments as
Ṗ = AP + P A> + Q.
8.6.0.2 At Measurements:
8.7 COVARIANCE UPDATE EQUATIONS
NEW MATERIAL:
Between measurements, the covariance evolves according to the equation
Ṗ = AP + P A> + Q.
One way to approximate this equation numerically is to use the Euler ap-
proximation

Pk+1 = Pk + Ts APk + Pk A> + Q .
However, given the numerical inaccuracy due to the Euler approximation, P

may not remain positive definite. We can solve this problem by discretizing
in a different way. Recall that the estimation error is given by
Z t
A(t−t0 )
x̃(t) = e x̃(t0 ) + eA(t−τ ) ξ(τ )dτ.
t0
Letting t = kTs and using the notation x̃k = x̃(kTs ), and assuming that
ξ(τ ) is held constant over each time interval and that ξk = ξ(kTs ), we get
Z Ts
ATs
x̃k+1 = e x̃k + ( eAτ dτ )ξk .
0
Using the approximation

T2
Ad = eATs ≈ I + ATs + A2 s
2
Z Ts Z Ts
Bd = ( eAτ dτ ) ≈ Idτ = Ts I,
0 0
we get
x̃k+1 = Ad x̃k + Ts ξk .
Defining the error covariance as
Pk = E{x̃k x̃>
k },
72 STATE ESTIMATION
the covariance update can be approximated as
Pk+1 = E{x̃k+1 x̃>

k+1 }
= E{(Ad x̃k + Ts ξk )(Ad x̃k + Ts ξk )> }

= E{Ad x̃k x̃> > > > 2 >
k Ad + Ts ξk x̃k Ad + Ad x̃k + Ts ξk ξk + Ts ξk ξk }
= Ad E{x̃k x̃> > > > > 2 >

k Ad + Ts E{P ξk x̃k }Ad + Ts Ad E{x̃k ξk } + Ts E{ξk ξk }
= A d Pk A > 2
d + Ts Q,
where we have made the assumption that the discrete estimation error and
the process noise are uncorrelated. Note that the discrete evolution equation
Pk+1 = Ad Pk A> 2
d + Ts Q
ensures that the error covariance remains positive definite.

During the measurement update equation, the covariance is updated as
P + = (I − LC)P − . (8.8)
The difficulty with this form of the update equation is that numerical error
may cause (I − LC) to be such that P + is not positive definite, even if P −
is positive definite. To address this issue, we go back to the equation for
covariance update given in equation (??):
P + = P − − P − C > L> − LCP − + LCP − C > L> + LRL> .
Rearranging we get
P + = (I − LC)P − (I − LC)> + LRL> .
Note that since P − and R are positive definite, that P + will also be positive
definite. Therefore, rather than using equation (8.8), it is more numerically
stable to use the so-called Joseph’s Stabilized form:
P + = (I − LC)P − (I − LC)> + LRL> .
The revised algorithm is given by Algorithm 3.
8.8 MEASUREMENT GATING
NEW MATERIAL: The performance of the Kalman filter is negatively im-

pacted by measurement outliers. The performance of the algorithm can of-
ten be improved by testing for outliers and discarding those measurements.
State Estimation 73
Algorithm 3 Continuous-discrete Extended Kalman Filter

1: Initialize: x̂ = 0.
2: Pick an output sample rate Tout which is much less than the sample
rates of the sensors.
3: At each sample time Tout :
4: for i = 1 to N do {Prediction Step}
5: Tp = Tout /N
6: x̂ = x̂ + TN out
f (x̂, u)
∂f
7: A = ∂x (x̂, u)
8: Ad = I + ATp + A2 Tp2
9: P = Ad P A> d + Tp Q
2
10: end for

11: if Measurement has been received from sensor i then {Measurement
Update}
12: Ci = ∂h ∂x (x̂, u[n])
i
13: Li = P Ci> (Ri + Ci P Ci> )−1

14: P = (I − Li Ci )P (I − Li Ci )> + Li Ri L>
i
15: x̂ = x̂ + Li (yi [n] − h(x̂, u[n])).
16: end if
Define the innovation sequence

vk = yk − C x̂−
k
= Cxk + ηk − C x̂−
k
= C x̃−
k + ηk ,
and note that since x̃−

k and ηk are zero mean that vk is zero mean. Therefore,
the covariance of vk is given by
Sk = E{vk vk> }
= E{(ηk + C x̃− − >
k )(ηk + C x̃k ) }
= R + CPk− C > ,
since ηk and x̃−

k are uncorrelated.
Note that the Kalman gain can be written as
Lk = Pk− C > Sk−1 .
The random variable
−1
zk = S k 2 v k
74 STATE ESTIMATION
is therefore an m-dimensional Gaussian random variable with zero mean

and covariance of I. Therefore the random variable wk = zk> zk is a χ2 (chi-
squared) random variable with m degrees-of-freedom. We can therefore
compute the probability that wk comes from a chi-squared distribution with
m- degrees-of-freedom, and only do a measurement update if the probability
is above a threshold.
For example, in Python, implementation of measurement gating, where
the measurement update is only performed when there is a 99% likelihood
that the measurement is not an outlier is given by
import numpy a s np
from s c i p y import s t a t s
d e f m e a s u r e m e n t u p d a t e ( s e l f , measurement , s t a t e ) :
# measurement upd ates
h = s e l f . h ( s e l f . x h a t , measurement , s t a t e )
C = j a c o b i a n ( s e l f . h , s e l f . x h a t , measurement , s t a t e )
y = np . a r r a y ( [ [ m e a s u r e m e n t . a c c e l x ,
measurement . a c c e l y ,
measurement . a c c e l z ] ] ) . T
S i n v = np . l i n a l g . i n v ( s e l f . R a c c e l + C @ s e l f . P @ C . T )
i f s t a t s . chi2 . s f ( ( y − h ) . T @ S inv @ ( y − h ) , df =3) >0.01:
L = s e l f . P @ C.T @ S inv
tmp = np . e y e ( 2 ) − L @ C
s e l f . P = tmp@self . P@tmp . T + L@self . R accel@L . T
s e l f . xhat = s e l f . xhat + L @ ( y − h )
NEW MATERIAL: The function stats.chi2.sf(x, df=3) is the

survival function for the χ2 distribution with df degrees-of-freedom and
returns the area of the tail of the probability density function above x. In
this implementation, the survival function must be computed for every time
step. An alternative is to compute a threshold on wk using the inverse sur-
vival function, and to only compute this once. For example, in the class
constructor we could use
s e l f . a c c e l t h r e s h o l d = s t a t s . c h i 2 . i s f ( q = 0 . 0 1 , d f = 3)
and then use gating thereshold

i f ( y − h ) . T @ S inv @ ( y − h ) < s e l f . a c c e l t h r e s h o l d
In matlab, we could use either

i f c h i 2 c d f ( ( y−h ) ’ ∗ S i n v ∗ ( y−h ) , 3 ) < 0 . 9 9
or
self . accel threshold = chi2inv (0.99 , 3);
i f ( y−h ) ’ ∗ S i n v ∗ ( y−h ) < s e l f . a c c e l t h r e s h o l d
State Estimation 75
8.9 ATTITUDE ESTIMATION
MODIFIED MATERIAL: Implementation of the Kalman filter requires the

Jacobians ∂f ∂h
∂x and ∂x . Accordingly we have
q cos φ tan θ − r sin φ tan θ, q sincos

φ+r cos φ

∂f 2θ
=
∂x −q sin φ − r cos φ, 0
 
0 qVa cos θ + g cos θ
∂h 
= −g cos φ cos θ −rVa sin θ − pVa cos θ + g sin φ sin θ .
∂x g sin φ cos θ (qVa + g cos φ) sin θ
8.10 GPS SMOOTHING
MODIFIED MATERIAL: Assuming that wind and airspeed are constant

we get
Va ψ̇(we cos ψ − wn sin ψ)

V̇g = .
Vg
MODIFIED MATERIAL: The Jacobian of f is given by
0 0 cos χ −Vg sin χ 0 0 0

 
0 0 sin χ Vg cos χ 0 0 0 
 
V̇ −ψ̇Va sin ψ ψ̇Va cos ψ ∂ V̇g 
0 0 − Vgg 0 Vg Vg ∂ψ 
∂f 
= 0 0 − Vg2 tan φ 0 0 ψ̇Va cos ψ 0  ,
 
∂x  g 
0 0 0 0 0 0 0  

0 0 0 0 0 0 0 
0 0 0 0 0 0 0
MODIFIED MATERIAL:
where yGP S = (yGP S,n , yGP S,e , yGP S,Vg , yGP S,χ , ywind,n , ywind,e ), u = V̂a ,
and
8.11 FULL STATE EKF
NEW MATERIAL: In this section we will expand upon the previous dis-
cussion to include the full aircraft state.
76 STATE ESTIMATION
8.11.1 Aircraft and sensor models
The model for the dynamics of the aircraft are similar to that presented in
the book. If we define p = (pn , pe , pd )> , v = (u, v, w)> , Θ = (φ, θ, ψ)> ,
ω = (p, q, r)> , then we can write the dynamics as
ṗ = R(Θ)v
v̇ = v× ω + a + R> (Θ)g
Θ̇ = S(Θ)ω
ḃ = 03×1
ẇ = 02×1 ,
where we have used the notation

 ×  
a 0 −c b
b =  c 0 −a ,
c −b a 0
and where
 
cθ cψ cθ sψ −sθ
R(Θ) = sφ sθ cψ − cφ sψ sφ sθ sψ + cφ cψ sφ cθ 
cφ sθ cψ + sφ sψ cφ sθ sψ − sφ cψ cφ cθ
 
1 sin φ tan θ cos φ tan θ
S(Θ) = 0 cos φ − sin φ  ,
0 sin φ sec θ cos φ sec θ
and where we have used the model for the accelerometer in equation (7.1)
to get
1
f = a + R> (Θ)g,
m
where g = (0, 0, g)> .

We will assume that the sensors that are available are the gyroscopes, the
accelerometers, the static and differential pressure sensors, the GPS north
and east measurements, and the GPS ground-speed and course measure-
State Estimation 77
ment. The models for the sensors are given by
yaccel = a + η accel
ygyro = ω + b + η gyro
yabs pres = ρghAGL + ηabs pres
ρVa2
ydiff pres = + ηdiff pres
2
yGPS,n = pn + νn [n]
yGPS,e = pe + νe [n]
p
yGPS,Vg = (Va cos ψ + wn )2 + (Va sin ψ + we )2 + ηV
yGPS,χ = atan2(Va sin ψ + we , Va cos ψ + wn ) + ηχ .
8.11.2 Propagation model for the EKF
Using the gyro and accelerometer models, the equations of motion can be
written as
ṗ = R(Θ)v
v̇ = v× (ygyro − b − η gyro ) + (yaccel − η accel ) + R> (Θ)g
Ω̇ = S(Θ)(ygyro − b − η gyro )
ḃ = 03×1
ẇ = 02×1 .
Defining
x = (p> , v> , Θ> , b> , w> )>

> >
y = (yaccel , ygyro )> ,
we get
ẋ = f (x, y) + Gg (x)η gyro + Ga η accel + η,

78 STATE ESTIMATION
where
 
R(Θ)v
v× (ygyro − b) + yaccel + R> (Θ)g
 
f (x, y) = 
 S(Θ)(ygyro − b) 

 03×1 
02×1 ,
 
03×3
 −v× 
 
Gg (x) = −S(Θ) 

 03×3 
02×3
 
03×3
−I3×3 
 
Ga =  03×3  ,

 03×3 
02×3
and where η is the general process noise associated with the model uncer-
tainty and assumed to have covariance Q.
The Jacobian of f is
4 ∂f
A(x, y) =
∂x
∂[R(Θ)v]
 
03×3 R(Θ) ∂Θ 03×3 03×2
∂ [R> (Θ)g]
03×3 −(ygyro − b)× −v×
 
∂Θ 03×2 
= 0 ∂[S(Θ)(ygyro −b)] .
 
 3×3 03×3 ∂Θ −S(Θ) 03×2 

03×3 03×3 03×3 03×3 03×2 
02×3 02×3 02×3 02×3 02×2
It is straightforward to show that when z ∈ R3 and A(z) is a 3 × 3 matrix

whose elements are functions of z, then for any w ∈ R3
∂ [A(z)w] ∂A(z)
∂A(z)

∂A(z)

= ∂z1 w, ∂z2 w, ∂z3 w .
∂z
State Estimation 79
Therefore
∂ [R(Θ)v] ∂R(Θ) ∂R(Θ) ∂R(Θ)

= ∂φ v, ∂θ v, ∂ψ v
>∂Θ
∂ R (Θ)g > > >

= ∂R∂φ(Θ) g, ∂R∂θ(Θ) g, ∂R∂ψ(Θ) g
∂Θ  
0 − cos θ 0
= g  cos θ cos φ − sin θ sin φ 0
−cosθ sin φ − sin θ cos φ 0
 ∂S(Θ) 
∂φ (ygyro − b)
∂ S(Θ)(ygyro − b)
=  ∂S(Θ)
∂θ (ygyro − b)
,
 
∂Θ ∂S(Θ)
∂ψ (ygyro − b)
where ∂R(Θ)
∂φ is the matrix where each element of R(Θ) has been differenti-
ated by φ.
The covariance of the noise term Gg η gyro + Ga η accel + η is
E[(Gg η gyro + Ga η accel + η)(Gg η gyro + Ga η accel + η)> ]

= Gg Qgyro G> >
g + Ga Qaccel Ga + Q,
where
2 2 2
Qgyro = diag([σgyro,x , σgyroy
, σgyroz
])
2 2 2
Qaccel = diag([σaccel,x , σaccely
, σaccelz
]),
and where
Q = diag([σp2n , σp2e , σp2d , σu2 , σv2 , σw

2 2
, σφ2 , σθ2 , σψ2 , σb2x , σb2y , σb2z , σw n
2
, σw e
])
are tuning parameters.
8.11.3 Sensor Update Equations

8.11.3.1 Static Pressure Sensor
The model for the static pressure sensor is
hstatic (x) = ρgh = −ρgpd .
Therefore, the Jacobian is given by
Cstatic (x) = (0, 0, −ρg, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) .

80 STATE ESTIMATION
8.11.3.2 Differential Pressure Sensor
The model for the differential pressure sensor is

1
hdiff (x) = ρVa2 ,
2
where
>
> >
Va2 = v − R (Θ)w v − R (Θ)w
w = (wn , we , 0)> .
Therefore
1 >
hdiff (x) = ρ v − R> (Θ)w v − R> (Θ)w .
2
The associated Jacobian is given by
 >
03×1
 v − R> (Θ)w 
>
 
Cdiff (x) = ρ 
 ∂ [R> (Θ)w] >

 .

 − ∂Θ v − R (Θ)w 
03×1
 
>

− I2×2 , 02×1 R(Θ) v − R (Θ)w
8.11.3.3 GPS North sensor
The model for the GPS north sensor is
hGPS,n (x) = pn
with associated Jacobian
CGPS,n (x) = (1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) .
8.11.3.4 GPS East sensor
The model for the GPS east sensor is
hGPS,e (x) = pe
with associated Jacobian
CGPS,e (x) = (0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) .

State Estimation 81
8.11.3.5 GPS ground-speed sensor
The inertial velocity in the world frame, projected onto the north-east plane
is given by

vg⊥ = I2×2 , 02×1 R(Θ)v. (8.9)
The ground-speed is given by
Vg (v, Θ) = kvg⊥ k .
The model for the sensor is therefore
hGPS,Vg (x) = kP R(Θ)vk ,

where P = I2×2 , 02×1 , and the associated Jacobian is given by

CGPS,Vg (x) = 01×3 , v> R> (Θ)P > P R(Θ), ∂Vg (v,Θ) >
∂Θ , 01×3 , 01×2 ,
∂Vg (v,Θ) >

where ∂Θ is computed numerically as
∂Vg (v, Θ) Vg (v, Θ + ∆ei ) − Vg (v, Θ)
= ,
∂Θi ∆
where ei is the 3 × 1 vector with one in the ith location and zeros elsewhere,
and ∆ is a small number.
8.11.3.6 GPS course
The course angle is the direction of vg⊥ = (vg⊥n , vg⊥e )> given in equa-
tion (8.9). Therefore, the course angle is given by
χ(v, Θ) = atan2(vg⊥e , vg⊥n ).
The model for the sensor is therefore
hGPS,χ (x) = atan2(vg⊥e , vg⊥n ),
and the associated Jacobian is given by

> ∂χ(v,Θ) >
CGPS,χ (x) = 01×3 , ∂χ(v,v) , , 01×3 , 01×2 ,
∂v ∂Θ
∂χ(v,v) > ∂χ(v,Θ) >

where ∂v and ∂Θ are computed numerically as
∂χ(v, Θ) χ(v + ∆ei , Θ) − χ(v, Θ)
=
∂vi ∆
∂χ(v, Θ) χ(v, Θ + ∆ei ) − χ(v, Θ)
= .
∂Θi ∆
82 STATE ESTIMATION
8.11.3.7 Pseudo Measurement for Side Slip Angle
With the given sensors, the side-slip angle, or side-to-side velocity is not
observable and can therefore drift. To help correct this situation, we can add
a pseudo measurement on the side-slip angle by assuming that it is zero.
The side slip angle is given by
vr
β= ,
Va
therefore an equivalent condition is that vr = 0. The airspeed vector is given
by
va = v − R(Θ)> w,
which implies that

vr (v, Θ, w) = 0 1 0 v − R(Θ)> w .

Therefore, the model for the pseudo-sensor is given by
hβ (x) = vr (v, Θ, w),
and the associated Jacobian is given by

> > ∂vr (v,Θ,w) >
Cβ (x) = 01×3 , ∂vr (v,Θ,w) , ∂vr (v,Θ,w) , 01×3 , , ,
∂v ∂Θ ∂w
where the partial derivatives are computed numerically as

∂vr (v, Θ, w) vr (v + ∆ei , Θ, w) − vr (v, Θ, w)
=
∂vi ∆
∂vr (v, Θ, w) vr (v, Θ + ∆ei , w) − vr (v, Θ, w)
=
∂Θi ∆
∂vr (v, Θ, w) vr (v, Θ, w + ∆ei ) − vr (v, Θ, w)
= .
∂wi ∆
The measurement used in the correction step is yβ = 0.
8.11.4 Algorithm for Direct Kalman Filter
We assume that the gyro, accelerometers, and pressure sensors are sampled
at rate Ts , which is the same rate that state estimates are required by the
controller. We also assume that GPS is sampled at TGPS >> Ts .
The algorithm for the direct Kalman filter is given as follows.
0. Initialize Filter.
State Estimation 83
Set
x̂ = (pn (0), pe (0), pd (0), Va (0), 0, 0, 0, 0, ψ(0), 0, 0, 0, 0, 0)> ,

P = diag [e2pn , e2pe , e2pd , e2u , e2v , e2w , e2φ , e2θ , e2ψ , e2bx , e2by , e2bz , e2wn , e2we ] ,
where e∗ is the standard deviation of the expected error in ∗.

1. At the fast sample rate Ts
1.a. Propagate x̂ and P according to
x̂˙ = f (x̂, y)
Ṗ = A(x̂, y)P + P A> (x̂, y) + Gg (x̂)Qgyros G> >
g (x̂) + Ga Qaccel Ga + Q
1.b. Update x̂ and P with the static pressure sensor according to

L = P − Cstatic
>
(x̂− )/(σabs
2 − − > −
pres + Cstatic (x̂ )P Cstatic (x̂ ))
P + = (I − LCstatic (x̂− ))P −
x̂+ = x̂− + L(yabs pres − hstatic (x̂− )).
1.c. Update x̂ and P with the differential pressure sensor according to
L = P − Cdiff
>
(x̂− )/(σdiff
2 − − > −
pres + Cdiff (x̂ )P Cdiff (x̂ ))
P + = (I − LCdiff (x̂− ))P −
x̂+ = x̂− + L(ydiff pres − hdiff (x̂− )).
1.d. Update x̂ and P with the side-slip pseudo measurement according to
L = P − Cβ> (x̂− )/(σβ2 + Cβ (x̂− )P − Cβ> (x̂− ))
P + = (I − LCβ (x̂− ))P −
x̂+ = x̂− + L(0 − hβ (x̂− )).
2. When GPS measurements are received at TGPS :
2.a. Update x̂ and P with the GPS north measurement according to
L = P − CGPS,n
>
(x̂− )/(σGPS,n
2
+ CGPS,n (x̂− )P − CGPS,n
>
(x̂− ))
P + = (I − LCGPS,n (x̂− ))P −
x̂+ = x̂− + L(yGPS,n − hGPS,n (x̂− )).
2.b. Update x̂ and P with the GPS east measurement according to
L = P − CGPS,e
>
(x̂− )/(σGPS,e
2
+ CGPS,e (x̂− )P − CGPS,e
>
(x̂− ))
P + = (I − LCGPS,e (x̂− ))P −
x̂+ = x̂− + L(yGPS,e − hGPS,e (x̂− )).
84 STATE ESTIMATION
2.c. Update x̂ and P with the GPS groundspeed measurement according to
L = P − CGPS,V
>
g
(x̂− )/(σGPS,V
2
g
+ CGPS,Vg (x̂− )P − CGPS,V
>
g
(x̂− ))
P + = (I − LCGPS,Vg (x̂− ))P −
x̂+ = x̂− + L(yGPS,Vg − hGPS,Vg (x̂− )).
2.d. Update x̂ and P with the GPS course measurement according to
L = P − CGPS,χ
>
(x̂− )/(σGPS,χ
2
+ CGPS,χ (x̂− )P − CGPS,χ
>
(x̂− ))
P + = (I − LCGPS,χ (x̂− ))P −
x̂+ = x̂− + L(yGPS,χ − hGPS,V χ (x̂− )).
8.12 CHAPTER SUMMARY

State Estimation 85
8.13 DESIGN PROJECT
MODIFIED MATERIAL:
8.1 Download the simulation files for this chapter from the web..
8.2 Implement the simple schemes described in Section 8.3 using low-
pass filters and model inversion to estimate the states pn , pe , h, Va , φ,
θ, χ, p, q, and r. Tune the bandwidth of the low-pass filter to observe
the effect. Different states may require a different filter bandwidth.
8.3 Modify the observer file to implement the extended Kalman filter for
roll and pitch angles described in Section 8.9. Tune the filter until you
are satisfied with the performance.
8.4 Modify the observer file to implement the extended Kalman filter for
position, heading, and wind described in Section 8.10. Tune the filter
until you are satisfied with the performance.
8.5 Change the simulation to use the estimated state in the autopilot as
opposed to the true states. Tune autopilot and estimator gains if nec-
essary. By changing the bandwidth of the low-pass filter, note that the
stability of the closed loop system is heavily influenced by this value.
Chapter Nine
Design Models for Guidance
9.1 AUTOPILOT MODEL
9.2 KINEMATIC MODEL OF CONTROLLED FLIGHT
9.2.1 Coordinated Turn

9.2.2 Accelerating Climb
MODIFIED MATERIAL: To derive the dynamics for the flight-path angle,

we will consider a pull-up maneuver in which the aircraft climbs along an
arc. Define the two dimensional plane P as the plane containing the velocity
vector vg and the vector from the center of mass of the aircraft to the instan-
taneous center of the circle defined by the pull-up maneuver. The free-body
diagram of the MAV in P is shown in figure 9.1. Since the airframe is rolled
Figure 9.1: Free-body diagram for a pull-up maneuver. The MAV is at a roll angle
of φ.
at an angle of φ, the projection of the lift vector onto P is Flift cos φ. The
centripetal force due to the pull-up maneuver is mVg γ̇. Therefore, summing
the forces in the ib -kb plane gives
Flift cos φ = mVg γ̇ + mg cos γ. (9.1)
88 DESIGN MODELS FOR GUIDANCE
Solving for γ̇ gives

g Flift
γ̇ = cos φ − cos γ . (9.2)
Vg mg
9.3 KINEMATIC GUIDANCE MODELS
9.4 DYNAMIC GUIDANCE MODEL
NEW MATERIAL:
9.5 THE DUBINS AIRPLANE MODEL
Unmanned aircraft, particularly smaller systems, fly at relatively low air-

speeds causing wind to have a significant effect on their performance. Since
wind effects are not known prior to the moment they act on an aircraft,
they are typically treated as a disturbance to be rejected in real time by the
flight control system, rather than being considered during the path planning.
It has been shown that vector-field-based path following methods, such as
those employed in this chapter, are particularly effective at rejecting wind
disturbances [3]. Treating wind as a disturbance also allows paths to be
planned relative to the inertial environment, which is important as UAVs
are flown in complex 3D terrain. Accordingly, when the Dubins airplane
model is used for path planning, the effects of wind are not accounted for
when formulating the equations of motion. In this case, the airspeed V is
the same as the groundspeed, the heading angle ψ is the same as the course
angle (assuming zero sideslip angle), and the flight-path angle γ is the same
as the air-mass-referenced flight-path angle [4].
Figure 9.2 depicts a UAV flying with airspeed V , heading angle ψ and
flight-path angle γ. Denoting the inertial position of the UAV as p =
(rn , re , rd )> , the kinematic relationship between the inertial velocity, v =
(ṙn , ṙe , ṙd )> , and the airspeed, heading angle, and flight-path angle can be
easily visualized as
   
ṙn V cos ψ cos γ
 ṙe  =  V sin ψ cos γ  ,
ṙd −V sin γ
where V = kvk.
Design Models for Guidance 89
ii (north)
ji (east) ṙd = V sin

v
ki (down)
horizontal component
of airspeed vector
ii
ṙn = V cos cos ṙe = V sin cos
V cos
Flight path projected onto ground
Figure 9.2: Graphical representation of aircraft kinematic model.
This chapter assumes that a low-level autopilot regulates the airspeed V to

a constant commanded value V c , the flight-path angle γ to the commanded
flight-path angle γ c , and the bank angle φ to the commanded bank angle φc .
In addition, the dynamics of the flight-path angle and bank angle loops are
assumed to be sufficiently fast that they can be ignored for the purpose of
path following. The relationship between the heading angle ψ and the bank
angle φ is given by the coordinated turn condition [4]
g
ψ̇ = tan φ,
V
where g is the acceleration due to gravity.

Under the assumption that the autopilot is well tuned and the airspeed,
flight-path angle, and bank angle states converge with the desired response
to their commanded values, then the following kinematic model is a good
90 DESIGN MODELS FOR GUIDANCE
description of the UAV motion

ṙn = V cos ψ cos γ c
ṙe = V sin ψ cos γ c (9.3)
ṙd = −V sin γ c
g
ψ̇ = tan φc .
V
Physical capabilities of the aircraft place limits on the achievable bank and
flight-path angles that can be commanded. These physical limits on the
aircraft are represented by the following constraints
|φc | ≤ φ̄ (9.4)
|γ c | ≤ γ̄. (9.5)
The kinematic model given by (9.3) with the input constraints (9.4) and (9.5)
will be referred to as the Dubins airplane. This model builds upon the model
originally proposed for the Dubins airplane in [?], which is given by
ṙn = V cos ψ
ṙe = V sin ψ (9.6)
ṙd = u1 |u1 | ≤ 1
ψ̇ = u2 |u2 | ≤ 1.
Although (9.3) is similar to (9.6), it captures the aircraft kinematics with
greater accuracy and provides greater insight into the aircraft behavior, and
is more consistent with commonly used aircraft guidance models. Note
however, that (9.3) is only a kinematic model that does not include aero-
dynamics, wind effects, or engine/thrust limits. While it is not sufficiently
accurate for low-level autopilot design, it is well suited for for high level
path planning and path following control design. In-depth discussions of
aircraft dynamic models can be found in [5, 6, 7, 1].
9.6 CHAPTER SUMMARY
9.7 DESIGN PROJECT
MODIFIED MATERIAL:
The objective of the assignment in this chapter is to estimate the autopilot
constants b∗ and to develop a reduced-order design models that can be used
Design Models for Guidance 91
to test and debug the guidance algorithm discussed in later chapters, prior
to implementation on the full simulation model. We will focus primarily on
the models given in equations (??) and (??).
9.1 Create a Simulink S-function or Python/Matlab class that implements

the model given in equation (??) and insert it in your MAV simulator.
For different inputs χc , hc , and Vac , compare the output of the two
models, and tune the autopilot coefficients bVa , bḣ , bh , bχ̇ , and bχ to
obtain similar behavior. You may need to re-tune the autopilot gains
obtained from the previous chapter.
Chapter Ten
Straight-line and Orbit Following
10.1 STRAIGHT-LINE PATH FOLLOWING
MODIFIED MATERIAL: MODIFIED MATERIAL: Referring to the ver-
Figure 10.1: Desired altitude calculation for straight-line path following in longi-
tudinal direction.
tical plane containing the path shown in figure 10.1(b) and using similar
triangles, we have the relationship
h + rd −qd
pd =p .
2 2
sn + se qn2 + qe2
MODIFIED MATERIAL: From figure 10.1(b), the desired altitude for an

aircraft at p following the straight-line path Pline (r, q) is given by
!
p qd
hd (r, p, q) = −rd − s2n + s2e p . (10.5)
qn2 + qe2
94 STRAIGHT-LINE AND ORBIT FOLLOWING
10.1.1 Longitudinal Guidance Strategy for Straight-line Following

10.1.2 Lateral Guidance Strategy for Straight-line Following
10.2 ORBIT FOLLOWING
NEW MATERIAL:
10.2.1 Feedforward with no wind
The commanded course angle for orbit following is given in equation (10.13)
as
d−ρ

c π −1
χ (t) = ϕ + λ + tan korbit .
2 ρ
One of the disadvantages of this strategy is that if the roll angle is used
to command the course, and if the course angle is currently correct, then the
roll angle will be zero. The controller can be improved by using a feedfor-
ward term on the roll angle.
If the UAV is on the orbit and there is no wind, then the desired heading
rate is given by
Va
ψ̇ d = λ .
R
Assuming a coordinated turn condition, the kinematics of the UAV are given
by equation (9.14) as
g
ψ̇ = tan φ.
Va
Setting these expressions equal to each other, and solving for the roll angle,
gives the feedforward term
2
−1 Va
φf f = λ tan . (10.6)
gR
In other words, if the UAV is currently on the orbit and is banked at φf f , and
assuming that airspeed and altitude are being maintained by the autopilot,
then the UAV will continue to fly on the orbit.
10.2.2 Feedforward with wind
When wind is present, the situation becomes more complicated. We will

assume in this section that the wind vector w = (wn , we , wd )> is known. In
Straight-line and Orbit Following 95
the wind case, a stationary orbit of radius R relative the ground is described
by the expression
Vg (t)
χ̇d (t) = λ ,
R
where Vg is the time varying ground speed. From equation (9.10) in the
book, the coordinated turn condition in wind is given by
g
χ̇ = tan φ cos(χ − ψ).
Vg
Equating these expressions and solving for φ results in the feedforward term
!
−1
Vg2
φf f = λ tan .
gR cos(χ − ψ)
From the wind triangle expression given in equation (2.12) we have that
1
sin(χ − ψ) = (we cos χ − wn sin χ).
Va cos γa
Using a simple identity from trigonometry we get
s 2
1
cos(χ − ψ) = 1 − (we cos χ − wn sin χ) .
Va cos γa
In a constant altitude orbit, the flight path angle γ = 0, and therefore equa-
tion (2.11), which comes from the wind triangle, becomes
wd
sin γa = ,
Va
from which we get that
s 2
wd
cos γa = 1− ,
Va
which implies that
s
(we cos χ − wn sin χ)2
cos(χ − ψ) = 1− .
Va2 − wd2
In a constant altitude orbit, the flight path angle γ = 0, and therefore

equation (2.10), which comes from the wind triangle, becomes
Vg2 − 2 (wn cos χ + we sin χ) Vg + (Vw2 − Va2 ) = 0.
Taking the positive root for Vg gives

q
Vg = (wn cos χ + we sin χ) + (wn cos χ + we sin χ)2 − Vw2 + Va2
q
= (wn cos χ + we sin χ) + Va2 − (wn sin χ − we cos χ)2 − wd2 .
The term under the square root will be positive when the airspeed is greater
than the windspeed, ensuring a positive groundspeed.
Therefore, the feedforward roll angle is given by
 q 2 
(w cos χ + w sin χ) + V 2 − (w sin χ − w cos χ)2 − w 2
n e a n e d
= λ tan−1 
 
φf f r ,
 2
Va2 −(wn sin χ−we cos χ)2 −wd

gR V 2 −w2
a d
(10.7)
which depends only on the wind speed, the current course angle, and the
airspeed. When the wind is zero, i.e., wn = we = wd = 0, then equa-
tion (10.7) simplifies to equation (10.6).
NEW MATERIAL:
10.3 3D VECTOR-FIELD PATH FOLLOWING
This section shows how to develop guidance laws to ensure that the kine-
matic model (9.3) follows straight-line and helical paths. Section ?? shows
how straight-line and helical paths are combined to produce minimum dis-
tance paths between start and end configurations.
10.3.1 Vector-field Methology
The guidance strategy will use the vector-field methodology proposed in [8],
and this section provides a brief overview. The path to be followed in R3
is specified as the intersection of two two-dimensional manifolds given by
α1 (r) = 0 and α2 (r) = 0 where α1 and α2 have bounded second partial
derivatives, and where r ∈ R3 . An underlying assumption is that the path
given by the intersection is connected and one-dimensional. Defining the
function
1 1
V (r) = α12 (r) + α22 (r),
2 2
gives
∂V ∂α1 ∂α2
= α1 (r) (r) + α2 (r) (r).
∂r ∂r ∂r
Note that − ∂V
∂r is a vector that points toward the path. Therefore following
∂V
− ∂r will transition the Dubins airplane onto the path. However simply fol-
lowing − ∂V∂r is insufficient since the gradient is zero on the path. When the
Dubins airplane is on the path, its direction of motion should be perpendic-
ular to both ∂α ∂α2 0
∂r and ∂r . Following [8] the desired velocity vector u ∈ R
1 3
can be chosen as
∂V ∂α1 ∂α2
u0 = −K1 + K2 × , (10.8)
∂r ∂r ∂r
where K1 and K2 are symmetric tuning matrices. It is shown in [8] that the
dynamics ṙ = u0 where u0 is given by equation (10.8), results in r asymp-
totically converging to a trajectory that follows the intersection of α1 and α2
if K1 is positive definite, and where the definiteness of K2 determines the
direction of travel along the trajectory.
The problem with equation (10.8) is that the magnitude of the desired
velocity u0 may not equal V , the velocity of the Dubin’s airplane. Therefore
u0 is normalized as
u0
u=V . (10.9)
ku0 k
Fortunately, the stability proof in [8] is still valid when u0 is normalized as
in equation (10.9).
Setting the NED components of the velocity of the Dubins airplane model
given in equation (9.3) to u = (u1 , u2 , u3 )> gives
V cos ψ d cos γ c = u1
V sin ψ d cos γ c = u2
−V sin γ c = u3 .
Solving for the commanded flight-path angle γ c , and the desired heading
angle ψ d results in the expressions
h u i
3
γ c = −satγ̄ sin−1 (10.10)
V
ψ d = atan2(u2 , u1 ),
where atan2 is the four quadrant inverse tangent, and where the saturation
function is defined as

a
 if x ≥ a
sata [x] = −a if x ≤ −a .

x otherwise

Assuming the inner-loop lateral-directional dynamics are accurately mod-

eled by the coordinated-turn equation, roll-angle commands yielding desir-
able turn performance can be obtained from the expression
h i
φc = satφ̄ kφ (ψ d − ψ) , (10.11)
where kφ is a positive constant.

Sections 10.3.2 and 10.3.3 applies the framework described in this section
to straight-line following and helix following, respectively.
10.3.2 Straight-line Paths
A straight-line path is described by the direction of the line and a point on

the line. Let c` = (cn , ce , cd )> be an arbitrary point on the line, and let the
direction of the line be given by the desired heading angle from north ψ` ,
and the desired flight-path angle γ` measured from the inertial north-east
plane. Therefore
   
qn cos ψ` cos γ`
4
q` =  qe  =  sin ψ` cos γ` 
qd − sin γ`
is a unit vector that points in the direction of the desired line. The straight-
line path is given by
Pline (c` , ψ` , γ` ) = r ∈ R3 : r = c` + σq` , σ ∈ R .

(10.12)
A unit vector that is perpendicular to the longitudinal plane defined by q`
is given by
 
− sin ψ`
4
nlon =  cos ψ`  .
0
Similarly, a unit vector that is perpendicular to the lateral plane defined by
q` is given by
 
− cos ψ` sin γ`
4
nlat = nlon × q` =  − sin ψ` sin γ`  .
− cos γ`
It follows that Pline is given by the intersection of the surfaces defined by
4
αlon (r) = n>
lon (r − c` ) = 0 (10.13)
4
αlat (r) = n>
lat (r − c` ) = 0. (10.14)
Figure 10.2 shows q` , c` , and the surfaces defined by αlon (r) = 0 and
αlat (r) = 0.
↵lon (r) = 0
nlat
c` q `
nlon
↵lat (r) = 0
Pline (r, q` )
Figure 10.2: This figure shows how the straight-line path Pline (c` , ψ` , γ` ) is de-
fined by the intersection of the two surfaces given by αlon (r) = 0 and αlat (r) = 0.
The gradients of αlon and αlat are given by

∂αlon
= nlon
∂r
∂αlat
= nlat .
∂r
Therefore, before normalization, the desired velocity vector is given by

u0line = K1 nlon n>lon + n n >
lat lat (r − c` ) + K2 (nlon × nlat ) . (10.15)
10.3.3 Helical Paths
A time parameterized helical path is given by

 
Rh cos(λh t + ψh )
r(t) = ch +  Rh sin(λh t + ψh )  , (10.16)
−tRh tan γh
 
rn
where r(t) =  re  (t) is the position along the path, ch = (cn , ce , cd )> is
rd
the center of the helix, and the initial position of the helix is
 
Rh cos ψh
r(0) = ch +  Rh sin ψh  ,
0
and where Rh is the radius, λh = +1 denotes a clockwise helix

(N→E→S→W), and λh = −1 denotes a counter-clockwise helix
(N→W→S→E), and where γh is the desired flight-path angle along the he-
lix.
To find two surfaces that define the helical path, the time parameterization
in (10.16) needs to be eliminated. Equation (10.16) gives
(rn − cn )2 + (re − ce )2 = Rh2 .
In addition, divide the east component of r − ch by the north component to
get
re − ce
tan(λh t + ψh ) =
rn − cn
Solving for t and plugging into the third component of (10.16) gives
re − ce

Rh tan γh −1
rd − cd = − tan − ψh .
λh rn − cn
Therefore, normalizing these equations by Rh results in
rn − cn 2 re − ce 2

αcyl (r) = + −1
Rh Rh
rd − cd re − ce

tan γh −1
αpl (r) = + tan − ψh .
Rh λh rn − cn
Normalization by Rh makes the gains on the resulting control strategy in-
variant to the size of the orbit.
A helical path is then defined as
Phelix (ch , ψh , λh , Rh , γh ) = {r ∈ R3 : αcyl (r) = 0 and αpl (r) = 0}.
(10.17)
The two surfaces αcyl (r) = 0 and αpl (r) = 0 are shown in figure 10.3 for
parameters ch = (0, 0, 0)> , Rh = 30 m, γh = 15π 180 rad, and λh = +1. The
associated helical path is the intersection of the two surfaces.
The gradients of αcyl and αpl are given by
∂αcyl >
= 2 rnR−c n
, 2 reR−c e
, 0
∂r h h
∂αpl tan γh −(re −ce ) tan γh (rn −ce ) 1

>
= 2
λh (rn −cn ) +(re −ce ) 2 , 2
λh (rn −cn ) +(re −ce )2 , Rh .
∂r
Before normalization, the desired velocity vector is given by
∂αcyl ∂αpl ∂αcyl ∂αpl

u0helix = K1 αcyl + αpl + λK2 × , (10.18)
∂r ∂r ∂r ∂r
Figure 10.3: A helical path for parameters ch = (0, 0, 0)> , Rh = 30 m, γh =

15π
180 rad, and λh = +1.
where
∂αcyl ∂αpl 2 re −ce >
× = Rh , − rnR−c n
, λh tan γh .
∂r ∂r Rh h
10.5 DESIGN PROJECT

Chapter Eleven
Path Manager
11.1 TRANSITIONS BETWEEN WAYPOINTS
MODIFIED MATERIAL: In other words, the guidance algorithm would

switch at the first time instant when
kp(t) − wi k ≤ b,
where b is the size of the ball and p(t) is the location of the MAV. MOD-
IFIED MATERIAL: MODIFIED MATERIAL: When the state machine
Figure 11.1: The geometry associated with inserting a fillet between waypoint
segments.
is in state state=1, the MAV is commanded to follow the straight-line

path along wi−1 wi , which is parameterized by r = wi−1 , and q = qi−1 ,
which are assigned in Lines ??–??. Lines ??–?? test to see if the MAV
has transitioned into the half plane shown as H1 in figure ??. If the MAV
has transitioned into H1 , then the state machine is updated to state=2.
MODIFIED MATERIAL: To be precise, let
N
4 X
|W| = kwi − wi−1 k
i=2
be defined as the length of the waypoint path W. Define |W|F as the path
length of the fillet-corrected waypoint path that will be obtained using Al-
gorithm ??. From figure 11.1 we see that the length of the fillet traversed
104 PATH MANAGER
by the corrected path is R(π − %i ). In addition, it is clear that the length

of the straight-line segment removed from |W| by traversing the fillet is
2R/ tan %2i . Therefore,
N
X 2R
|W|F = |W| + R(π − %i ) − , (11.1)
tan %2i
i=2
where %i is given in equation (??).
11.2 DUBINS PATHS
11.2.1 Definition of Dubins Path

11.2.2 Path Length Computation
11.2.2.1 Case I: R-S-R
11.2.2.2 Case II: R-S-L
11.2.2.3 Case III: L-S-R
11.2.2.4 Case IV: L-S-L
11.2.3 Algorithm for Tracking Dubins Paths
MODIFIED MATERIAL:
11.4 DESIGN PROJECT

Path Manager 105
Algorithm 4 Find Dubins Parameters:

(L, cs , λs , ce , λe , z1 , q1 , z2 , z3 , q3 )
= findDubinsParameters(ps , χs , pe , χe , R)
Input: Start configuration (ps , χs ), End configuration (pe , χe ), Radius R.
Require: kps − pe k ≥ 3R.
Require: R is larger than minimum turn radius of MAV.
>
1: crs ← ps + RRz π2 (cos χs , sin χs , 0) ,
>
2: cls ← ps + RRz −π

2 (cos χs , sin χs , 0)
>
3: cre ← pe + RRz π2 (cos χe , sin χe , 0) ,
>
4: cle ← pe + RRz − π2 (cos χe , sin χe , 0)

5: Compute L1 , L2 , L3 , and L3 using equations (??) through (??).
6: L ← min{L1 , L2 , L3 , L4 }.
7: if arg min{L1 , L2 , L3 , L4 } = 1 then
8: cs ← crs , λs ← +1, ce ← cre , λe ← +1
−cs
9: q1 ← kccee −c sk
,
z1 ← cs + RRz − π2 q1 ,

10:
11: z2 ← ce + RRz − π2 q1
12: else if arg min{L1 , L2 , L3 , L4 } = 2 then
13: cs ← crs , λs ← +1, ce ← cle , λe ← −1
14: ` ← kce − cs k,
15: ϑ ← angle(ce − cs ),
16: ϑ2 ← ϑ − π2 + sin−1 2R `
17: q1 ← Rz ϑ2 + π2 e1 ,
18: z1 ← cs + RRz (ϑ2 ) e1 ,
19: z2 ← ce + RRz (ϑ2 + π) e1
21: cs ← cls , λs ← −1, ce ← cre , λe ← +1
22: ` ← kce − cs k,
23: ϑ ← angle(ce − cs ),
24: ϑ2 ← cos−1 2R `
q1 ← Rz ϑ + ϑ2 − π2 e1 ,

25:
26: z1 ← cs + RRz (ϑ + ϑ2 ) e1 ,
27: z2 ← ce + RRz (ϑ + ϑ2 − π) e1
29: cs ← cls , λs ← −1, ce ← cle , λe ← −1
ce −cs
30: q1 ← kce −cs k ,
z1 ← cs + RRz π2 q1 ,

31:
32: z2 ← ce + RRz π2 q1
33: end if
34: z3 ← pe
35: q3 ← Rz (χe )e1
Chapter Twelve
Path Planning
12.1 POINT-TO-POINT ALGORITHMS
12.1.1 Voronoi Graphs
MODIFIED MATERIAL: If σ is unconstrained, then its optimizing value

is
(v1 − p)> (v1 − v2 )

σ∗ = ,
kv1 − v2 k2
and
s
2
((v1 − p)> (v1 − v2 ))
D(v1 , v2 , p) = kp − v1 k2 − .
kv1 − v2 k2
If we define

D(v1 , v2 , p)
 if σ ∗ ∈ [0, 1]
0 4
D (v1 , v2 , p) = kp − v1 k if σ ∗ < 0
if σ ∗ > 1,

kp − v2 k

then the distance between the point set Q and the line segment v1 v2 is given
by
0
D(v1 , v2 , Q) = min D (v1 , v2 , p).
p∈Q
108 PATH PLANNING
12.1.2 Rapidly Exploring Random Trees

12.1.2.1 RRT Waypoint Planning over 3-D Terrain
12.1.2.2 RRT Dubins Path Planning in a 2-D Obstacle Field
12.3 DESIGN PROJECT

Chapter Thirteen
Vision-guided Navigation
13.1 GIMBAL AND CAMERA FRAMES AND PROJECTIVE GEOMETRY
MODIFIED MATERIAL:
(a) Top view. (b) Side view.
Figure 13.1: A graphic showing the relationship between the gimbal and camera
frames and the vehicle and body frames.
13.1.1 Camera Model
13.2 GIMBAL POINTING
MODIFIED MATERIAL: The next step is to determine the desired azimuth

b
and elevation angles that will align the optical axis with ˇ`d . In the camera
frame, the optical axis is given by (0, 0, 1)c . Therefore, the objective is to
110 VISION-GUIDED NAVIGATION
select the commanded gimbal angles αaz c and αc so that

el
 ˇb   
`xd 0
4
ˇ`b = ˇb  = Rb (αc , αc )Rg 0
d
 ` yd g az el c (13.1)
`ˇb 1
 zd c c c c cos αc
  
cos αel cos αaz − sin αaz − sin αel az 0 0 1 0
c sin αc c − c sin αc  1 0 0 0
=  cos αel az cos α az sin αel az
c
sin αel 0 c
cos αel 0 1 0 1
(13.2)
c c
 
cos αel cos αaz
c sin αc 
= cos αel
 az . (13.3)
c
sin αel
13.3 GEOLOCATION
13.3.1 Range to Target Using the Flat-earth Model

13.3.2 Geolocation Using an Extended Kalman Filter
13.4 ESTIMATING TARGET MOTION IN THE IMAGE PLANE
13.4.1 Digital Low-pass Filter and Differentiation

13.4.2 Apparent Motion Due to Rotation
13.5 TIME TO COLLISION
13.5.1 Computing Time to Collision from Target Size

13.5.2 Computing Time to Collision from the Flat-earth Model
13.6 PRECISION LANDING

Vision-guided Navigation 111
13.8 DESIGN PROJECT

Appendix A
Nomenclature and Notation
NOMENCLATURE
NOTATION
Appendix B
Quaternions
B.1 ANOTHER LOOK AT COMPLEX NUMBERS
B.2 QUATERNION ROTATIONS
B.3 CONVERSION BETWEEN EULER ANGLES AND QUATERNIONS
B.4 CONVERSION BETWEEN QUATERNIONS AND

ROTATION MATRICES
B.5 QUATERNION KINEMATICS
B.6 AIRCRAFT KINEMATIC AND DYNAMIC EQUATIONS

Appendix C
Simulation Using Object Oriented Programming
C.1 INTRODUCTION
NEW MATERIAL: Object oriented programming is well adapted to the

implementation of complex dynamic systems like the simulator project de-
veloped in the book. This supplement will explain how the Python program-
ming language can be used to implement the project. We will outline one
particular way that the simulator can be constructed, as well as the specific
data structures (messages) that are passed between elements of the architec-
ture.
The architecture outlined in the book is shown for convenience in fig-
ure C.1. The basic idea beh
destination, map
obstacles
state estimates
path planner
waypoints
status
path manager
path definition
tracking error
airspeed, path following

altitude,
course position error
commands
autopilot
servo commands
state estimator
wind unmanned aircraft
on-board sensors
Figure C.1: Architecture outlined in the uavbook.
C.2 NUMERICAL SOLUTIONS TO DIFFERENTIAL EQUATIONS
NEW MATERIAL:
This section provides a brief overview of techniques for numerically solv-
ing ordinary differential equations, when the initial value is specified. In
118 SIMULATION USING OBJECT ORIENTED PROGRAMMING
particular, we are interested in solving the differential equation

dx
(t) = f (x(t), u(t)) (C.1)
dt
with initial condition x(t0 ) = x0 , where x(t) ∈ Rn , and u(t) ∈ Rm , and
where we assume that f (x, u) is sufficiently smooth to ensure that unique
solutions to the differential equation exist for every x0 .
Integrating both sides of equation (C.1) from t0 to t gives
Z t
x(t) = x(t0 ) + f (x(τ ), u(τ )) dτ. (C.2)
t0
The difficulty with solving equation (C.2) is that x(t) appears on both sides
of the equation, and cannot therefore be solved explicitly for x(t). Nu-
merical solutions techniques for ordinary differential equations attempt to
approximate the solution by approximating the integral in equation (C.2).
Most techniques break the solution into small time increments, usually equal
to the sample rate, which we denote by Ts . Accordingly we write equa-
tion (C.2) as
Z t−Ts Z t
x(t) = x(t0 ) + f (x(τ ), u(τ )) dτ + f (x(τ ), u(τ )) dτ
t0 t−Ts
Z t
= x(t − Ts ) + f (x(τ ), x(τ )) dτ. (C.3)
t−Ts
The most simple form of numerical approximation for the integral in

equation (C.3) is to use the left endpoint method as shown in figure C.2,
where
Z t
f (x(τ ), u(τ )) dτ ≈ Ts f (x(t − Ts ), u(t − Ts )).
t−Ts
4
Using the notation xk = x(t0 + kTs ) we get the numerical approximation
to the differential equation in equation (C.1) as
xk = xk−1 + Ts f (xk−1 , uk−1 ), x0 = x(t0 ). (C.4)
Equation (C.4) is called the Euler method, or the Runge-Kutta first order
method (RK1).
While the RK1 method is often effective for numerically solving ODEs, it
typically requires a small sample size Ts . The advantage of the RK1 method
is that it only requires one evaluation of f (·, ·).
Simulation Using Object Oriented Programming 119
Figure C.2: Approximation of integral using the left endpoint method results in
Runge-Kutta first order method (RK1).
To improve the numerical accuracy of the solution, a second order method

can be derived by approximating the integral in equation (C.3) using the
trapezoidal rule shown in figure ??, where
b
f (a) + f (b)
Z
f (ξ)dξ ≈ (b − a) .
a 2
Accordingly,
Figure C.3: Approximation of integral using the trapezoidal rule results in Runge-
Kutta second order method (RK2).
t
Ts
Z

f (x(τ ), u(τ ))dτ ≈ f (x(t − Ts ), u(t − Ts )) + f (x(t), u(t)) .
t−Ts 2
Since x(t) is unknown, we use the RK1 method to approximate it as

x(t) ≈ x(t − Ts ) + Ts f (x(t − Ts ), u(t − Ts )).
In addition, we typically assume that u(t) is constant over the interval [t −
Ts , Ts ) to get
t
Ts h
Z
f (x(τ ), u(τ ))dτ ≈ f (x(t − Ts ), u(t − Ts ))
t−Ts 2
i
+ f (x(t − Ts ) + Ts f (x(t − Ts ), u(t − Ts )), u(t − Ts )) .
Accordingly, the numerical integration routine can be written as

x0 = x(t0 )
X1 = f (xk−1 , uk−1 ) (C.5)
X2 = f (xk−1 + Ts X1 , uk−1 )
Ts
xk = xk−1 + (X1 + X2 ) .
2
Equations (C.5) is the Runge-Kutta second order method or RK2. It requires
two function calls to f (·, ·) for every time sample.
The most common numerical method for solving ODEs is the Runge-
Kutta forth order method RK4. The RK4 method is derived using Simpson’s
Rule
Z b
b−a

a+b
f (ξ)dξ ≈ f (a) + 4f + f (b) ,
a 6 2
which is derived by approximating the area under the integral using a parabola,
as shown in figure ??. The standard strategy is to write the approximation
as
Z b
b−a

a+b a+b
f (ξ)ξ ≈ f (a) + 2f + 2f + f (b) ,
a 6 2 2
and then to use
X1 = f (a) (C.6)

Ts a+b
X2 = f (a + X1 ) ≈ f (C.7)
2 2

Ts a+b
X3 = f (a + X2 ) ≈ f (C.8)
2 2
X4 = f (a + Ts X3 ) ≈ f (b), (C.9)
Figure C.4: Approximation of integral using Simpson’s rule results in Runge-

Kutta fourth order method (RK4).
resulting in the RK4 numerical integration rule

x0 = x(t0 )
X1 = f (xk−1 , uk−1 )
Ts
X2 = f (xk−1 + X1 , uk−1 ) (C.10)
2
Ts
X3 = f (xk−1 + X2 , uk−1 )
2
X4 = f (xk−1 + Ts X3 , uk−1 )
Ts
xk = xk−1 + (X1 + 2X2 + 2X3 + X4 ) .
6
It can be shown that the RK4 method matches the Taylor series approxi-
mation of the integral in equation (C.3) up to the fourth order term. Higher
order methods can be derived, but generally do not result in significantly
better numerical approximations to the ODE. The step size Ts must still be
chosen to be sufficiently small to result in good solutions. It is also possible
to develop adaptive step size solvers. For example the ODE45 algorithm in
Matlab, solves the ODE using both an RK4 and an RK5 algorithm. The two
solutions are then compared and if they are sufficiently different, then the
step size is reduced. If the two solutions are close, then the step size can be
increased.
A Python implementation of the RK4 method for the dynamics

ẋ1 x2
= ,
ẋ2 sin(x1 ) + u
y = x1
with initial conditions x = (0, 0)> , would be as follows:

import numpy a s np
import m a t p l o t l i b . p y p l o t a s p l t
c l a s s dynamics :
def init ( s e l f , Ts ) :
s e l f . t s = Ts
# set i n i t i a l conditions
s e l f . s t a t e = np . a r r a y ( [ [ 0 . 0 ] , [ 0 . 0 ] ] )
’ ’ ’ I n t e g r a t e ODE u s i n g Runge−K u t t a RK4 a l g o r i t h m ’ ’ ’
ts = self . ts
X1 = s e l f . d e r i v a t i v e s ( s e l f . s t a t e , u )
X2 = s e l f . d e r i v a t i v e s ( s e l f . s t a t e + t s / 2 ∗ X1 , u )
X3 = s e l f . d e r i v a t i v e s ( s e l f . s t a t e + t s / 2 ∗ X2 , u )
X4 = s e l f . d e r i v a t i v e s ( s e l f . s t a t e + t s ∗ X3 , u )
s e l f . s t a t e += t s / 6 ∗ ( X1 + 2∗X2 + 2∗X3 + X4 )
def output ( s e l f ) :
’ ’ ’ Returns system output ’ ’ ’
y = s e l f . s t a t e . item (0)
return y
def derivatives ( self , x , u ):

’ ’ ’ Return xdot f o r the dynamics xdot = f ( x , u ) ’ ’ ’
x1 = x . i t e m ( 0 )
x2 = x . i t e m ( 1 )
x 1 d o t = x2
x 2 d o t = np . s i n ( x1 ) + u
x d o t = np . a r r a y ( [ [ x 1 d o t ] , [ x 2 d o t ] ] )
return x dot
# i n i t i a l i z e the system
Ts = 0 . 0 1 # s i m u l a t i o n s t e p s i z e
s y s t e m = d y n a m i c s ( Ts )
# i n i t i a l i z e t h e s i m u l a t i o n t i m e and p l o t d a t a
sim time = 0.0
time = [ sim time ]
y = [ system . output ( ) ]
# main s i m u l a t i o n l o o p
while sim time < 1 0 . 0 :
u=np . s i n ( s i m t i m e ) # s e t i n p u t t o u= s i n ( t )
system . update ( u ) # propagate the system dynamics
s i m t i m e += Ts # increment the simulation time
# update data for p l o t t i n g

time . append ( s i m t i m e )
y . append ( system . o u t p u t ( ) )
# plot output y
p l t . p l o t ( time , y )
p l t . x l a b e l ( ’ time ( s ) ’ )
plt . ylabel ( ’ output y ’ )
p l t . show ( )
Appendix D
Animation
In the study of aircraft dynamics and control, it is essential to be able to

visualize the motion of the airframe. In this appendix we describe how to
create 3D animation using Python, Matlab, and Simulink.
D.1 3D ANIMATION USING PYTHON
D.2 3D ANIMATION USING MATLAB
D.3 3D ANIMATION USING SIMULINK
In the study of aircraft dynamics and control, it is essential to be able to

visualize the motion of the airframe. In this section we describe how to
create animations in Matlab/Simulink.
D.4 HANDLE GRAPHICS IN MATLAB
>> p l o t h a n d l e = p l o t ( t , s i n ( t ) )
>> s e t ( p l o t h a n d l e , ’ YData ’ , c o s ( t ) )
>> p l o t h a n d l e 1 = p l o t ( t , s i n ( t ) )
>> h o l d on
>> p l o t h a n d l e 2 = p l o t ( t , c o s ( t ) )
>> s e t ( p l o t h a n d l e 2 , ’ YData ’ , c o s ( 2 ∗ t ) )
D.5 ANIMATION EXAMPLE: INVERTED PENDULUM
f u n c t i o n h a n d l e = drawBase ( y , w i d t h , h e i g h t , gap , h a n d l e , mode )

X = [ y−w i d t h / 2 , y+ w i d t h / 2 , y+ w i d t h / 2 , y−w i d t h / 2 ] ;
Y = [ gap , gap , gap + h e i g h t , gap + h e i g h t ] ;
i f isempty ( handle ) ,
h a n d l e = f i l l (X, Y, ’m’ , ’ EraseMode ’ , mode ) ;
else
126 ANIMATION
s e t ( h a n d l e , ’ XData ’ ,X, ’ YData ’ ,Y ) ;

end
f u n c t i o n h a n d l e = drawRod ( y , t h e t a , L , gap , h e i g h t , h a n d l e , mode )

X = [ y , y+L∗ s i n ( t h e t a ) ] ;
Y = [ gap + h e i g h t , gap + h e i g h t + L∗ c o s ( t h e t a ) ] ;
h a n d l e = p l o t (X, Y, ’ g ’ , ’ EraseMode ’ , mode ) ;
else
s e t ( h a n d l e , ’ XData ’ ,X, ’ YData ’ ,Y ) ;
end
f u n c t i o n drawPendulum ( u )
% process inputs to function
y = u(1);
theta = u(2);
t = u(3);
% drawing parameters
L = 1;
gap = 0 . 0 1 ;
width = 1 . 0 ;
height = 0.1;
% define persistent variables

p e r s i s t e n t base handle
p e r s i s t e n t rod handle
% f i r s t time function i s called ,

% i n i t i a l i z e p l o t and p e r s i s t e n t v a r s
i f t ==0 ,
figure (1) , clf
t r a c k w i d t h =3;
p l o t ([ − t r a c k w i d t h , t r a c k \ w i d t h ] , [ 0 , 0 ] , ’ k ’ ) ;
h o l d on
b a s e h a n d l e = drawBase ( y , w i d t h , h e i g h t , gap , [ ] , ’ n o r m a l ’ ) ;
r o d h a n d l e = drawRod ( y , t h e t a , L , gap , h e i g h t , [ ] , ’ n o r m a l ’ ) ;
a x i s ([ − t r a c k w i d t h , t r a c k w i d t h , −L , 2∗ t r a c k w i d t h −L ] ) ;
% a t e v e r y o t h e r t i m e s t e p , r e d r a w b a s e and r o d
else
drawBase ( y , w i d t h , h e i g h t , gap , b a s e h a n d l e ) ;
drawRod ( y , t h e t a , L , gap , h e i g h t , r o d h a n d l e ) ;
end
D.6 ANIMATION EXAMPLE: SPACECRAFT USING LINES
f u n c t i o n XYZ= s p a c e c r a f t P o i n t s
% d e f i n e p o i n t s on t h e s p a c e c r a f t i n l o c a l NED c o o r d i n a t e s
Animation 127
XYZ = [ . . .
1 1 0;... % point 1
1 −1 0;... % point 2
−1 −1 0;... % point 3
−1 1 0;... % point 4
1 1 0;... % point 1
1 1 −2;... % point 5
1 −1 −2;... % point 6
1 −1 0;... % point 2
1 −1 −2;... % point 6
−1 −1 −2;... % point 7
−1 −1 0;... % point 3
−1 −1 −2;... % point 7
−1 1 −2;... % point 8
−1 1 0;... % point 4
−1 1 −2;... % point 8
1 1 −2;... % point 5
1 1 0;... % point 1
1.5 1.5 0;... % point 9
1.5 −1.5 0;... % point 10
1 −1 0;... % point 2
1.5 −1.5 0;... % point 10
−1.5 −1.5 0;... % point 11
−1 −1 0;... % point 3
−1.5 −1.5 0;... % point 11
−1.5 1.5 0;... % point 12
−1 1 0;... % point 4
−1.5 1.5 0;... % point 12
1.5 1.5 0;... % point 9
] ’;
f u n c t i o n XYZ= r o t a t e (XYZ, p h i , t h e t a , p s i )
% define rotation matrix
R roll = [ . . .
1, 0, 0;...
0 , c o s ( p h i ) , −s i n ( p h i ) ; . . .
0 , sin ( phi ) , cos ( phi ) ] ;
R pitch = [ . . .
cos ( t h e t a ) , 0 , sin ( t h e t a ) ; . . .
0, 1, 0;...
−s i n ( t h e t a ) , 0 , c o s ( t h e t a ) ] ;
R yaw = [ . . .
c o s ( p s i ) , −s i n ( p s i ) , 0 ; . . .
sin ( p s i ) , cos ( p s i ) , 0 ; . . .
0 , 0 , 1];
R = R r o l l ∗ R p i t c h ∗R yaw ;
% rotate vertices
XYZ = R∗XYZ;
128 ANIMATION
f u n c t i o n XYZ = t r a n s l a t e (XYZ, pn , pe , pd )
XYZ = XYZ + r e p m a t ( [ pn ; pe ; pd ] , 1 , s i z e (XYZ , 2 ) ) ;
Drawing the spacecraft at the desired location is accomplished using the

following Matlab code:
f u n c t i o n h a n d l e = d r a w S p a c e c r a f t B o d y ( pn , pe , pd , p h i , t h e t a , p s i , h a n d l e , mode )
% d e f i n e p o i n t s on s p a c e c r a f t i n l o c a l NED c o o r d i n a t e s
NED = s p a c e c r a f t P o i n t s ;
% r o t a t e s p a c e c r a f t by p h i , t h e t a , p s i
NED = r o t a t e (NED, p h i , t h e t a , p s i ) ;
% t r a n s l a t e s p a c e c r a f t t o [ pn ; pe ; pd ]
NED = t r a n s l a t e (NED, pn , pe , pd ) ;
% t r a n s f o r m v e r t i c e s f r o m NED t o XYZ ( f o r m a t l a b r e n d e r i n g )
R = [...
0, 1, 0;...
1, 0, 0;...
0 , 0 , −1;...
];
XYZ = R∗NED;
% plot spacecraft
h a n d l e = p l o t 3 (XYZ ( 1 , : ) , XYZ ( 2 , : ) , XYZ ( 3 , : ) , ’ EraseMode ’ , mode ) ;
else
s e t ( h a n d l e , ’ XData ’ ,XYZ ( 1 , : ) , ’ YData ’ ,XYZ ( 2 , : ) , ’ ZData ’ ,XYZ ( 3 , : ) ) ;
drawnow
end
D.7 ANIMATION EXAMPLE: SPACECRAFT USING VERTICES AND FACES
f u n c t i o n [V, F , p a t c h c o l o r s ] = s p a c e c r a f t
% Define the v e r t i c e s ( physical location of v e r t i c e s )
V = [...
1 1 0 ; . . . % point 1
1 −1 0 ; . . . % point 2
−1 −1 0 ; . . . % point 3
−1 1 0 ; . . . % point 4
1 1 −2;... % point 5
1 −1 −2;... % point 6
−1 −1 −2;... % point 7
−1 1 −2;... % point 8
1.5 1.5 0 ; . . . % point 9
1 . 5 −1.5 0 ; . . . % p o i n t 10
−1.5 −1.5 0 ; . . . % p o i n t 11
−1.5 1 . 5 0 ; . . . % p o i n t 12
];
% d e f i n e f a c e s a s a l i s t o f v e r t i c e s numbered a b o v e
F = [...
1, 2, 6, 5 ; . . . % front
Animation 129
4, 3, 7, 8 ; . . . % back
1, 5, 8, 4 ; . . . % right
2, 6, 7, 3;... % left
5, 6, 7, 8 ; . . . % top
9 , 10 , 11 , 1 2 ; . . . % bottom
];
% d e f i n e c o l o r s f o r each f a c e
myred = [1 , 0 , 0];
mygreen = [ 0 , 1 , 0 ] ;
myblue = [0 , 0 , 1];
myyellow = [ 1 , 1 , 0 ] ;
mycyan = [0 , 1 , 1];
patchcolors = [ . . .
myred ; . . . % front
mygreen ; . . . % b a c k
myblue ; . . . % right
myyellow ; . . . % l e f t
mycyan ; . . . % top
mycyan ; . . . % bottom
];
end
The vertices are shown in figure ?? and are defined in Lines 3–16. The
faces are defined by listing the indices of the points that define the face.
For example, the front face, defined in Line 19, consists of points 1 − 2 −
6 − 5. Faces can be defined by N -points, where the matrix that defines the
faces has N columns, and the number of rows is the number of faces. The
color for each face is defined in Lines 32–39. Matlab code that draws the
spacecraft body is listed below.
f u n c t i o n h a n d l e = d r a w S p a c e c r a f t B o d y ( pn , pe , pd ,
phi , t h e t a , psi ,
h a n d l e , mode )
% d e f i n e p o i n t s on s p a c e c r a f t
[V, F , p a t c h c o l o r s ] = s p a c e c r a f t ;
% r o t a t e and t h e n t r a n s l a t e s p a c e c r a f t
V = r o t a t e (V’ , p h i , t h e t a , p s i ) ’ ;
V = t r a n s l a t e (V’ , pn , pe , pd ) ’ ;
% t r a n s f o r m NED t o ENU f o r r e n d e r i n g
R = [...
0, 1, 0;...
1, 0, 0;...
0 , 0 , −1;...
];
V = V∗R ;
i f isempty ( handle )
h a n d l e = p a t c h ( ’ V e r t i c e s ’ , V, ’ F a c e s ’ , F , . . .
’ FaceVertexCData ’ , p a t c h c o l o r s , . . .
’ FaceColor ’ , ’ f l a t ’ , . . .
130 ANIMATION
’ EraseMode ’ , mode ) ;
else
s e t ( h a n d l e , ’ V e r t i c e s ’ ,V, ’ F a c e s ’ , F ) ;
end
end
Appendix E
Airframe Parameters
Table E.1: Physical parameters for the Aerosonde UAV.

Physical Param Value Motor Param Value
m 11.0 kg Vmax 44.4 V
Jx 0.824 kg-m2 Dprop 0.5 m
Jy 1.135 kg-m2 KV 145 RPM/V
Jz 1.759 kg-m2 KQ 0.0659 V-s/rad
Jxz 0.120 kg-m2 Rmotor 0.042 Ohms
S 0.55 m2 i0 1.5 A
b 2.9 m CQ 2 -0.01664
c 0.19 m CQ 1 0.004970
ρ 1.2682 kg/m3 CQ 0 0.005230
e 0.9 CT2 -0.1079
CT1 -0.06044
CT0 0.09357
132 AIRFRAME PARAMETERS
Table E.2: Aerodynamic coefficients for the Aerosonde UAV.

Longitudinal Coef. Value Lateral Coef. Value
CL0 0.23 C Y0 0
CD 0 0.043 Cl 0 0
Cm0 0.0135 Cn0 0
CLα 5.61 C Yβ -0.83
CDα 0.030 Clβ -0.13
Cmα -2.74 Cnβ 0.073
CLq 7.95 C Yp 0
CDq 0 Clp -0.51
Cm q -38.21 Cnp -0.069
CLδe 0.13 C Yr 0
CDδe 0.0135 Clr 0.25
Cmδe -0.99 Cnr -0.095
M 50 C Yδ a 0.075
α0 0.47 Clδa 0.17
0.16 Cnδa -0.011
CD p 0 C Yδ r 0.19
Cl δ r 0.0024
Cnδr -0.069
Appendix F
Trim and Linearization
F.1 NUMERICAL COMPUTATION OF THE JACOBIAN
NEW MATERIAL:
The MAV dynamics can be described at a high level by the dynamics
ẋ = f (x, u),
where x ∈ R12 and u ∈ R4 . Linearization requires the computation of the

Jacobians ∂f ∂f
∂x and ∂u , where
∂f
∂f ∂f ∂f

= ∂x ∂x2 ... ∂xn ,
∂x 1
where
 ∂f 
1
∂xi
 ∂f
 ∂x2i 

∂f
= . 
 .. 
∂xi  
∂fn
∂xi
is a column vector. By definition we have that

∂f f (x + ei ) − f (x)
(x) = lim ,
∂xi →0
where ei ∈ R12 is the column vector of all zeros except for a one in the ith
row. Therefore, by letting be a small fixed number, we get that
∂f f (x + ei ) − f (x)
(x) ≈ ,
∂xi
which can be computed numerically. Python code that computes the Jaco-
bian of f at (x, u) is given below.
import numpy a s np
def df dx ( x , u ) :
# take p a r t i a l of f ( x , u ) with respect to x
eps = 0.01 # d e v i a t i o n
134 TRIM AND LINEARIZATION
A = np . z e r o s ( ( 1 2 , 1 2 ) ) # J a c o b i a n o f f w r t x
f at x = f (x , u)
f o r i i n range ( 0 , 1 2 ) :
x e p s = np . copy ( x )
x e p s [ i ] [ 0 ] += e p s # add e p s t o i t h s t a t e
f a t x e p s = f ( x eps , u )
d f d x i = ( f a t x e p s − f a t x ) / eps
A[ : , i ] = df dxi [ : , 0 ]
return A
NEW MATERIAL:
The simulation developed in the book can be done using either Euler an-
gles or a unit quaternion to represent the attitude of the MAV. The state space
and transfer function models are all with respect to Euler angles. Therefore,
if unit quaternions are used instead of Euler angles, then we need a tech-
nique to compute the Jacobians with respect to Euler angles. Let
xe = (pn , pe , pd , u, v, w, φ, θ, ψ, p, q, r)> ∈ R12
represent the state using Euler angles and let
xq = (pn , pe , pd , u, v, w, e0 , ex , ey , ez , p, q, r)> ∈ R13
represent the state using unit quaternions for attitude. Let T : R13 → R12
be the mapping that converts quaternion representation so that xe = T (xq ).
Specifically,
pn
 
pn
 
 pe 
pe
 pd  
   
pd

u 
   
u

v
   
v
   
w 
   
w

T  e0  =  ,
   
   Θφ (e0 , ex , ey , ez ) 
 ex  
   Θθ (e0 , ex , ey , ez ) 

e
 y 
 e  Θψ (e0 , ex , ey , ez )

 z  p 
p  
  
q q 
r
r
where Θ(e) is given in equation (??).

Trim and Linearization 135
Since xe = T (xq ) we have

∂T
ẋe = ẋq
∂xq
∂T
= f (xq , u)
∂xq
∂T
= f (T −1 (xe ), u),
∂xq
where T −1 (xe ) is given in equation (??), and where
 
I3×3 03×3 03×4 03×3
∂T 03×3 I3×3 03×4 03×3 
= 03×3 03×3 ∂Θ 03×3 
 (F.1)
∂xq ∂e
03×3 03×3 03×4 I3×3 ,
and where ∂Θ∂e can be approximated numerically using the technique out-
lined above.
F.2 TRIM AND LINEARIZATION IN SIMULINK
[X, U, Y,DX] =TRIM ( ’SYS ’ , X0 , U0 , Y0 , IX , IU , IY , DX0 , IDX ) ,

MODIFIED MATERIAL: For our situation we know that
ẋ∗ = ([don’t care], [don’t care], Va∗ sin γ ∗ , 0, 0, 0, 0, 0, Va∗ /R∗ cos(γ ∗ ), 0, 0, 0)> ,
therefore we let
DX = [0; 0; Va*sin(gamma); 0; 0; 0; 0; 0; Va/R*cos(gamma); 0;
0; 0]
IDX = [3; 4; 5; 6; 7; 8; 9; 10; 11; 12] .
Similarly, the initial state, inputs, and outputs can be specified as X0 = [0; 0; 0; Va;
0; 0; 0; gamma; 0; 0; 0; 0]
IX0 = []
U0 = [0; 0; 0; 1]
IU0 = []
Y0 = [Va; 0; 0]
IY0 = [1,3].
[A, B , C , D] =LINMOD( ’SYS ’ ,X, U) ,
where X and U are the state and input about which the Simulink diagram
is to be linearized, and [A,B,C,D] is the resulting state-space model. If
the linmod command is used on the Simulink diagram shown in figure ??,
where there are twelve states and four inputs, the resulting state space equa-
tions will include the models given in equations (??) and (??). To obtain
equation (??) for example, you could use the following steps:
[A, B , C , D] = l i n m o d ( f i l e n a m e , x t r i m , u t r i m )
A l a t = A( [ 5 , 10 , 12 , 7 , 9 ] , [ 5 , 10 , 12 , 7 , 9 ] ) ;
B l a t = B( [ 5 , 10 , 12 , 7 , 9 ] , [ 3 , 4 ] ) ;
NEW MATERIAL:
If your Simulink implementation uses quaternions for the state, then
linmod returns state space equations with respect to xq , whereas the stan-
dard state space equations require the state space equations with respect to
xe . Suppose that the quaternion state space equations are given by
ẋq = fq (xq , u),
and that the euler state space equations are given by
ẋe = fe (xe , u),
and recall from the discussion above that xe = T (xq ). Differentiating with
respect to time we get
∂T
ẋe = ẋq
∂xq
∂T
= fq (xq , u)
∂xq
∂T
= fq (T −1 (xe ), u)
∂xq
= fe (xe , u).
Therefore the Jacobian with respect to xe is given by
∂fe
∂T
∂ ∂x q
fq (T −1 (xe ), u)
=
∂xe ∂xe
∂T ∂fq ∂T −1
= ,
∂xq ∂xq ∂xe
∂T
where ∂xq given in equation (F.1) and where
 
I3×3 03×3 03×3 03×3
∂T −1 03×3 I3×3 03×3 03×3 
=
04×3 ∂Q
 (F.2)
∂xq 04×3 ∂Θ 04×3 
03×3 03×3 03×3 I3×3 ,
∂Q
and where Q(Θ) is given by equation (??) and where ∂Θ can be computed
numerically.
Trim and Linearization 137
The Jacobian for the B matrix can be found similarly as

∂fe ∂T ∂fq
= .
∂u ∂xq ∂u
F.3 TRIM AND LINEARIZATION IN MATLAB
NEW MATERIAL:
In Matlab, trim can be computed using the built in optimization function
fmincon. In general fmincon can be configured to minimize nonlinear
functions with general constraints. For example, to minimize the function
J(x) = x>x − γ subject to the constraints that x1 = x2 and sin(x3 ) ≤
−0.5, the appropriate Matlab code is as follows:
x0 = [ 1 0 ; 5 ; −20; 8 ] ; % i n i t i a l g u e s s f o r t h e optimum
gam = 3 ; % p a r a m e t e r p a s s e d i n t o o b j e c t i v e f u n c t i o n
x o p t = f m i n c o n ( @ o b j e c t i v e , x0 , [ ] , [ ] , [ ] , [ ]
[ ] , [ ] , @ c o n s t r a i n t s , [ ] , gam ) ;
% o b j e c t i v e f u n c t i o n t o be m i n i m i z e d
f u n c t i o n J = o b j e c t i v e ( x , gam )
J = x∗x ’ − gam ;
end
% nonlinear constraints for trim optimization

f u n c t i o n [ c , c e q ] = c o n s t r a i n t s ( x , gam )
% i n e q u a l i t y c o n s t r a i n t s c ( x)<=0
c (1) = sin (x (3)) + 0.5;
e q u a l i t y c o n s t r a i n t s c e q ( x )==0
c e q ( 1 ) = x (1) − x ( 2 ) ;
end
F.4 TRIM AND LINEARIZATION IN PYTHON
NEW MATERIAL:
In Python, trim can be computed using the scipy optimize library. For
example, to minimize the function J(x) = x>x − γ subject to the con-
straints that x1 = x2 and sin(x3 ) ≤ −0.5, the appropriate Python code is as
follows: import numpy as np from scipy.optimize import minimize
# i n i t i a l g u e s s f o r t h e optimum
x0 = np . a r r a y ( [ [ 1 0 ; 5 ; −20; 8 ] ] ) . T ;
gam = 3 ; # p a r a m e t e r p a s s e d i n t o o b j e c t i v e f u n c t i o n
# f i r s t c o n s t r a i n t i s e q u a l i t y c o n s t r a i n t x1==x2
# s e c o n d c o n s t r a i n t i s i n e q u a l i t y c o n s t r a i n t s i n ( x3 )<=−0.5
c o n s = ( { ’ t y p e ’ : ’ eq ’ , ’ f u n ’ : lambda x : x [0] − x [ 1 ] , } ,
{ ’ t y p e ’ : ’ i n e q ’ , ’ f u n ’ : lambda x : np . s i n ( x [ 2 ] ) + 0 . 5 } )
# s o l v e t h e m i n i m i z a t i o n problem
r e s = m i n i m i z e ( o b j e c t i v e , x0 ,
method = ’SLSQP ’ ,
a r g s = ( gam ) ,
c o n s t r a i n t s = cons ,
o p t i o n s ={ ’ f t o l ’ : 1 e −10 , ’ d i s p ’ : T r u e } )
# extract optimal st ate
x o p t = np . a r r a y ( [ r e s . x ] ) . T
# o b j e c t i v e f u n c t i o n t o be m i n i m i z e d
d e f o b j e c t i v e ( x , gam ) :
J = np . d o t ( x . T , x ) − gam
return J
Appendix G
Essentials from Probability Theory

Appendix H
Sensor Parameters
H.1 RATE GYROS
H.2 ACCELEROMETERS
H.3 PRESSURE SENSORS
H.4 DIGITAL COMPASS/MAGNETOMETER
H.5 GPS
Appendix I
Useful Formulas and other Information
I.1 CONVERSION FROM KNOTS TO MPH
I.2 DENSITY OF AIR

Bibliography
[1] T. R. Yechout, S. L. Morris, D. E. Bossert, and W. F. Hallgren, Introduc-

tion to Aircraft Flight Mechanics. AIAA Education Series, American
Institute of Aeronautics and Astronautics, 2003.
[2] R. F. Stengel, Flight Dynamics. Princeton University Press, 2004.
[3] D. R. Nelson, D. B. Barber, T. W. McLain, and R. W. Beard, “Vector

Field Path Following for Miniature Air Vehicles,” IEEE Transactions
on Robotics, vol. 37, pp. 519–529, jun 2007.
[4] R. W. Beard and T. W. McLain, Small Unmanned Aircraft: Theory and

Practice. Princeton University Press, 2012.
[5] W. F. Phillips, Mechanics of Flight. Wiley, 2004.
[6] B. L. Stevens and F. L. Lewis, Aircraft Control and Simulation. Hobo-

ken, New Jersey: John Wiley & Sons, Inc., 2nd ed., 2003.
[7] R. C. Nelson, Flight Stability and Automatic Control. Boston, Mas-

sachusetts: McGraw-Hill, 2nd ed., 1998.
[8] V. M. Goncalves, L. C. A. Pimenta, C. A. Maia, B. C. O. Durtra, G. A. S.

Pereira, B. C. O. Dutra, and G. A. S. Pereira, “Vector Fields for Robot
Navigation Along Time-Varying Curves in n-Dimensions,” IEEE Trans-
actions on Robotics, vol. 26, pp. 647–659, aug 2010.
Index
Absolution Pressure Sensor, 57 Ego Motion, 110

Accelerometers, 57, 75 Estiamtion
Aerodynamic Coefficients, 8 Heading, 75
AIRSPEED, 3 Estimation
Airspeed, 4, 13, 16 Course, 75
Angle of Attack, 16 Position, 75
Autopilot Roll and Pitch Angles, 75
Airspeed Hold using Throttle, 46 Wind, 75
Altitude Hold Using Pitch, 45
Course Hold, 37 Flat Earth Model, 110
Pitch Attitude Hold, 43 Flight Path Angle
Yaw damper, 38 Inertial Referenced, 88
Bandwidth Separation, 38 GPS, 57, 75

GROUND SPEED, 3
Ground Speed, 4, 13, 59
Coordinate Frames, 3, 109
Body Frame, 3, 109
Heading Angle, 3
Gimbal Frame, 109 helical path following, 99
Inertial Frame, 3
Stability Frame, 3
Kalman Filter
Vehicle Frame, 3 Continuous-discrete to Estimate Roll and
Vehicle-1 Frame, 3 Pitch, 75
Vehicle-2 Frame, 3 Continuous-discrete to Position, Course,
Wind Frame, 3 Wind, Heading, 75
Coordinated Turn, 23, 87 Basic Explanation, 70
Course Angle, 3, 59 Derivation, 70
Crab Angle, 3 Geolocation, 110
Kinematics
Design Models, 1, 21, 87 Position, 22
Differential Pressure Sensor, 57 Rotation, 22
Dubins airplane, 90
Dubins Path Lateral Motion, 23, 26, 35
Computation, 104 Linearization, 26
Definition, 104 Longitudinal Motion, 7, 23, 26, 43
RRT Paths through Obstacle Field, 108 Low Pass Filter, 110
Tracking, 104 Low-pass Filter, 61
Dutch-roll Mode, 28
Dynamics path following, 96
Rotational Motion, 5 Phugoid Mode, 28
148 INDEX
PID: Digital Implementation, 48

Precision Landing, 110
Rapidly Exploring Random Trees (RRT),

108
3-D Terrain, 108
Using Dubins Paths, 108
Rate Gyros, 57, 75
Roll Mode, 28
Rotation Matrices, 3
Short-period Mode, 28
Side Slip Angle, 16
Simulation Model, 1, 22, 133
Spiral-divergence Mode, 28
State-Space Models
Longitudinal, 26
State-space Models
Lateral, 26
straight-line path following, 98
Successive Loop Closure, 33
Transfer Functions
Elevator to Pitch, 43
Pitch to Altitude, 45
Roll to Course, 37
Throttle to Airspeed, 46
Trim, 23
vector field guidance model, 96

Voronoi Path Planning, 107
Wind Gusts, 13
Dryden Model, 14
WIND SPEED, 3
Wind Speed, 4, 13
Wind Triangle, 3, 4

Uavbook Supplement

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Uavbook Supplement

Uploaded by

Copyright:

Available Formats

uavbook March 18, 2020

Small Unmanned Aircraft

Small Unmanned Aircraft

Theory and Practice

PRINCETON UNIVERSITY PRESS

3. Kinematics and Dynamics 5

4. Forces and Moments 7

5. Linear Design Models 21

7. Sensors for MAVs 57

9. Design Models for Guidance 87

10. Straight-line and Orbit Following 93

11. Path Manager 103

11.1 Transitions Between Waypoints 103

12. Path Planning 107

13. Vision-guided Navigation 109

A. Nomenclature and Notation 113

C. Simulation Using Object Oriented Programming 117

E. Airframe Parameters 131

F. Trim and Linearization 133

F.2 Trim and Linearization in Simulink 135

G. Essentials from Probability Theory 139

H. Sensor Parameters 141

I. Useful Formulas and other Information 143

1.1 SYSTEM ARCHITECTURE

1.2 DESIGN MODELS

1.3 DESIGN PROJECT

2.1 ROTATION MATRICES

2.2 MAV COORDINATE FRAMES

2.2.1 The inertial frame F i

2.3 AIRSPEED, WIND SPEED, AND GROUND SPEED

MODIFIED MATERIAL: Combining expressions, we can express the air-

2.4 THE WIND TRIANGLE

MODIFIED MATERIAL: Similarly, the airspeed vector in the inertial frame

2.5 DIFFERENTIATION OF A VECTOR

2.6 CHAPTER SUMMARY

NOTES AND REFERENCES

2.7 DESIGN PROJECT

Kinematics and Dynamics

3.1 STATE VARIABLES

3.3 RIGID-BODY DYNAMICS

3.3.1 Rotational Motion

3.4 CHAPTER SUMMARY

NOTES AND REFERENCES

3.5 DESIGN PROJECT

3.1 Implement the MAV equations of motion given in equations (??)

Forces and Moments

4.1 GRAVITATIONAL FORCES

NEW MATERIAL: Using a unit quaternion to represent attitude instead of

4.2 AERODYNAMIC FORCES AND MOMENTS

4.2.1 Control Surfaces

MODIFIED MATERIAL: Mathematically, we can convert between rudder-

4.2.2 Longitudinal Aerodynamics

MODIFIED MATERIAL: The parameter eos is the Oswald efficiency fac-

4.2.3 Lateral Aerodynamics

MODIFIED MATERIAL: The coefficient CY0 is the value of the lateral

4.2.4 Aerodynamic Coefficients

4.3 PROPULSION FORCES AND MOMENTS

where ρ is the density of air, D is the propeller diameter, Ωp is the propeller

As shown in figures 4.1 and 4.2, aerodynamic coefficients can be approxi-

C T = (-0.047394) J2 + (-0.13803) J + (0.11221)

and torque produced by the propeller are given by

CQ, data and 2nd-order curve fit

2 C Q = (-0.015729) J2 + (0.0031409) J + (0.006199)