Small Unmanned Aircraft - Brigham Young Universityuavbook.byu.edu/lib/exe/fetch.php?media=uavbook... · 2020. 3. 18. · 2.3 Airspeed, Wind Speed, and Ground Speed3 2.4 The Wind Triangle3

uavbook March 18, 2020

Small Unmanned Aircraft



Small Unmanned Aircraft

Theory and PracticeSupplement

Randal W. BeardTimothy W. McLain

PRINCETON UNIVERSITY PRESS

PRINCETON AND OXFORD



Contents

1. Introduction 11.1 System Architecture 11.2 Design Models 11.3 Design Project 1

2. Coordinate Frames 32.1 Rotation Matrices 32.2 MAV Coordinate Frames 32.3 Airspeed, Wind Speed, and Ground Speed 32.4 The Wind Triangle 32.5 Differentiation of a Vector 42.6 Chapter Summary 42.7 Design Project 4

3. Kinematics and Dynamics 53.1 State Variables 53.2 Kinematics 53.3 Rigid-body Dynamics 53.4 Chapter Summary 53.5 Design Project 5

4. Forces and Moments 74.1 Gravitational Forces 74.2 Aerodynamic Forces and Moments 74.3 Propulsion Forces and Moments 84.4 Atmospheric Disturbances 134.5 Chapter Summary 174.6 Design Project 18

5. Linear Design Models 215.1 Summary of Nonlinear Equations of Motion 215.2 Coordinated Turn 235.3 Trim Conditions 235.4 Transfer Function Models 235.5 Linear State-space Models 265.6 Chapter Summary 305.7 Design Project 30

vi CONTENTS

6. Autopilot Design 336.1 Successive Loop Closure 336.2 Total Energy Control 496.3 LQR Control 516.4 Chapter Summary 546.5 Design Project 55

7. Sensors for MAVs 577.1 Accelerometers 577.2 Rate Gyros 577.3 Pressure Sensors 577.4 Digital Compasses 577.5 Global Positioning System 577.6 Chapter Summary 597.7 Design Project 59

8. State Estimation 618.1 Benchmark Maneuver 618.2 Low-pass Filters 618.3 State Estimation by Inverting the Sensor Model 618.4 Complementary Filter 618.5 Dynamic-observer Theory 708.6 Derivation of the Continuous-discrete Kalman Filter 708.7 Covariance Update Equations 718.8 Measurement Gating 728.9 Attitude Estimation 758.10 GPS Smoothing 758.11 Full State EKF 758.12 Chapter Summary 848.13 Design Project 85

9. Design Models for Guidance 879.1 Autopilot Model 879.2 Kinematic Model of Controlled Flight 879.3 Kinematic Guidance Models 889.4 Dynamic Guidance Model 889.5 The Dubins Airplane Model 889.6 Chapter Summary 909.7 Design Project 90

10. Straight-line and Orbit Following 9310.1 Straight-line Path Following 9310.2 Orbit Following 9410.3 3D Vector-field Path Following 9610.4 Chapter Summary 10110.5 Design Project 101

11. Path Manager 103

CONTENTS vii

11.1 Transitions Between Waypoints 10311.2 Dubins Paths 10411.3 Chapter Summary 10411.4 Design Project 104

12. Path Planning 10712.1 Point-to-Point Algorithms 10712.2 Chapter Summary 10812.3 Design Project 108

13. Vision-guided Navigation 10913.1 Gimbal and Camera Frames and Projective Geometry 10913.2 Gimbal Pointing 10913.3 Geolocation 11013.4 Estimating Target Motion in the Image Plane 11013.5 Time to Collision 11013.6 Precision Landing 11013.7 Chapter Summary 11013.8 Design Project 111

A. Nomenclature and Notation 113

B. Quaternions 115B.1 Another look at complex numbers 115B.2 Quaternion Rotations 115B.3 Conversion Between Euler Angles and Quaternions 115B.4 Conversion Between Quaternions and

Rotation Matrices 115B.5 Quaternion Kinematics 115B.6 Aircraft Kinematic and Dynamic Equations 115

C. Simulation Using Object Oriented Programming 117C.1 Introduction 117C.2 Numerical Solutions to Differential Equations 117

D. Animation 125D.1 3D Animation Using Python 125D.2 3D Animation Using Matlab 125D.3 3D Animation Using Simulink 125D.4 Handle Graphics in Matlab 125D.5 Animation Example: Inverted Pendulum 125D.6 Animation Example: Spacecraft using Lines 126D.7 Animation Example: Spacecraft Using Vertices and Faces 128

E. Airframe Parameters 131

F. Trim and Linearization 133F.1 Numerical Computation of the Jacobian 133

viii CONTENTS

F.2 Trim and Linearization in Simulink 135F.3 Trim and Linearization in Matlab 137F.4 Trim and Linearization in Python 137

G. Essentials from Probability Theory 139

H. Sensor Parameters 141H.1 Rate Gyros 141H.2 Accelerometers 141H.3 Pressure Sensors 141H.4 Digital Compass/Magnetometer 141H.5 GPS 141

I. Useful Formulas and other Information 143I.1 Conversion from knots to mph 143I.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 Fv

2.2.3 The vehicle-1 frame Fv1

2.2.4 The vehicle-2 frame Fv2

2.2.5 The body frame Fb

2.2.6 The stability frame Fs

2.2.7 The wind frame Fw

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

Vba =

urvrwr

= Rbw

Va00

=

cosβ cosα − sinβ cosα − sinαsinβ cosβ 0

cosβ sinα − sinβ sinα cosα

Va00

,

2.4 THE WIND TRIANGLE

4 COORDINATE FRAMES

MODIFIED MATERIAL: Similarly, the airspeed vector in the inertial framecan be expressed as

Via = Va

cos(ψ + β) cos γasin(ψ + β) cos γa− sin γa

,

where Va = ‖Va‖. Under the assumption that the sideslip angle is zero, thewind triangle can be expressed in inertial coordinates as

Vg

cosχ cos γsinχ cos γ− sin γ

= Va

cos(ψ + β) cos γasin(ψ + β) cos γa− sin γa

+

wnwewd

. (2.1)

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 andmoments 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 inAppendix 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 andconvincing yourself that the motion is appropriate.

3.3 Since Jxz is non-zero, there is gyroscopic coupling between roll andyaw. 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 theroll and yaw axes. Verify that when Jxz is not zero, there is couplingbetween the roll and yaw axes.



Chapter Four

Forces and Moments

4.1 GRAVITATIONAL FORCES

NEW MATERIAL: Using a unit quaternion to represent attitude instead ofEuler angles, the gravity force can be expressed as

f bg = mg

2(exez − eye0)2(eyez + exe0)e2z + e2

0 − e2x − e2

y

.

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δr

)=

1

2

(1 11 −1

)(δrrδrl

).

MODIFIED MATERIAL: Mathematically, we can convert between elevonsand aileron-elevator signals with(

δeδa

)=

1

2

(1 1−1 1

)(δerδ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 lateralforce 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 theairspeed of the aircraft Va and the throttle setting δt ∈ [0, 1]. We assumethat the thrust and torque vectors produced by the propeller/motor is alignedwith the rotation axes of the motor and denote the magnitude of the thrustby Tp and the magnitude of the torque by Qp.

Based on propeller theory [], the standard model for the thrust and torqueproduced by a propeller is given by

Tp(Ωp, CT ) =ρD4

4π2Ω2pCT

Qp(Ωp, CQ) =ρD5

4π2Ω2pCQ,

where ρ is the density of air, D is the propeller diameter, Ωp is the propellerspeed in radians per second, and CT and CQ are non-dimensional aerody-namic coefficients. The aerodynamic coefficients are found experimentallyand typical plots are shown in figures 4.1 and 4.2, where CT and CQ areplotted as a function of the nondimensional advanced ratio

J(Ωp, Va) =2πVaΩpD

.

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) ≈ CT2J2 + CT1J + CT0

CQ(J) ≈ CQ2J2 + CQ1J + CQ0,

where the coefficients CT∗ and CQ∗ are unitless coefficients that are deter-mined experimentally from data. The quadratic approximations are shownas solid lines in figures 4.1 and 4.2. Combining these equations, the thrust

Forces and Moments 9

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7

advance ratio - J (1/rev)

-0.02

0

0.02

0.04

0.06

0.08

0.1

0.12

CT, d

ata

and

2nd-

orde

r cu

rve

fit

CT = (-0.047394) J2 + (-0.13803) J + (0.11221)

Figure 4.1: Thrust coefficient versus advance ratio for APC Thin Electric 10x5propeller [?].

and torque produced by the propeller are given by

Tp(Ωp, Va) =ρD4

4π2Ω2p

(CT2J

2(Ωp, Va) + CT1J(Ωp, Va) + CT0

)=

(ρD4CT0

4π2

)Ω2p +

(ρD3CT1Va

2π

)Ωp + (ρD2CT2V

2a )

(4.1)

Qp(Ωp, Va) =ρD5

4π2Ω2p

(CQ2J

2(Ωp, Va) + CQ1J(Ωp, Va) + CQ0

)=

(ρD5CQ0

4π2

)Ω2p +

(ρD4CQ1Va

2π

)Ωp + (ρD3CQ2V

2a ).

(4.2)

The speed of the propeller Ωp will be determined by the torque applied tothe propeller by the motor. For a DC motor, the steady-state torque gener-ated for a given input voltage Vin is given by

Qm = KQ

[1

R(Vin −KV Ωm)− i0

], (4.3)

where KQ is the motor torque constant, R is the resistance of the motorwindings, KV is the back-emf voltage constant, Ωm is the angular speed ofthe 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


0 0.1 0.2 0.3 0.4 0.5 0.6 0.7

advance ratio - J (1/rev)

0

1

2

3

4

5

6

7

CQ

, dat

a an

d 2n

d-or

der

curv

e fit

10-3

CQ

= (-0.015729) J2 + (0.0031409) J + (0.006199)

Figure 4.2: Torque coefficient versus advance ratio for APC Thin Electric 10x5propeller [?].

energy domains. If consistent units (such as SI units) are utilized, KQ andKT are identical in value.1 It should be noted that when a voltage changeis applied to the motor, the motor changes speed according to the dynamicbehavior of the motor. For a fixed-wing aircraft, these transients are signif-icantly faster than the aircraft dynamics and we can neglect them withoutnegatively 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 groupinglike terms gives

(ρD5

(2π)2CQ0

)Ω2p +

(ρD4

2πCQ1Va +

K2Q

R

)Ωp

+

(ρD3CQ2V

2a −

KQ

RVin +KQi0

)= 0. (4.4)

1We assume KQ has units of N-m/A and KV has units of V-sec/rad. In RC aircraft motordatasheets, it is common for KV to be specified in units of rpm/V.


Denoting the coefficients of this quadratic equation as

a =ρD5

(2π)2CQ0

b =ρD4

2πCQ1Va +

K2Q

R

c = ρD3CQ2V2a −

KQ

RVin +KQi0,

the positive root given by

Ωp(Vin, Va) =−b+

√b2 − 4ac

2a(4.5)

defines the operating speed of the propeller as a function of airspeed Va andmotor voltage Vin.

Using the approach above to find the operating speed, the thrust andtorque characteristics of the motor/propeller combination over the full rangeof voltage and airspeed inputs can be quantified. A family of curves, one foreach level of voltage input, is plotted versus airspeed and shows the thrustand torque generated by a specific motor/propeller pair in figures 4.3 and4.4. Although the voltages are plotted at discrete intervals, the voltage inputis assumed to be continuously variable. Figure 4.3 in particular illustratesthe full range of thrust performance for the selected motor/propeller pair.As expected, the maximum thrust for a given voltage setting occurs at zeroairspeed 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 canoccur when an aircraft is gliding and descending in a motor-off state and theairspeed is being controlled using the pitch angle of the aircraft.

In this book, we will approximate the input voltage as a linear function ofthe throttle command, which is roughly true for PWM commands applied toan 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,


0 5 10 15 20 25 30

airspeed (m/s)

0

5

10

15

thru

st (

N)

2 V

4 V

6 V

8 V

10 V

12 V

Figure 4.3: Propeller thrust versus airspeed for various motor voltage settings.

0 5 10 15 20 25 30

airspeed (m/s)

0

0.05

0.1

0.15

0.2

torq

ue (

N-m

)

2 V

4 V

6 V

8 V

10 V

12 V

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 thedynamics of the aircraft. In chapter 2, we defined Vg as the velocity of theairframe relative to the ground, Va as the velocity of the airframe relative tothe surrounding air mass, and Vw as the velocity of the air mass relative tothe ground, or in other words, the wind velocity. As shown in equation (??),the relationship between ground velocity, air velocity, and wind velocity isgiven by

Vg = Va + Vw. (4.7)

For simulation purposes, we will assume that the total wind vector can berepresented as

Vw = Vws + Vwg ,

where Vws is a constant vector that represents a steady ambient wind, andVwg is a stochastic process that represents wind gusts and other atmosphericdisturbances. The ambient (steady) wind is typically expressed in the iner-tial frame as

Viws =

wnsweswds

,

where wns is the speed of the steady wind in the north direction, wes isthe speed of the steady wind in the east direction, and wds is the speed ofthe steady wind in the down direction. The stochastic (gust) componentof the wind is typically expressed in the aircraft body frame because theatmospheric effects experienced by the aircraft in the direction of its forwardmotion occur at a higher frequency than do those in the lateral and downdirections. 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 σwgust description (m) (m) (m) (m/s) (m/s)low altitude, light turbulence 50 200 50 1.06 0.7low altitude, moderate turbulence 50 200 50 2.12 1.4medium altitude, light turbulence 600 533 533 1.5 1.5medium altitude, moderate turbulence 600 533 533 3.0 3.0

frame components as

Vbwg =

uwgvwgwwg

.

Experimental results indicate that a good model for the non-steady gustportion of the wind model is obtained by passing white noise through alinear time-invariant filter given by the von Karmen turbulence spectrumin [2]. Unfortunately, the von Karmen spectrum does not result in a rationaltransfer function. A suitable approximation of the von Karmen model isgiven by the Dryden transfer functions

Hu(s) = σu

√2VaπLu

1

s+ VaLu

Hv(s) = σv

√3VaπLv

(s+ Va√

3Lv

)(s+ Va

Lv

)2

Hw(s) = σw

√3VaπLw

(s+ Va√

3Lw

)(s+ Va

Lw

)2 ,

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 theairspeed of the vehicle. The Dryden models are typically implemented as-suming a constant nominal airspeed Va0 . The parameters for the Drydengust model are defined in MIL-F-8785C. Suitable parameters for low andmedium 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 passedthrough the Dryden filters to produce the gust components expressed in thevehicle 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 termscan be expressed mathematically as

Vbw =

uwvwww

= Rbv(φ, θ, ψ)

wnsweswds

+

uwgvwgwwg

,

where Rbv is the rotation matrix from the vehicle to the body frame givenin equation (??). From the components of the wind velocity Vb

w and the

Figure 4.5: The wind is modeled as a constant wind field plus turbulence. Theturbulence is generated by filtering white noise with a Dryden model.

ground velocity Vbg, we can calculate the body-frame components of the

airspeed vector as

Vba =

urvrwr

=

u− uwv − vww − ww

.

From the body-frame components of the airspeed vector, we can calculatethe airspeed magnitude, the angle of attack, and the sideslip angle according


to equation (??) as

Va =√u2r + v2

r + w2r

α = tan−1

(wrur

)β = sin−1

(vr√

u2r + v2

r + w2r

).

These expressions for Va, α, and β are used to calculate the aerodynamicforces and moments acting on the vehicle. The key point to understand isthat wind and atmospheric disturbances affect the airspeed, the angle of at-tack, and the sideslip angle. It is through these parameters that wind andatmospheric 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

Y (s) =as+ b

s2 + cs+ dU(s),

first put the system into control canonical form

x =

(−c −d1 0

)x+

(10

)u

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

xk+1 =

(1− Tsc −TsdTs 1

)xk +

(Ts0

)uk

yk =(a b

)xk.

For the Dryden model gust models, the input uk will be zero mean Gaussiannoise with unity variance.

Python code that implements the transfer function above is given as fol-lows:

import numpy as np

c l a s s t r a n s f e r f u n c t i o n :


def i n i t ( s e l f , Ts ) :s e l f . t s = Ts# s e t i n i t i a l c o n d i t i o n ss 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 space models 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 u p d a t e ( s e l f , u ) :’ ’ ’ Update s t a t e space model ’ ’ ’s e l f . s t a t e = s e l f . A @ s e l f . s t a t e + s e l f . B ∗ uy = s e l f . C @ s e l f . s t a t ere turn y

# i n i t i a l i z e t h e s y s t e mTs = 0 . 0 1 # s i m u l a t i o n s t e p s i z esys tem = 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 loops i m t i m e = 0 . 0whi le s i m t i m e < 1 0 . 0 :

u=np . random . randn ( ) # ( w h i t e n o i s e )y = sys tem . u p d a t e ( u ) # u pd a t e based on c u r r e n t i n p u ts i m t i m e += Ts # i n c r e m e n t t h e s i m u l a t i o n t i m e

4.5 CHAPTER SUMMARY

MODIFIED MATERIAL: The total forces on the MAV can be summarizedas follows:

fxfyfz

=

−mg sin θmg cos θ sinφmg cos θ cosφ

+

Tp00

+

1

2ρV 2

a S

CX(α) + CXq(α) c2Va

q

CY0 + CYββ + CYpb

2Vap+ CYr

b2Va

r

CZ(α) + CZq(α) c2Va

q

+

1

2ρV 2

a S

CXδe (α)δeCYδa δa + CYδr δr

CZδe (α)δe

, (4.8)


where

CX(α)4= −CD(α) cosα+ CL(α) sinα

CXq(α)4= −CDq cosα+ CLq sinα

CXδe (α)4= −CDδe cosα+ CLδe sinα (4.9)

CZ(α)4= −CD(α) sinα− CL(α) cosα

CZq(α)4= −CDq sinα− CLq cosα

CZδe (α)4= −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 andZ 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:

lmn

=1

2ρV 2

a S

b[Cl0 + Clββ + Clp

b2Va

p+ Clrb

2Var]

c[Cm0 + Cmαα+ Cmq

c2Va

q]

b[Cn0 + Cnββ + Cnp

b2Va

p+ Cnrb

2Var]

+1

2ρV 2

a S

b[Clδa δa + Clδr δr

]c[Cmδe δe

]b[Cnδa δa + Cnδr δr

]+

Qp00

. (4.10)


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 statewind along the NED inertial axes.

4.2 Add forces and moments to the dynamics of the MAV. The inputs tothe MAV should now be elevator, throttle, aileron, and rudder. Theaerodynamic coefficients are given in Appendix E.


4.3 Verify your simulation by setting the control surface deflections todifferent values. Observe the response of the MAV. Does it behave asyou 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 forcesand moments appear in the literature ranging from linear, uncoupled modelsto highly nonlinear models with significant cross coupling. In this section,we summarize the six-degree-of-freedom, 12-state equations of motion withthe quasi-linear aerodynamic and propulsion models developed in chapter 4.We characterize them as quasi-linear because the lift and drag terms arenonlinear in the angle of attack, and the propeller thrust is nonlinear in thethrottle command. For completeness, we will also present the linear modelsfor lift and drag that are commonly used. Incorporating the aerodynamicand propulsion models described in chapter 4 into equations (??)-(??), we

22 LINEAR DESIGN MODELS

get the following equations of motion:

pn = (cos θ cosψ)u+ (sinφ sin θ cosψ − cosφ sinψ)v

+ (cosφ sin θ cosψ + sinφ sinψ)w (5.1)pe = (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)

u = rv − qw − g sin θ +ρV 2

a S

2m

[CX(α) + CXq(α)

cq

2Va

+ CXδe (α)δe

]+Tp(δt, Va)

m(5.4)

v = pw − ru+ g cos θ sinφ+ρV 2

a S

2m

[CY0 + CYββ

+ CYpbp

2Va+ CYr

br

2Va+ CYδa δa + CYδr δr

](5.5)

w = qu− pv + g cos θ cosφ+ρV 2

a S

2m

[CZ(α)

+ CZq(α)cq

2Va+ CZδe (α)δe

](5.6)

φ = p+ q sinφ tan θ + r cosφ tan θ (5.7)

θ = q cosφ− r sinφ (5.8)

ψ = q sinφ sec θ + r cosφ sec θ (5.9)

p = Γ1pq − Γ2qr +1

2ρV 2

a Sb[Cp0 + Cpββ + Cpp

bp

2Va

+ Cprbr

2Va+ Cpδa δa + Cpδr δr

](5.10)

q = Γ5pr − Γ6(p2 − r2) +ρV 2

a Sc

2Jy

[Cm0 + Cmαα

+ Cmqcq

2Va+ Cmδe δe

](5.11)

r = Γ7pq − Γ1qr +1

2ρV 2

a Sb[Cr0 + Crββ + Crp

bp

2Va

+ Crrbr

2Va+ Crδa δa + Crδr δr

], (5.12)

MODIFIED MATERIAL: Further, it is common to model drag as a nonlin-

Linear Design Models 23

ear quadratic function of the lift as

CD(α) = CDp +(CL0 + CLαα)2

πeosAR,

where eos is the Oswald efficiency factor and AR is the aspect ratio of thewing.

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 componentof the groundspeed, Vg cos γ.

5.3 TRIM CONDITIONS

MODIFIED MATERIAL: For fixed-wing aircraft, the states are given by

x4= (pn, pe, pd, u, v, w, φ, θ, ψ, p, q, r)

> (5.13)

and the inputs are given by

u4= (δ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 derivethe transfer functions from throttle and pitch angle to airspeed. Toward thatobjective, note that if wind speed is zero, then Va =

√u2 + v2 + w2, which

implies that

Va =uu+ vv + ww

Va.

Using equation (??), we get

Va = 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

Va = cosα

rv − qw + r − g sin θ

+ρV 2

a S

2m

[−CD(α) cosα+CL(α) sinα+ (−CDq cosα+CLq sinα)

cq

2Va

+ (−CDδe cosα+ CLδe sinα)δe

]+

1

mTp(δt, Va)

+ sinα

qur − pvr + g cos θ cosφ

+ρV 2

a S

2m

[−CD(α) sinα−CL(α) cosα+ (−CDq sinα−CLq cosα)

cq

2Va

+ (−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

Va = rVa cosα sinβ − pVa sinα sinβ

− g cosα sin θ + g sinα cos θ cosφ

+ρV 2

a S

2m

[−CD0 − CDαα− CDq

cq

2Va− CDδe δe

]+

1

mTp(δt, Va) cosα+ dV1

= (rVa cosα− pVa sinα) sinβ

− g sin(θ − α)− g sinα cos θ(1− cosφ)

+ρV 2

a S

2m


cq

2Va− CDδe δe

]+

1

mTp(δt, Va) cosα+ dV1

= −g sin γ +ρV 2

a S

2m


cq

2Va− CDδe δe

]+

1

mTp(δt, Va) + dV2 , (5.16)

where

dV2 = (rVa cosα− pVa sinα) sinβ − g sinα cos θ(1− cosφ)

+1

mTp(δt, Va)(cosα− 1) + dV1 .

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 Vaand δ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

equation (5.16) by letting Va4= Va − V ∗a be the deviation of Va from trim,

θ4= θ−θ∗ be the deviation of θ from trim, and δt

4= δ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

˙Va = −g cos(θ∗ − α∗)θ (5.17)

+

ρV ∗a S

m

[−CD0 − CDαα∗ − CDδe δ

∗e

]+

1

m

∂Tp∂Va

(δ∗t , V∗a )

Va

+1

m

∂Tp∂δt

(δ∗t , V∗a )δt + dV

= −aV1 Va + aV2 δt − aV3 θ + dV , (5.18)

where

aV1 =ρV ∗a S

m

[CD0 + CDαα

∗ + CDδe δ∗e

]− 1

m

∂Tp∂Va

(δ∗t , V∗a )

aV2 =1

m

∂Tp∂δt

(δ∗t , V∗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

α = tan−1(wu

)Va =

√u2 + w2


Table 5.1: Lateral State-Space Model Coefficients.

Lateral Formula

YvρSbv∗

4mV ∗a

[CYpp

∗ + CYrr∗]

+ρSv∗

m

[CY0 + CYββ

∗ + CYδa δ∗a + CYδr δ

∗r

]+ρSCYβ

2m

√u∗2 + w∗2

Yp w∗ + ρV ∗a Sb4m CYp

Yr −u∗ + ρV ∗a Sb4m CYr

YδaρV ∗2a S

2m CYδaYδr

ρV ∗2a S2m CYδr

LvρSb2v∗

4V ∗a

[Cppp

∗ + Cprr∗]

+ρSbv∗[Cp0 + Cpββ

∗ + Cpδa δ∗a + Cpδr δ

∗r

]+ρSbCpβ

2

√u∗2 + w∗2

Lp Γ1q∗ + ρV ∗a Sb

2

4 CppLr −Γ2q

∗ + ρV ∗a Sb2

4 CprLδa

ρV ∗2a Sb2 Cpδa

LδrρV ∗2a Sb

2 CpδrNv

ρSb2v∗

4V ∗a

[Crpp

∗ + Crrr∗]

+ρSbv∗[Cr0 + Crββ

∗ + Crδa δ∗a + Crδr δ

∗r

]+ρSbCrβ

2

√u∗2 + w∗2

Np Γ7q∗ + ρV ∗a Sb

2

4 CrpNr −Γ1q

∗ + ρV ∗a Sb2

4 CrrNδa

ρV ∗2a Sb2 Crδa

NδrρV ∗2a Sb

2 CrδrA14 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

u = −qw − g sin θ +ρ(u2 + w2)S

2m

[CX0 + CXα tan−1

(wu

)+ CXδe δe

]+ρ√u2 + w2S

4mCXqcq + Tp(δt,

√u2 + w2) (5.19)

w = qu+ g cos θ +ρ(u2 + w2)S

2m

[CZ0 + CZα tan−1

(wu

)+ CZδe δe

]+ρ√u2 + w2S

4mCZqcq (5.20)

q =1

2Jyρ(u2 + w2)cS

[Cm0 + Cmα tan−1

(wu

)+ Cmδe δe

]+

1

4Jyρ√u2 + w2SCmqc

2q (5.21)

θ = 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)−1BU(s),


Table 5.2: Longitudinal State-Space Model Coefficients.

Longitudinal FormulaXu

u∗ρSm

[CX0 + CXαα

∗ + CXδe δ∗e

]−ρSw∗CXα

2m +ρScCXqu

∗q∗

4mV ∗a+

∂Tp∂u (δ∗t , V

∗a )

Xw −q∗ + w∗ρSm

[CX0 + CXαα

∗ + CXδe δ∗e

]+ρScCXqw

∗q∗

4mV ∗a+

ρSCXαu∗

2m +∂Tp∂w (δ∗t , V

∗a )

Xq −w∗ +ρV ∗a SCXq c

4m

Xδe

ρV ∗2a SCXδe2m

Xδt∂Tp∂δt

(δ∗t , V∗a )

Zu q∗ + u∗ρSm

[CZ0 + CZαα

∗ + CZδe δ∗e

]−ρSCZαw

∗

2m +u∗ρSCZq cq

∗

4mV ∗a

Zww∗ρSm

[CZ0 + CZαα

∗ + CZδe δ∗e

]+ρSCZαu

∗

2m +ρw∗ScCZq q

∗

4mV ∗a

Zq u∗ +ρV ∗a SCZq c

4m

ZδeρV ∗2a SCZδe

2m

Muu∗ρScJy

[Cm0 + Cmαα

∗ + Cmδe δ∗e

]−ρScCmαw

∗

2Jy+

ρSc2Cmq q∗u∗

4JyV ∗a

Mww∗ρScJy

[Cm0 + Cmαα

∗ + Cmδe δ∗e

]+ρScCmαu

∗

2Jy+

ρSc2Cmq q∗w∗

4JyV ∗a

MqρV ∗a Sc

2Cmq4Jy

Mδe

ρV ∗2a ScCmδe2Jy


and letting C =(0, 1

), the transfer function from δr to r is given by

r(s) =(0 1

)(sI −

(Yv

YrV ∗a cosβ∗

NvV∗a cosβ∗ Nr

))−1(

YδrV ∗a cosβ∗

Nδr

)δr(s)

=

(Nδrs+ (YδrNv −NδrYv)

s2 − (Yv +Nr)s+ (YvNr − YrNv)

)δr(s). (5.24)

5.6 CHAPTER SUMMARY


5.7 DESIGN PROJECT

MODIFIED MATERIAL:

5.1 Create a function that computes the trim state and the trim inputs fora desired airspeed of Va and a desired flight path angle of ±γ. Set theinitial condition in your simulation to the trim state and trim input,and verify that the aircraft maintains trim until numerical errors causeit to drift.

5.2 Create a function that computes the transfer function modelsdescribed in this chapter, linearized about the trim state and trim in-puts.

5.3 Create a function that computes the longitudinal and lateral statespace models described in this chapter, linearized around trim.

5.4 Compute eigenvalues of A lon and notice that one of the eigenvalueswill be zero and that there are two complex conjugate pairs. Using theformula

(s+ λ)(s+ λ∗) = s2 + 2<λs+ |λ|2 = s2 + 2ζωns+ ω2n,

extract ωn and ζ from the two complex conjugate pairs of poles. Thepair with the larger ωn correspond to the short-period mode, andthe pair with the smaller ωn correspond to the phugoid mode. Thephugoid and short-period modes can be excited by starting the simu-lation in a wings-level, constant-altitude trim condition, and placingan impulse on the elevator. By placing an impulse on the elevator,convince yourself that the eigenvalues of A lon adequately predictthe short period and phugoid modes.


5.5 Compute eigenvalues of A lat and notice that there is an eigenvalueat zero, a real eigenvalue in the right half plane, a real eigenvalue inthe left half plane, and a complex conjugate pair. The real eigenvaluein 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 bystarting 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 ofA lat adequately predict the roll, spiral-divergence, and dutch-rollmodes.



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. Formost flight maneuvers of interest, autopilots designed on the assumption ofdecoupled 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 autopilotsignificantly 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 simplefeedback loops in succession around the open-loop plant dynamics ratherthen designing a single (presumably more complicated) control system. Toillustrate how this approach can be applied, consider the open-loop systemshown in figure 6.1. The open-loop dynamics are given by the product ofthree transfer functions in series: P (s) = P1(s)P2(s)P3(s). Each of thetransfer functions has an output (y1, y2, y3) that can be measured and usedfor 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 areinterested in controlling the output y3. Instead of closing a single feedbackloop with y3, we will instead close feedback loops around y1, y2, and y3 insuccession, as shown in figure 6.2. We will design the compensators C1(s),C2(s), andC3(s) in succession. A necessary condition in the design processis that the inner loop has the highest bandwidth, with each successive loopbandwidth a factor of 5 to 10 times smaller in frequency.

Figure 6.1: Open-loop transfer function modeled as a cascade of three transferfunctions.

Examining the inner loop shown in figure 6.2, the goal is to design aclosed-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-looptransfer function y1(s)/r1(s) can be modeled as a gain of 1. This is depictedschematically in figure 6.3. With the inner-loop transfer function modeledas a gain of 1, design of the second loop is simplified because it includesonly 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 thenext loop so that it is a factor of W smaller than the preceding loop, whereS is typically in the range of 5-10. In this case, we require ωBW2 <

1W ωBW1

thus ensuring that the unity gain assumption on the inner loop is not violatedover the range of frequencies that the middle loop operates.

Figure 6.3: Successive loop closure design with inner loop modeled as a unitygain.

With the two inner loops operating as designed, y2(s)/r2(s) ≈ 1 and thetransfer function from r2(s) to y2(s) can be replaced with a gain of 1 forthe design of the outermost loop, as shown in figure 6.4. Again, there is abandwidth constraint on the design of the outer loop: ωBW3 < 1

W2ωBW2.

Because each of the plant models P1(s), P2(s), and P3(s) is first or secondorder, 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 longitudinaldynamics 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 aunity 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χ . Andthe dutch-roll mode is effectively damped using the yaw damper with gainkr. The idea with successive loop closure is that the gains are successivelychosen beginning with the inner loop and working outward. In particular,kdφ and kpφ are usually selected first, and kpχ and kiχ are usually chosensecond. The gain kr is selected independently of the other gains.

rc = 0<latexit sha1_base64="sY/S3B+9UzeqCPkQe8fnoqpU4Zs=">AAAB7nicbZC7SgNBFIbPxltcb1FLm8EgWIXdNNoEAzaWEcwFkhhmJ7PJkNnZZeasEJaAr2BjoYitb+B72Pk2Ti6FJv4w8PH/5zDnnCCRwqDnfTu5tfWNza38truzu7d/UDg8apg41YzXWSxj3Qqo4VIoXkeBkrcSzWkUSN4MRtfTvPnAtRGxusNxwrsRHSgRCkbRWk19z0iFeL1C0St5M5FV8BdQvPp0K48AUOsVvjr9mKURV8gkNabtewl2M6pRMMknbic1PKFsRAe8bVHRiJtuNht3Qs6s0ydhrO1TSGbu746MRsaMo8BWRhSHZjmbmv9l7RTDy24mVJIiV2z+UZhKgjGZ7k76QnOGcmyBMi3srIQNqaYM7YVcewR/eeVVaJRLvuVbr1gtw1x5OIFTOAcfLqAKN1CDOjAYwRO8wKuTOM/Om/M+L805i55j+CPn4wdADZBJ</latexit><latexit sha1_base64="z/2dypsx4m/Lm2xSi8WWnVAGI8I=">AAAB7nicbZDLSgMxFIbP1Fsdb1WXboJFcFVmurGbYsGNywr2Au1YMmmmDc1kQpIRytCHcONCERdufAPfw434NqaXhbb+EPj4/3PIOSeUnGnjed9Obm19Y3Mrv+3u7O7tHxQOj5o6SRWhDZLwRLVDrClngjYMM5y2paI4DjlthaOrad66p0qzRNyasaRBjAeCRYxgY62WuiOoirxeoeiVvJnQKvgLKF5+uFX59uXWe4XPbj8haUyFIRxr3fE9aYIMK8MIpxO3m2oqMRnhAe1YFDimOshm407QmXX6KEqUfcKgmfu7I8Ox1uM4tJUxNkO9nE3N/7JOaqJKkDEhU0MFmX8UpRyZBE13R32mKDF8bAETxeysiAyxwsTYC7n2CP7yyqvQLJd8yzdesVaGufJwAqdwDj5cQA2uoQ4NIDCCB3iCZ0c6j86L8zovzTmLnmP4I+f9BzGckb0=</latexit><latexit sha1_base64="z/2dypsx4m/Lm2xSi8WWnVAGI8I=">AAAB7nicbZDLSgMxFIbP1Fsdb1WXboJFcFVmurGbYsGNywr2Au1YMmmmDc1kQpIRytCHcONCERdufAPfw434NqaXhbb+EPj4/3PIOSeUnGnjed9Obm19Y3Mrv+3u7O7tHxQOj5o6SRWhDZLwRLVDrClngjYMM5y2paI4DjlthaOrad66p0qzRNyasaRBjAeCRYxgY62WuiOoirxeoeiVvJnQKvgLKF5+uFX59uXWe4XPbj8haUyFIRxr3fE9aYIMK8MIpxO3m2oqMRnhAe1YFDimOshm407QmXX6KEqUfcKgmfu7I8Ox1uM4tJUxNkO9nE3N/7JOaqJKkDEhU0MFmX8UpRyZBE13R32mKDF8bAETxeysiAyxwsTYC7n2CP7yyqvQLJd8yzdesVaGufJwAqdwDj5cQA2uoQ4NIDCCB3iCZ0c6j86L8zovzTmLnmP4I+f9BzGckb0=</latexit><latexit sha1_base64="HVFfYrvNas4GrfXg18Rb57AAalI=">AAAB7nicbZBNS8NAEIYn9avWr6pHL4tF8FSSXvQiFLx4rGA/oI1ls920SzebsDsRSuiP8OJBEa/+Hm/+G7dpDtr6wsLDOzPszBskUhh03W+ntLG5tb1T3q3s7R8cHlWPTzomTjXjbRbLWPcCargUirdRoOS9RHMaBZJ3g+ntot594tqIWD3gLOF+RMdKhIJRtFZXPzJyQ9xhtebW3VxkHbwCalCoNax+DUYxSyOukElqTN9zE/QzqlEwyeeVQWp4QtmUjnnfoqIRN36WrzsnF9YZkTDW9ikkuft7IqORMbMosJ0RxYlZrS3M/2r9FMNrPxMqSZErtvwoTCXBmCxuJyOhOUM5s0CZFnZXwiZUU4Y2oYoNwVs9eR06jbpn+d6tNRtFHGU4g3O4BA+uoAl30II2MJjCM7zCm5M4L86787FsLTnFzCn8kfP5A+Z2jow=</latexit>

kr<latexit sha1_base64="gDdB5A2KY4anfCNAK37VFc74rgA=">AAAB7HicbZDPSgMxEMZnq9Za/1U9egkWwVPZ7UWPBS8eW3DbQruUbDrbhmazS5IVytJn8OJBEa8+h8/gTXwZ0z8Hbf0g8OObGTLzhang2rjul1PY2t4p7pb2yvsHh0fHlZPTtk4yxdBniUhUN6QaBZfoG24EdlOFNA4FdsLJ7bzeeUCleSLvzTTFIKYjySPOqLGWPxnkajaoVN2auxDZBG8F1Uax9f0BAM1B5bM/TFgWozRMUK17npuaIKfKcCZwVu5nGlPKJnSEPYuSxqiDfLHsjFxaZ0iiRNknDVm4vydyGms9jUPbGVMz1uu1uflfrZeZ6CbIuUwzg5ItP4oyQUxC5peTIVfIjJhaoExxuythY6ooMzafsg3BWz95E9r1mme5ZdOow1IlOIcLuAIPrqEBd9AEHxhweIRneHGk8+S8Om/L1oKzmjmDP3LefwAPEJD+</latexit><latexit sha1_base64="RGvNkIiSrkKJODxAw8/FonS73aU=">AAAB7HicbZBNSwMxEIZn/ai1flU9egkWwVPZ7UWPBS8eW3DbQltKNp1tQ7PZJckKZelvEEFEEa/6Xzx6E/+M6cdBW18IPLwzQ2beIBFcG9f9ctbWNzZzW/ntws7u3v5B8fCooeNUMfRZLGLVCqhGwSX6hhuBrUQhjQKBzWB0Na03b1FpHssbM06wG9GB5CFn1FjLH/UyNekVS27ZnYmsgreAUjVX//54vH+v9YqfnX7M0gilYYJq3fbcxHQzqgxnAieFTqoxoWxEB9i2KGmEupvNlp2QM+v0SRgr+6QhM/f3REYjrcdRYDsjaoZ6uTY1/6u1UxNedjMuk9SgZPOPwlQQE5Pp5aTPFTIjxhYoU9zuStiQKsqMzadgQ/CWT16FRqXsWa7bNCowVx5O4BTOwYMLqMI11MAHBhzu4AmeHek8OC/O67x1zVnMHMMfOW8/hcGS1g==</latexit><latexit sha1_base64="RGvNkIiSrkKJODxAw8/FonS73aU=">AAAB7HicbZBNSwMxEIZn/ai1flU9egkWwVPZ7UWPBS8eW3DbQltKNp1tQ7PZJckKZelvEEFEEa/6Xzx6E/+M6cdBW18IPLwzQ2beIBFcG9f9ctbWNzZzW/ntws7u3v5B8fCooeNUMfRZLGLVCqhGwSX6hhuBrUQhjQKBzWB0Na03b1FpHssbM06wG9GB5CFn1FjLH/UyNekVS27ZnYmsgreAUjVX//54vH+v9YqfnX7M0gilYYJq3fbcxHQzqgxnAieFTqoxoWxEB9i2KGmEupvNlp2QM+v0SRgr+6QhM/f3REYjrcdRYDsjaoZ6uTY1/6u1UxNedjMuk9SgZPOPwlQQE5Pp5aTPFTIjxhYoU9zuStiQKsqMzadgQ/CWT16FRqXsWa7bNCowVx5O4BTOwYMLqMI11MAHBhzu4AmeHek8OC/O67x1zVnMHMMfOW8/hcGS1g==</latexit><latexit sha1_base64="U0a30+4YvkzOVSFcqtGZHPhgSIw=">AAAB7HicbZBNS8NAEIYn9avWr6pHL4tF8FSSXvRY8OKxgmkLbSib7aZdutmE3YlQQn+DFw+KePUHefPfuGlz0NYXFh7emWFn3jCVwqDrfjuVre2d3b3qfu3g8Oj4pH561jVJphn3WSIT3Q+p4VIo7qNAyfup5jQOJe+Fs7ui3nvi2ohEPeI85UFMJ0pEglG0lj8b5XoxqjfcprsU2QSvhAaU6ozqX8NxwrKYK2SSGjPw3BSDnGoUTPJFbZgZnlI2oxM+sKhozE2QL5ddkCvrjEmUaPsUkqX7eyKnsTHzOLSdMcWpWa8V5n+1QYbRbZALlWbIFVt9FGWSYEKKy8lYaM5Qzi1QpoXdlbAp1ZShzadmQ/DWT96EbqvpWX5wG+1WGUcVLuASrsGDG2jDPXTABwYCnuEV3hzlvDjvzseqteKUM+fwR87nDxkkjsw=</latexit>

Cr(s2 + 2!ns + !2

n)

(s + psp)(s2 + 2dr!ndrs + !2

ndr)

<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><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><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><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>

r<latexit sha1_base64="7VmVkfJ34GzMAfPk2ppqwcHbw3E=">AAAB6XicbZC7SgNBFIbPeo3rLWppMxgEq7CbRhsxYGMZxVwgWcLsZDYZMju7zJwVwhLwAWwsFLH1IXwPO9/GyaXQxB8GPv7/HOacE6ZSGPS8b2dldW19Y7Ow5W7v7O7tFw8OGybJNON1lshEt0JquBSK11Gg5K1UcxqHkjfD4fUkbz5wbUSi7nGU8iCmfSUiwSha60673WLJK3tTkWXw51C6+nQvHwGg1i1+dXoJy2KukElqTNv3UgxyqlEwycduJzM8pWxI+7xtUdGYmyCfTjomp9bpkSjR9ikkU/d3R05jY0ZxaCtjigOzmE3M/7J2htFFkAuVZsgVm30UZZJgQiZrk57QnKEcWaBMCzsrYQOqKUN7nMkR/MWVl6FRKfuWb71StQIzFeAYTuAMfDiHKtxADerAIIIneIFXZ+g8O2/O+6x0xZn3HMEfOR8/ZSiOsw==</latexit><latexit sha1_base64="E0THBuggfHWUnJr9vp8B8bg4diM=">AAAB6XicbZDLSgMxFIZP6q2Ot6pLN8EiuCoz3ehGLLhxWcVeoB1KJs20oZnMkGSEMvQN3LhQxG0fwvdwI76NmbYLbf0h8PH/55BzTpAIro3rfqPC2vrG5lZx29nZ3ds/KB0eNXWcKsoaNBaxagdEM8ElaxhuBGsnipEoEKwVjG7yvPXIlOaxfDDjhPkRGUgeckqMte6V0yuV3Yo7E14FbwHl6w/nKpl+OfVe6bPbj2kaMWmoIFp3PDcxfkaU4VSwidNNNUsIHZEB61iUJGLaz2aTTvCZdfo4jJV90uCZ+7sjI5HW4yiwlRExQ72c5eZ/WSc14aWfcZmkhkk6/yhMBTYxztfGfa4YNWJsgVDF7ayYDoki1Njj5EfwlldehWa14lm+c8u1KsxVhBM4hXPw4AJqcAt1aACFEJ7gBV7RCD2jN/Q+Ly2gRc8x/BGa/gBWt5An</latexit><latexit sha1_base64="E0THBuggfHWUnJr9vp8B8bg4diM=">AAAB6XicbZDLSgMxFIZP6q2Ot6pLN8EiuCoz3ehGLLhxWcVeoB1KJs20oZnMkGSEMvQN3LhQxG0fwvdwI76NmbYLbf0h8PH/55BzTpAIro3rfqPC2vrG5lZx29nZ3ds/KB0eNXWcKsoaNBaxagdEM8ElaxhuBGsnipEoEKwVjG7yvPXIlOaxfDDjhPkRGUgeckqMte6V0yuV3Yo7E14FbwHl6w/nKpl+OfVe6bPbj2kaMWmoIFp3PDcxfkaU4VSwidNNNUsIHZEB61iUJGLaz2aTTvCZdfo4jJV90uCZ+7sjI5HW4yiwlRExQ72c5eZ/WSc14aWfcZmkhkk6/yhMBTYxztfGfa4YNWJsgVDF7ayYDoki1Njj5EfwlldehWa14lm+c8u1KsxVhBM4hXPw4AJqcAt1aACFEJ7gBV7RCD2jN/Q+Ly2gRc8x/BGa/gBWt5An</latexit><latexit sha1_base64="6OOY3nqChLZrJnTzIcHQr3tHOk4=">AAAB6XicbZBNS8NAEIYn9avWr6pHL4tF8FSSXuyx4MVjFfsBbSib7aRdutmE3Y1QQv+BFw+KePUfefPfuGlz0NYXFh7emWFn3iARXBvX/XZKW9s7u3vl/crB4dHxSfX0rKvjVDHssFjEqh9QjYJL7BhuBPYThTQKBPaC2W1e7z2h0jyWj2aeoB/RieQhZ9RY60FVRtWaW3eXIpvgFVCDQu1R9Ws4jlkaoTRMUK0HnpsYP6PKcCZwURmmGhPKZnSCA4uSRqj9bLnpglxZZ0zCWNknDVm6vycyGmk9jwLbGVEz1eu13PyvNkhN2PQzLpPUoGSrj8JUEBOT/Gwy5gqZEXMLlCludyVsShVlxoaTh+Ctn7wJ3Ubds3zv1lqNIo4yXMAlXIMHN9CCO2hDBxiE8Ayv8ObMnBfn3flYtZacYuYc/sj5/AELoIz2</latexit>1

<latexit sha1_base64="7BARNB3AEjf56WHCl1/2ub4je0g=">AAAB6XicbZC7SgNBFIbPxluMt6ilzWAQbAy7abQzYGMZxVwgWcLs5GwyZHZ2mZkVwpI3EMFCEVvfyM7O17Bzcik08YeBj/8/hznnBIng2rjup5NbWV1b38hvFra2d3b3ivsHDR2nimGdxSJWrYBqFFxi3XAjsJUopFEgsBkMryZ58x6V5rG8M6ME/Yj2JQ85o8Zat2det1hyy+5UZBm8OZQuv5PHLwCodYsfnV7M0gilYYJq3fbcxPgZVYYzgeNCJ9WYUDakfWxblDRC7WfTScfkxDo9EsbKPmnI1P3dkdFI61EU2MqImoFezCbmf1k7NeGFn3GZpAYlm30UpoKYmEzWJj2ukBkxskCZ4nZWwgZUUWbscQr2CN7iysvQqJQ9yzduqVqBmfJwBMdwCh6cQxWuoQZ1YBDCAzzDizN0npxX521WmnPmPYfwR877DwkEj+4=</latexit><latexit sha1_base64="IJU2sXXlCh+hIwHh8Vb0L3ln+Mg=">AAAB6XicbZC7SgNBFIbPeo3xFrW0GQyCjWE3jXYGbCyj5AZJCLOTs8mQ2dllZlYIS95ABQtFbH0Fn8TOxvewc3IpNPGHgY//P4c55/ix4Nq47qeztLyyurae2chubm3v7Ob29ms6ShTDKotEpBo+1Si4xKrhRmAjVkhDX2DdH1yO8/otKs0jWTHDGNsh7UkecEaNtW5OvU4u7xbcicgieDPIX3zHD1+V9/tyJ/fR6kYsCVEaJqjWTc+NTTulynAmcJRtJRpjyga0h02Lkoao2+lk0hE5tk6XBJGyTxoycX93pDTUehj6tjKkpq/ns7H5X9ZMTHDeTrmME4OSTT8KEkFMRMZrky5XyIwYWqBMcTsrYX2qKDP2OFl7BG9+5UWoFQue5Ws3XyrCVBk4hCM4AQ/OoARXUIYqMAjgDp7g2Rk4j86L8zotXXJmPQfwR87bDyNHkYE=</latexit><latexit sha1_base64="IJU2sXXlCh+hIwHh8Vb0L3ln+Mg=">AAAB6XicbZC7SgNBFIbPeo3xFrW0GQyCjWE3jXYGbCyj5AZJCLOTs8mQ2dllZlYIS95ABQtFbH0Fn8TOxvewc3IpNPGHgY//P4c55/ix4Nq47qeztLyyurae2chubm3v7Ob29ms6ShTDKotEpBo+1Si4xKrhRmAjVkhDX2DdH1yO8/otKs0jWTHDGNsh7UkecEaNtW5OvU4u7xbcicgieDPIX3zHD1+V9/tyJ/fR6kYsCVEaJqjWTc+NTTulynAmcJRtJRpjyga0h02Lkoao2+lk0hE5tk6XBJGyTxoycX93pDTUehj6tjKkpq/ns7H5X9ZMTHDeTrmME4OSTT8KEkFMRMZrky5XyIwYWqBMcTsrYX2qKDP2OFl7BG9+5UWoFQue5Ws3XyrCVBk4hCM4AQ/OoARXUIYqMAjgDp7g2Rk4j86L8zotXXJmPQfwR87bDyNHkYE=</latexit><latexit sha1_base64="nCNoHcQNYdPwoNpMmEigAYg9mcE=">AAAB6XicbZBNT8JAEIan+IX4hXr0spGYeJG0XPRI4sUjGgsk0JDtsoUN222zOzUhDf/AiweN8eo/8ua/cYEeFHyTTZ68M5OdecNUCoOu++2UNja3tnfKu5W9/YPDo+rxSdskmWbcZ4lMdDekhkuhuI8CJe+mmtM4lLwTTm7n9c4T10Yk6hGnKQ9iOlIiEoyitR6uvEG15tbdhcg6eAXUoFBrUP3qDxOWxVwhk9SYnuemGORUo2CSzyr9zPCUsgkd8Z5FRWNugnyx6YxcWGdIokTbp5As3N8TOY2Nmcah7Ywpjs1qbW7+V+tlGN0EuVBphlyx5UdRJgkmZH42GQrNGcqpBcq0sLsSNqaaMrThVGwI3urJ69Bu1D3L926t2SjiKMMZnMMleHANTbiDFvjAIIJneIU3Z+K8OO/Ox7K15BQzp/BHzucP3dSM2A==</latexit>

rs

rs + 1<latexit sha1_base64="nDdTjYfytUOD/ZXe2e1MvdjBApo=">AAACBnicbZDLSsNAFIZP6q3WW9SlCINFEISSFEGXBUFcVrAXaEOYTCft0MkkzEyEErJy42O4deNCEbc+gzvfxukF0dYfBj7+cw5nzh8knCntOF9WYWl5ZXWtuF7a2Nza3rF395oqTiWhDRLzWLYDrChngjY005y2E0lxFHDaCoaX43rrjkrFYnGrRwn1ItwXLGQEa2P59mE3lJhkXY1TXyKV/xA6RW7u22Wn4kyEFsGdQblWuTp7BIC6b392ezFJIyo04Vipjusk2suw1Ixwmpe6qaIJJkPcpx2DAkdUednkjBwdG6eHwliaJzSauL8nMhwpNYoC0xlhPVDztbH5X62T6vDCy5hIUk0FmS4KU450jMaZoB6TlGg+MoCJZOaviAywyUWb5EomBHf+5EVoViuu4RuTRhWmKsIBHMEJuHAONbiGOjSAwD08wQu8Wg/Ws/VmvU9bC9ZsZh/+yPr4BnAImdk=</latexit><latexit sha1_base64="p7FlzB8RlrkinhcGs17cUmdW4Lw=">AAACBnicbZDLSsNAFIZPvNZ6i7oUYbAIglCSIuiyIoi4qmAv0IQwmU7aoZMLMxOhhKzc+AY+gxsXinTrM7jzbZxeEG39YeDjP+dw5vx+wplUlvVlLCwuLa+sFtaK6xubW9vmzm5DxqkgtE5iHouWjyXlLKJ1xRSnrURQHPqcNv3+5ajevKdCsji6U4OEuiHuRixgBCtteeaBEwhMMkfh1BNI5j+ETpCde2bJKltjoXmwp1Cqlq9On4YXNzXP/HQ6MUlDGinCsZRt20qUm2GhGOE0LzqppAkmfdylbY0RDql0s/EZOTrSTgcFsdAvUmjs/p7IcCjlIPR1Z4hVT87WRuZ/tXaqgnM3Y1GSKhqRyaIg5UjFaJQJ6jBBieIDDZgIpv+KSA/rXJROrqhDsGdPnodGpWxrvtVpVGCiAuzDIRyDDWdQhWuoQR0IPMAzvMKb8Wi8GO/GcNK6YExn9uCPjI9vEqebEg==</latexit><latexit sha1_base64="p7FlzB8RlrkinhcGs17cUmdW4Lw=">AAACBnicbZDLSsNAFIZPvNZ6i7oUYbAIglCSIuiyIoi4qmAv0IQwmU7aoZMLMxOhhKzc+AY+gxsXinTrM7jzbZxeEG39YeDjP+dw5vx+wplUlvVlLCwuLa+sFtaK6xubW9vmzm5DxqkgtE5iHouWjyXlLKJ1xRSnrURQHPqcNv3+5ajevKdCsji6U4OEuiHuRixgBCtteeaBEwhMMkfh1BNI5j+ETpCde2bJKltjoXmwp1Cqlq9On4YXNzXP/HQ6MUlDGinCsZRt20qUm2GhGOE0LzqppAkmfdylbY0RDql0s/EZOTrSTgcFsdAvUmjs/p7IcCjlIPR1Z4hVT87WRuZ/tXaqgnM3Y1GSKhqRyaIg5UjFaJQJ6jBBieIDDZgIpv+KSA/rXJROrqhDsGdPnodGpWxrvtVpVGCiAuzDIRyDDWdQhWuoQR0IPMAzvMKb8Wi8GO/GcNK6YExn9uCPjI9vEqebEg==</latexit><latexit sha1_base64="lJlMqHzIwMl/9rWXK/4QEudtzoI=">AAACBnicbZDLSsNAFIZPvNZ6i7oUYbAIglCSbnRZcOOygr1AE8JkOmmHTiZhZiKUkJUbX8WNC0Xc+gzufBunbRBt/WHg4z/ncOb8YcqZ0o7zZa2srq1vbFa2qts7u3v79sFhRyWZJLRNEp7IXogV5UzQtmaa014qKY5DTrvh+Hpa795TqVgi7vQkpX6Mh4JFjGBtrMA+8SKJSe5pnAUSqeKH0AVyi8CuOXVnJrQMbgk1KNUK7E9vkJAspkITjpXqu06q/RxLzQinRdXLFE0xGeMh7RsUOKbKz2dnFOjMOAMUJdI8odHM/T2R41ipSRyazhjrkVqsTc3/av1MR1d+zkSaaSrIfFGUcaQTNM0EDZikRPOJAUwkM39FZIRNLtokVzUhuIsnL0OnUXcN3zq1ZqOMowLHcArn4MIlNOEGWtAGAg/wBC/waj1az9ab9T5vXbHKmSP4I+vjG17gmFE=</latexit>

Figure 6.5: Autopilot for lateral control using successive loop closure.

The following sections describe the design of the lateral autopilot usingsuccessive 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 rollrate, 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 onthe 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

Hφ/φc(s) =kpφaφ2

s2 + (aφ1 + aφ2kdφ)s+ kpφaφ2.

Note that the DC gain is equal to one. If the desired response is given by thecanonical second-order transfer function

Hdφ/φc(s) =

ω2nφ

s2 + 2ζφωnφs+ ω2

nφ

,

then equating denominator polynomial coefficients, we get

ω2nφ

= kpφaφ2 (6.1)

2ζφωnφ = aφ1 + aφ2kdφ . (6.2)

Accordingly, the proportional and derivative gains are given by

kpφ =ω2nφ

aφ2

kdφ =2ζφωnφ − aφ1

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 φ hasbeen adequately tuned, then Hφ/φc ≈ 1 over the range of frequencies from0 to ωnφ . Under this condition, the block diagram of figure 6.5 can besimplified to the block diagram in figure 6.7 for the purposes of designingthe 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 commandedcourse χc. From the simplified block diagram, the transfer functions fromthe inputs χc and dχ to the output χ are given by

χ =g/Vgs

s2 + kpχg/Vgs+ kiχg/Vgdχ +

kpχg/Vgs+ kiχg/Vg

s2 + kpχg/Vgs+ kiχg/Vgχc. (6.3)

Note that if dχ and χc are constants, then the final value theorem impliesthat χ→ χc. The transfer function from χc to χ has the form

Hχ =2ζχωnχs+ ω2

nχ

s2 + 2ζχωnχs+ ω2nχ

. (6.4)

As with the inner feedback loops, we can choose the natural frequency anddamping of the outer loop and from those values calculate the feedbackgains kpχ and kiχ . Figure 6.8 shows the frequency response and the stepresponse for Hχ. Note that because of the numerator zero, the standardintuition 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

ω2nχ = g/Vgkiχ

2ζχωnχ = g/Vgkpχ .

38 AUTOPILOT DESIGN

0 2 4 6 8 10 120

0.2

0.4

0.6

0.8

1

1.2

1.4

(sec)

−60

−50

−40

−30

−20

−10

0

10

mag

nitu

de (d

B)

10−2 10−1 100 101 102 103−90

−60

−30

0

phas

e (d

eg)

frequency (rad/sec)

Figure 6.8: Frequency and step response for a second-order system with a transferfunction 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)

kiχ = ω2nχ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 andouter feedback loops. Adequate separation can be achieved by letting

ωnχ =1

Wχωnφ ,

where the separation Wχ is a design parameter that is usually chosen tobe greater than five. Generally, more bandwidth separation is better. Morebandwidth separation requires either slower response in the χ loop (lowerωnχ), or faster response in the φ loop (higher ωnφ). Faster response usuallycomes at the cost of requiring more actuator control authority, which maynot be possible given the physical constraints of the actuators.

The output of the course hold loop is

φc(t) = kpχ(χc(t)− χ(t)) + kiχ

∫ t

−∞(χc(τ)− χ(τ))dτ.

6.1.1.3 Yaw Damper using the Rudder

NEW MATERIAL: As discussed in the previous section, the aileron is usedto control the roll angle, and subsequently the course angle. In deriving thetransfer function from the roll angle to the course angle we assumed thatthe 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 angleis not measured directly. An alternative is to use the rudder to regulate theyaw rate to zero. The difficulty with doing so, however, is that in a turn wedo not desire the yaw rate to be zero and so a direct control implementationwould create competing objectives between the course controller and theyaw rate controller. The solution to this dilemma is to employ a so-calledwashout filter to only allow feedback from yaw rate to the rudder for highfrequency yaw rate. A wash-out filter is simply a high-pass filter.

Recall from Equation (5.24) that the transfer function from the rudder tothe yaw rate is given by

r(s) =

(Nδrs+ (YδrNv −NδrYv)

s2 − (Yv +Nr)s+ (YvNr − YrNv)

)δr(s). (6.7)

The yaw-damper control loop is shown in Figure 6.9, where pwo is the poleof 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 signalfor frequencies above pwo. The negative sign in the top path is due to thefact 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 rshown in Equation 6.7 correspond to the lightly-damped dutch roll mode.Essentially, the objective of the yaw damper is damp out the dutch-rollmode. Therefore, the pole of the washout filter should be selected to be be-low the natural frequency of the dutch-roll mode. The canonical expressions2 + 2ζdrωndrs+ω2

ndr= s2− (Yv +Nr)s+ (YvNr−YrNv) approximates

the natural frequency of the dutch-roll mode as

ωndr ≈√YvNr − YrNv.

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

r(s) =

(−(Nδrs+ (YδrNv −NδrYv))

s2 + (−Yv −Nr − krNδr )s+ (YvNr − YrNv − kr(YδrNv −NδrYv))

)rc(s),

where the closed-loop poles are given by

pyaw-damper =Yv +Nr + krNδr

2

± j

√(YvNr − YrNv − kr(Yδr −NvNδrYv)−

(Yv +Nr + krNδr

2

)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 ofclosed-loop poles have the same magnitude, i.e., when(

Yv +Nr + krNδr

2

)2

= (YvNr − YrNv − kr(Yδr −NvNδrYv)−(Yv +Nr + krNδr

2

)2

.

Simplifying, we get that kr is the positive root of the quadratic equation

N2δrk

2r + 2(NrNδr + YδrNv)kr + (Y 2

v +N2r + 2YrNv) = 0,

resulting in

kr = −NrNδr + YδrNv

N2δr

+

√√√√(NrNδr + YδrNv

N2δr

)2

−(Y 2v +N2

r + 2YrNv

N2δr

).

For the Aerosonde model given in the appendix, we get pwo = 0.45 andkr = 0.196.

Figure 6.10 shows the lateral-directional performance of the Aerosondeaircraft in response to a doublet roll command, both without the yaw damperand with the yaw damper. The upper plot shows the roll response of theaircraft to the doublet roll command. Without the yaw damper, the lightlydamped dutch-roll dynamics couple into the closed-loop roll dynamics caus-ing the roll dynamics to be less-damped than anticipated. The lower plotshows the yaw (heading) angle of the aircraft with and without the yawdamper. 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 ailerondeflection 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 adverseyaw and is caused by the negative yawing moment produced by the posi-tive aileron deflection as modeled by the Nδr aerodynamic coefficient. Thisnon-minimum phase zero further aggravates the negative effect of the lightlydamped dutch-roll mode, making the implementation of the yaw damper anecessity for well-behaved flight.

0 1 2 3 4 5 6 7 8 9 10

time (s)

-30

-20

-10

0

10

20

30

roll

angl

e (d

eg)

desiredw/o yaw damperwith yaw damper

0 1 2 3 4 5 6 7 8 9 10

time (s)

-10

0

10

20

30

40

yaw

ang

le (

deg)

w/o yaw damperwith yaw damper

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 thelateral-directional control of the aircraft is that it may be necessary to takethe speed of the dutch-roll mode into consideration when using successive-loop closure to set the bandwidth of the outer-loop course control. Ratherthan keeping the bandwidth of the outer course loop at least a factor of tenbelow the bandwidth of the roll loop. Better performance may result fromsetting the outer course-loop bandwidth at least a factor of ten slower thanthe 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

δr(s) = kr

(s

s+ pwo

)r(s).

Converting to the time domain results in the differential equation

δr = −pwo δr + krr.

Integrating once gives

δr(t) = −pwo∫ t

−∞δr(σ)dσ + krr(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 tothe state-space equations

ξ = −pwo ξ + krr

δr = −pwo ξ + krr.

Using a Euler approximation for the derivative, the discrete-time equationsare

ξk = −pwo ξk−1 + krrk−1

δrk = −pwo ξk + krrk.

The corresponding Python code is shown below.

c l a s s yawDamper :def i n i t ( s e l f , k r , p wo , Ts ) :

s e l f . x i = 0 .s e l f . Ts = Tss e l f . k r = k rs e l f . p wo = p wo

Autopilot Design 43

def u p d a t e ( s e l f , u ) :s e l f . x i = s e l f . x i

+ s e l f . Ts ∗ (− s e l f . p wo ∗ s e l f . x i+ s e l f . k r ∗ r )

d e l t a r = −p wo ∗ s e l f . x i + s e l f . k r ∗ rre turn 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 pitchrate damping for the innermost loop. The pitch attitude is regulated with theproportional gain kpθ . The altitude is regulated with the proportional andintegral gains kph and kih . The airpseed is regulated using the proportionaland integral gains kpVa and kiVa . The idea with successive loop closure isthat the gains are successively chosen beginning with the inner loop andworking outward. In particular, kdθ and kpθ are usually selected first, andkph and kih are usually chosen second. The gains kpVa and kiVa are selectedindependently of the other gains.

VatV ca

kpV

kiV

s

Figure 6.12: Autopilot for longitudinal control using successive loop closure.

The following sections describe the design of the longitudinal autopilotusing successive loop closure.

6.1.2.1 Pitch Hold using the Elevator

MODIFIED MATERIAL: The pitch attitude hold loop is similar to theroll 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 givenby

Hθ/θc(s) =kpθaθ3

s2 + (aθ1 + kdθaθ3)s+ (aθ2 + kpθaθ3). (6.9)

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 transferfunction

Hdθ/θc =

KθDCω2nθ

s2 + 2ζθωnθs+ ω2

nθ

,

then, equating denominator coefficients, we get

ω2nθ

= aθ2 + kpθaθ3 (6.10)

2ζθωnθ = aθ1 + kdθaθ3 (6.11)

KθDCω2nθ

= kpθaθ3 . (6.12)

Solving for kpθ , kdθ , and KθDC we get

kpθ =ω2nθ− aθ2aθ3

kdθ =2ζ

θωnθ − aθ1aθ3

KθDC =kpθaθ3ω2nθ

,

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 limitthe 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 developmentof 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 designedand that θ ≈ KθDCθ

c, the altitude hold loop using the commanded pitch canbe 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

h(s) =

KθDCVakph

(s+

kihkph

)s2 +KθDCVakphs+KθDCVakih

hd(s)

+

(s

s2 +KθDCVakphs+KθDCVakih

)dh(s),

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 independentof aircraft parameters and is dependent only on the known airspeed. Thegains 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

ωnh =1

Whωnθ ,

where the bandwidth separation Wh is a design parameter that is usuallybetween five and fifteen. If the desired response of the altitude hold loop is

46 AUTOPILOT DESIGN

given by the canonical second-order transfer function

Hdh/hc =

ω2nh

s2 + 2ζhωnhs+ ω2

nh

,

then, equating denominator coefficients, we get

ω2nh

= KθDCVakih

2ζhωnh = KθDCVakph .

Solving these expressions for kih and kph , we get

kih =ω2nh

KθDCVa(6.13)

kph =2ζ

hωnh

KθDCVa. (6.14)

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

θc(t) = kph (hc(t)− h(t)) + kih

∫ t

−∞(hc(τ)− h(τ))dτ.

6.1.2.3 Airspeed Hold using Throttle

MODIFIED MATERIAL: Using PI control for the airspeed loop resultsin the closed-loop system is shown in figure 6.15. If we use proportional

VatV ca

kpV

kiV

s

Figure 6.15: Airspeed hold using throttle.

control, then

Va(s) =

(aV2kpV

s+ (aV1 + aV2kpV )

)V ca (s) +

(1

s+ (aV1 + aV2kpV )

)dV (s).

Autopilot Design 47

Note that the DC gain is not equal to one and that step disturbances are notrejected. If, on the other hand, we use proportional-integral control, then

Va =

(aV2(kpV s+ kiV )

s2 + (aV1 + aV2kpV )s+ aV2kiV

)V ca

+

(1

s2 + (aV1 + aV2kpV )s+ aV2kiV

)dV .

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 canbe determined using the same technique we have used previously. Equatingthe closed-loop transfer function denominator coefficients with those of acanonical second-order transfer function, we get

ω2nV

= aV2kiV

2ζV ωnV = aV1 + aV2kpV .

Inverting these expressions gives the control gains

kiV =ω2nV

aV2(6.15)

kpV =2ζV ωnV − aV1

aV2. (6.16)

The design parameters for this loop are the damping coefficient ζV and thenatural frequency ωnV .

Note that since V ca = V c

a − V ∗a and Va = Va − V ∗a , the error signal infigure 6.15 is

e = V ca − Va = V c

a − Va.Therefore, the control loop shown in figure 6.15 can be implemented with-out knowledge of the trim velocity V ∗a . 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 thoughtof as a step disturbance, and the integrator will wind up to reject the distur-bances.

The throttle command is therefore

δt(t) = kpV (V ca (t)− Va(t)) + kiV

∫ t

−∞(V ca (τ)− 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 as np

c l a s s p i d c o n t r o l :def i n i t ( s e l f , kp = 0 . 0 , k i = 0 . 0 , kd = 0 . 0 , Ts = 0 . 0 1 ,

s igma = 0 . 0 5 , l i m i t = 1 . 0 ) :s e l f . kp = kps e l f . k i = k is e l f . kd = kds e l f . Ts = Tss e l f . l i m i t = l i m i ts e l f . i n t e g r a t o r = 0 . 0s e l f . e r r o r d e l a y 1 = 0 . 0s e l f . e r r o r d o t d e l a y 1 = 0 . 0# g a i n s f o r d i f f e r e n t i a t o rs e l f . a1 = ( 2 . 0 ∗ s igma − Ts ) / ( 2 . 0 ∗ s igma + Ts )s e l f . a2 = 2 . 0 / ( 2 . 0 ∗ s igma + Ts )

def u p d a t e ( s e l f , y r e f , y , r e s e t f l a g = F a l s e ) :i f r e s e t f l a g == True :

s e l f . i n t e g r a t o r = 0 . 0s e l f . e r r o r d e l a y 1 = 0 . 0s e l f . y d o t = 0 . 0s e l f . y d e l a y 1 = 0 . 0s e l f . y d o t d e l a y 1 = 0 . 0

# compute t h e e r r o re r r o r = y r e f − y# u pd a t e t h e i n t e g r a t o r u s i n g t r a p a z o i d a l r u l es e l f . i n t e g r a t o r = s e l f . i n t e g r a t o r \

+ ( 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 )# u pd a t e t h e d i f f e r e n t i a t o re 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 lu = s e l f . kp ∗ e r r o r \

+ s e l f . k i ∗ 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 tu s a t = s e l f . s a t u r a t e ( 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 t o keep u o u t o f s a t u r a t i o ni f np . abs ( s e l f . k i ) > 0 . 0 0 0 1 :

s e l f . i n t e g r a t o r = s e l f . i n t e g r a t o r \+ ( 1 . 0 / s e l f . k i ) ∗ ( u s a t − u )

# u pd a t e t h e d e l a y e d v a r i a b l e ss e l f . e r r o r d e l a y 1 = e r r o r

Autopilot Design 49

s e l f . e r r o r d o t d e l a y 1 = e r r o r d o tre turn 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 = F a l s e ) :

i f r e s e t f l a g == True :s e l f . i n t e g r a t o r = 0 . 0s e l f . e r r o r d e l a y 1 = 0 . 0

# compute t h e e r r o re r r o r = y r e f − y# u pd a t e t h e i n t e g r a t o r u s i n g t r a p a z o i d a l r u l es e l f . i n t e g r a t o r = s e l f . i n t e g r a t o r \

+ ( 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 lu = s e l f . kp ∗ e r r o r \

+ s e l f . k i ∗ s e l f . i n t e g r a t o r \− s e l f . kd ∗ ydo t

# s a t u r a t e PID c o n t r o l a t l i m i tu s a t = s e l f . s a t u r a t e ( 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 t o keep u o u t o f s a t u r a t i o ni f np . abs ( s e l f . k i ) > 0 . 0 0 0 1 :

s e l f . i n t e g r a t o r = s e l f . i n t e g r a t o r \+ ( 1 . 0 / s e l f . k i ) ∗ ( u s a t − u )

s e l f . e r r o r d e l a y 1 = e r r o rre turn u s a t

def s a t u r a t e ( s e l f , u ) :# s a t u r a t e u a t +− s e l f . l i m i ti f u >= s e l f . l i m i t :

u s a t = s e l f . l i m i te 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 te l s e :

u s a t = ure turn 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 bookis the number of required loops and the state machine for altitude controlshown in page 113. In this note we will describe a simplier scheme based onthe energy states of the system. The ideas in this supplement are motivatedby [?, ?].

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 stronglycoupled. 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. Bothof these quantities have a significant effect on both altitude and airspeed.Rather than attempt to decouple these effects by operating different loops indifferent 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 = 12m ‖v‖

2. If weuse the velocity of the aircraft relative to the air mass, then K = 1

2mV2a .

The reference kinetic energy is given by Kref = 12m(V c

a )2. Therefore, theerror in kinetic energy is given by

Kerror4= Kref −K =

1

2m((V c

a )2 − V 2a ).

The potential energy of a body with mass m is given by U = U0 + mghwhere U0 is the potential of ground level when the altitude h = 0. Thereference potential energy is given by Uref = U0 + mghc. Therefore, theerror 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 usinga PI controller:

δt(t) = kpEE(t) + kiE

∫ t

−∞E(τ) dτ.

The pitch command is used to regulate the energy balance using a PI con-troller:

θc(t) = kpBB(t) + kiB

∫ t

−∞B(τ) dτ.

As a matter of practical consideration, the TECS scheme works better forlarge deviations in altitude if the altitude error in (6.17) is saturated as

Uerror = mg sathe(hc − h),

Autopilot Design 51

where he > 0 is the largest altitude error used to compute Uerror, and sat isthe 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

J(x0) = minu(t),t≥0

∫ ∞0

x>(τ)Qx(τ) + u>(τ)Ru(τ)dτ.

If (A,B) is controllable, and (A,Q1/2) is observable, then a unique optimalcontrol exists and is given in linear feedback form as

ulqr(t) = −Klqrx(t),

where the LQR gain is given by

Klqr = R−1B>P,

and where P is the symmetric positive definite solution of the AlgebraicRiccati Equation

PA+A>P +Q− PBR−1B>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 bediagonal matrices

Q =

q1 0 . . . 00 q2 . . . 0...

. . ....

0 0 . . . qn

R =

r1 0 . . . 00 r2 . . . 0...

. . ....

0 0 . . . rm

,

where n is the number of states, m is the number of inputs, and qi ≥ 0ensures Q is positive semi-definite, and ri > 0 ensure R is positive definite.

For the lateral and longitudinal autopilots, we have seen that we needintegral 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 todrive z to a reference signal zc and further suppose that zc is a step, i.e.,zc = 0. The first step is to augment the state with the integrator

xI =

∫ t

−∞(z(τ)− zc) dτ.

Defining the augmented state as ξ = (x>, x>I )>, results in the augmentedstate space equations

ξ = Aξ + Bu,

where

A =

(A 0H 0

)B =

(B0

).

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 ofmotion are given by

xlat = Alatxlat +Blatulat,

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

xI =

∫(χ− χc)dt.

Since χ ≈ ψ, we approximate xI as

xI =

∫(Hlatxlat − χc)dt,

where Hlat = (0, 0, 0, 0, 1).The augmented lateral state equations are therefore

ξlat = Alatξlat + Blatulat,

Autopilot Design 53

where

Alat =

(Alat 0Hlat 0

)Blat =

(Blat

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

xlon = Alonxlon +Blonulon,

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 altitudehc, and the airspeed Va to commanded airspeed V c

a . Therefore, we augmentthe state with

xI =

( ∫(h− hc)dt∫

(Va − V ca )dt

)=

∫ (Hlonxlon −

(hc

V ca

))dt,

where

Hlon =

(0 0 0 0 1u∗

Vaw∗

Va0 0 0

).

The augmented longitudinal state equations are therefore

ξlon = Alonξlon + Blonulon,

where

Alon =

(Alon 0Hlon 0

)Blon =

(Blon

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 longitudinalautopilots, as described in this chapter. You can use either successive loopclosure, total energy control, or LQR.

6.1 Implement the longitudinal autopilot and tune the gains. The input thelongitudinal autopilot is the commanded airspeed and the commandedaltitude. To make the process easier, you may want to initialize thesystem to trim inputs and then tune airspeed control first, followed bythe pitch loop, and then the altitude loop.

6.2 Implement the lateral autopilot. The input to the lateral autopilot isthe 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 commandcourse 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

yaccel,x =ρV 2

a S

2m

[CX(α) + CXq(α)

cq

2Va+ CXδe (α)δe

]+ Tp(δt, Va) + ηaccel,x

yaccel,y =ρV 2

a S

2m

[CY0 + CYββ + CYp

bp

2Va+ CYr

br

2Va

+ CYδa δa + CYδr δr

]+ ηaccel,y (7.1)

yaccel,z =ρV 2

a S

2m

[CZ(α) + CZq(α)

cq

2Va+ CZδe (α)δe

]+ ηaccel,z.

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 anothersatellite narrows the possible positions to the the intersection of the the twospheres around the satellites, which is a circle. A third measurement nar-rows the possible positions to the intersection of the three spheres aroundthe satellites, which is two points. The point closest to the surface of theearth is taken as the position. To resolve position in three dimensions witha receiver clock offset error, at least four measurements are needed. The ge-ometry associated with the pseudo-range measurements from four differentsatellites form a system of four nonlinear algebraic equations in four un-knowns: latitude, longitude, and altitude of the GPS receiver, and receiverclock time offset [?].

satellite 1

satellite 2satellite 3

satellite 4

r4

r3r2

r1

receiver

Figure 7.1: Pseudo-range measurements from four satellites used to trilaterate theposition 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

Vg =√V 2n + V 2

e (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 inthis section is to produce estimates of the Euler angles φ, θ, and ψ, whenthey 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 thebody 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 Gaussianrandom variable with known variance. Integrating the output of the rategyros gives

φgyro(t)4=

∫ t

−∞ygyro,x(τ)dτ = φ(t) +

∫ t

−∞(bp(τ) + ηp(τ)) dτ

= φ(t) + βφ(t)

θgyro(t)4=

∫ t

−∞ygyro,y(τ)dτ = θ(t) +

∫ t

−∞(bq(τ) + ηq(τ)) dτ

= θ(t) + βθ(t)

ψgyro(t)4=

∫ t

−∞ygyro,z(τ)dτ = ψ(t) +

∫ 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 andpitch angles, or from a magnetometer, in the case of the yaw angle. Lettingvb = (u, v, w)>, using equation (??) for the rotation matrix in terms of theEuler 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 Gaussiannoise. 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

φaccel(t)4= tan−1

(yaccel,y − byyaccel,z − bz

)= tan−1

(v + ru− pw + g cos θ sinφ+ ηyw + pv − qu+ g cos θ cosφ+ ηz

)= φ(t) + νφ(t) + ηφ

θaccel(t)4= sin−1

(yaccel,x − bx

g

)= sin−1

(u+ qw − rv + g sin θ + ηx

g

)= θ(t) + νθ(t) + ηθ.

The approximation of φ and θ given by the accelerometers will be mostaccurate 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 signalsthat are linked through the dynamics to the true roll and pitch angles, φacceland θaccel are only good approximations at low frequencies. Therefore, weassume that νφ and νθ are signals with high frequency content. We note thatthis assumption is violated in many flight scenarios for fixed wing aircraftlike when the vehicle is in a fixed loiter configuration where νφ and νθ areconstant.

In the case of the magnetometer, the measurement is first processed bysubtracting any known biases, rotating to remove the inclination and decli-nation angles, and then rotating to the body level frame as

mv1 = Rv1b (φ, θ)R>z (δ)R>y (ι)(ymag − bmag) (8.1)

= Rv1b (φ, θ)Ry(ι)Rz(δ)

(mb + νmag + ηmag

)(8.2)

=

cθ sθsφ sθcφ0 cφ −sφ−sθ cθsφ cθcφ

cι 0 sι0 1 0−sι 0 cι

cδ sδ 0−sδ cδ 0

0 0 1

(8.3)

·(mb + νmag + ηmag

), (8.4)

where δ is the declination angle, ι is the inclination angle, η is zero meanGaussian noise, and ν represents all other magnetic interference that comesfrom, 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(mv1y ,m

v1x )

= ψ(t) + νψ(t) + ηψ.

64 STATE ESTIMATION

We will assume that νψ is a high frequency signal, and therefore, that a lowpass filtered version of ψmag is essentially ψ over low frequencies.

We will present the derivation of the simple complementary filter for theroll angle φ. The derivation for the pitch and yaw angles is similar. Theintuitive idea of the complementary filter is to estimate the roll angle byblending a high pass version of φgyro and a low pass version of φaccel as

φ = HHPF (s)φgyro +HLPF (s)φaccel

where HHPF (s) is a high pass filter and HLPF (s) is a low pass filter. Since

φ = HHPF (s)φgyro +HLPF (s)φaccel

= HHPF (s) [φ+ βφ] +HLPF (s) [φ+ νφ + ηφ]

= [HHPF (s) +HLPF (s)]φ+HHPF (s)βφ +HLPF (s) [νφ + ηφ] ,

if the frequency content of βφ is below the cut-off frequency of HHPF andthe frequency content of νφ + ηφ is above the cut-off frequency of HLPF

then

φ = [HHPF (s) +HLPF (s)]φ,

which implies that the filters HHPF and HLPF need to be selected so that

HHPF (s) +HLPF (s) = 1.

For example, ifHLPF (s) =kps+kp

then we need to selectHHPF (s) = ss+kp

.The block diagram for a naive implementation of the complementary filteris 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 hasseveral drawbacks that include the need to implement two filters and also thefact that bias rejection properties are not obvious. A better implementationstrategy is to use a feedback configuration, as explained below. Considerthe feedback loop show in figure 8.2. Following standard block diagrammanipulation we get that

y(s) =

(1

1 + PC

)do(s) +

(P

1 + PC

)di(s) +

(PC

1 + PC

)r(s)

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

S =1

1 + PC

is called the sensitivity function, and the tranfer function

T =PC

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 frequencyand small (<< 1) at high frequency, then the sensitivity function S(s) isa high pass filter and the complementary sensitivity function T (s) is a lowpass filter. Therefore, the feedback structure can be used to implement acomplementary 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

S(s) =1

1 + PC=

s

s+ kp=

1

1 +kps

we set P (s) = 1/s and C(s) = kp as shown in figure 8.4. Figure 8.4 alsoindicates that φgyro is the integral of the measurement ygyro,x. Clearly, theoutput disturbance of φgyro is equivalent to an input disturbance of ygyro,xas shown in figure 8.5.

66 STATE ESTIMATION



As mentioned above, the gyro measurement contains the true roll ratep plus a nearly constant bias bφ. We can use the final value theorem todetermine the response of the feedback system shown above to a constantbφ as the input disturbance. The relevant transfer function is

φ(s) =P

1 + PCbφ(s).

The final value theorem gives

φss = limt→∞

φ(t)

= lims→0

s

(P

1 + PC

)b

s

= lims→0

bP

1 + PC.

When P = 1s and C = kp we get

φss = lims→0

bs

1 +kps

= lims→0

b

s+ kp

=b

kp.

State Estimation 67

Therefore, the effect of the bias can be reduced by increasing kp but cannotbe eliminated. However, using a PI structure forC(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


φss = lims→0

bs

1 +kp+ki/s

s

= lims→0

bs

ss + kps+ ki

= 0.

From the block diagram, we see that the differential equations that describethe 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 theintegrator to emphasize the fact that the role of the integrator is to estimatethe bias and to subtract the bias from the gyro measurement.

We also note that a Lyapunov argument can be used to prove the stabilityof the complementary filter given in equation (8.5) in the absence of noise.Indeed, consider the Lyapunov function candidate

V =1

2(φ− φ)2 +

1

2ki(bφ − bφ)2.

68 STATE ESTIMATION

Differentiating and using the system and filter dynamics gives

V = (φ− φ)(φ− ˙φ) +

1

ki(bφ − bφ)(bφ − ˙

bφ)

= (φ− φ)(p− (ygyro,x − b)− kp(φaccel − φ))

− 1

ki(bφ − bφ)(−ki)(φaccel − φ)

= (φ− φ)((ygyro,x − b)− (ygyro,x − b)− kp(φ+ νφ − φ))

+ (bφ − bφ)(φ+ νφ − φ)

≤ −kp(φ− φ)2 + γ|νφ|∥∥∥∥(φ− φb− b

)∥∥∥∥ .If νφ = 0 then LaSalle’s invariance principle can be used to show asymptoticconvergence of the complementary filter. For non-zero νφ, the filter error isbounded 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)− z(t− Ts)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:

• φaccel[n] = tan−1(yaccel,y [n]−byyaccel,z [n]−bz

)• θaccel[n] = sin−1

(yaccel,x[n]−bx

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]− Tskieφ[n]

• bθ[n] = bθ[n− 1]− Tskieθ[n]

• bψ[n] = bψ[n− 1]− Tskieψ[n]

Step 4: Update Euler angle estimates:

• φ[n] = φ[n− 1] = Ts

((ygyro,x[n]− bφ[n]) + kpeφ[n]

)• θ[n] = θ[n− 1] = Ts

((ygyro,y[n]− bθ[n]) + kpeθ[n]

)• ψ[n] = ψ[n− 1] = Ts

((ygyro,z[n]− bψ[n]) + kpeψ[n]

)RWB: Change the previous into a python implementation.

70 STATE ESTIMATION

Algorithm 2 Continuous-discrete Observer1: 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+

(ToutN

)f(x, u)

6: end for7: if A measurement has been received from sensor i then Measurement

Update8: 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 withcovariance 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 Exξ> as

Exξ> = Ee(A−LC)tx0ξ

>(t) +

∫ t

0e(A−LC)(t−τ)ξ(τ)ξ>(τ) dτ

−∫ t

0e(A−LC)(t−τ)Lη(τ)ξ>(τ) dτ

=

∫ t

0e(A−LC)(t−τ)Qδ(t− τ) dτ

=1

2Q,

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 + PA> +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 + PA> +Q.

One way to approximate this equation numerically is to use the Euler ap-proximation

Pk+1 = Pk + Ts

(APk + PkA

> +Q).

However, given the numerical inaccuracy due to the Euler approximation, Pmay not remain positive definite. We can solve this problem by discretizingin a different way. Recall that the estimation error is given by

x(t) = eA(t−t0)x(t0) +

∫ t

t0

eA(t−τ)ξ(τ)dτ.

Letting t = kTs and using the notation xk = x(kTs), and assuming thatξ(τ) is held constant over each time interval and that ξk = ξ(kTs), we get

xk+1 = eATs xk + (

∫ Ts

0eAτdτ)ξk.

Using the approximation

Ad = eATs ≈ I +ATs +A2T2s

2

Bd = (

∫ Ts

0eAτdτ) ≈

∫ Ts

0Idτ = TsI,

we get

xk+1 = Adxk + Tsξk.

Defining the error covariance as

Pk = Exkx>k ,

72 STATE ESTIMATION

the covariance update can be approximated as

Pk+1 = Exk+1x>k+1

= E(Adxk + Tsξk)(Adxk + Tsξk)>= EAdxkx>k Ad + Tsξkx

>k A>d +Adxk + Tsξkξ

>k + T 2

s ξkξ>k

= AdExkx>k A>d + TsEPξkx>k A>d + TsAdExkξ>k + T 2sEξkξ>k

= AdPkA>d + T 2

sQ,

where we have made the assumption that the discrete estimation error andthe process noise are uncorrelated. Note that the discrete evolution equation

Pk+1 = AdPkA>d + T 2

sQ

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 errormay 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 forcovariance 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 positivedefinite. Therefore, rather than using equation (8.8), it is more numericallystable 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 Filter1: 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 Step5: Tp = Tout/N

6: x = x+(ToutN

)f(x, u)

7: A = ∂f∂x (x, u)

8: Ad = I +ATp +A2T 2p

9: P = AdPA>d + T 2

pQ10: end for11: if Measurement has been received from sensor i then Measurement

Update12: Ci = ∂hi

∂x (x, u[n])

13: Li = PC>i (Ri + CiPC>i )−1

14: P = (I − LiCi)P (I − LiCi)> + LiRiL>i

15: x = x+ Li (yi[n]− h(x, u[n])).16: end if

Define the innovation sequence

vk = yk − Cx−k= Cxk + ηk − Cx−k= Cx−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 = Evkv>k = E(ηk + Cx−k )(ηk + Cx−k )>= R+ CP−k C

>,

since ηk and x−k are uncorrelated.Note that the Kalman gain can be written as

Lk = P−k C>S−1

k .

The random variable

zk = S− 1

2k vk

74 STATE ESTIMATION

is therefore an m-dimensional Gaussian random variable with zero meanand covariance of I . Therefore the random variable wk = z>k zk is a χ2 (chi-squared) random variable with m degrees-of-freedom. We can thereforecompute the probability that wk comes from a chi-squared distribution withm- degrees-of-freedom, and only do a measurement update if the probabilityis above a threshold.

For example, in Python, implementation of measurement gating, wherethe measurement update is only performed when there is a 99% likelihoodthat the measurement is not an outlier is given by

import numpy as npfrom s c i p y import s t a t sdef 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 u p d a t e sh = s e l f . h ( s e l f . xha 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 . xha t , measurement , s t a t e )y = np . a r r a y ( [ [ measurement . 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 . c h i 2 . s f ( ( y − h ) . T @ S i n v @ ( y − h ) , d f =3) >0.01:

L = s e l f . P @ C . T @ S i n vtmp = np . eye ( 2 ) − L @ Cs e l f . P = tmp@self . P@tmp . T + L@self . R accel@L . Ts e l f . x h a t = s e l f . x h a t + L @ ( y − h )

NEW MATERIAL: The function stats.chi2.sf(x, df=3) is thesurvival function for the χ2 distribution with df degrees-of-freedom andreturns the area of the tail of the probability density function above x. Inthis implementation, the survival function must be computed for every timestep. 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 classconstructor 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 i n v @ ( 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

s e l f . a c c e l t h r e s h o l d = c h i 2 i n v ( 0 . 9 9 , 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 theJacobians ∂f

∂x and ∂h∂x . Accordingly we have

∂f

∂x=

(q cosφ tan θ − r sinφ tan θ, q sinφ+r cosφ

cos2 θ−q sinφ− r cosφ, 0

)∂h

∂x=

0 qVa cos θ + g cos θ−g cosφ cos θ −rVa sin θ − pVa cos θ + g sinφ sin θg sinφ cos θ (qVa + g cosφ) sin θ

.

8.10 GPS SMOOTHING

MODIFIED MATERIAL: Assuming that wind and airspeed are constantwe get

Vg =Vaψ(we cosψ − wn sinψ)

Vg.

MODIFIED MATERIAL: The Jacobian of f is given by

∂f

∂x=

0 0 cosχ −Vg sinχ 0 0 00 0 sinχ Vg cosχ 0 0 0

0 0 − VgVg

0 −ψVa sinψVg

ψVa cosψVg

∂Vg∂ψ

0 0 − gV 2g

tanφ 0 0 ψVa cosψ 0

0 0 0 0 0 0 00 0 0 0 0 0 00 0 0 0 0 0 0

,

MODIFIED MATERIAL:where yGPS = (yGPS,n, yGPS,e, yGPS,Vg , yGPS,χ, ywind,n, ywind,e), u = Va,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 inthe 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

abc

× =

0 −c bc 0 −a−b a 0

,

and where

R(Θ) =

cθcψ cθsψ −sθsφsθcψ − cφsψ sφsθsψ + cφcψ sφcθcφsθcψ + sφsψ cφsθsψ − sφcψ cφcθ

S(Θ) =

1 sinφ tan θ cosφ tan θ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

mf = a +R>(Θ)g,

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 northand 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

ydiff pres =ρV 2

a

2+ ηdiff pres

yGPS,n = pn + νn[n]

yGPS,e = pe + νe[n]

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 bewritten 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 = (y>accel,y>gyro)>,

we get

x = f(x,y) +Gg(x)ηgyro +Gaηaccel + η,

78 STATE ESTIMATION

where

f(x,y) =

R(Θ)v

v×(ygyro − b) + yaccel +R>(Θ)gS(Θ)(ygyro − b)

03×1

02×1,

Gg(x) =

03×3

−v×−S(Θ)03×3

02×3

Ga =

03×3

−I3×3

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

A(x,y)4=∂f

∂x

=

03×3 R(Θ) ∂[R(Θ)v]

∂Θ 03×3 03×2

03×3 −(ygyro − b)×∂[R>(Θ)g]

∂Θ −v× 03×2

03×3 03×3∂[S(Θ)(ygyro−b)]

∂Θ −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 matrixwhose elements are functions of z, then for any w ∈ R3

∂ [A(z)w]

∂z=((

∂A(z)∂z1

)w,

(∂A(z)∂z2

)w,

(∂A(z)∂z3

)w).

State Estimation 79

Therefore

∂ [R(Θ)v]

∂Θ=(∂R(Θ)∂φ v, ∂R(Θ)

∂θ v, ∂R(Θ)∂ψ v

)∂[R>(Θ)g

]∂Θ

=(∂R>(Θ)∂φ g, ∂R>(Θ)

∂θ g, ∂R>(Θ)∂ψ g

)= g

0 − cos θ 0cos θ 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 + η)>]

= GgQgyroG>g +GaQaccelG

>a +Q,

where

Qgyro = diag([σ2gyro,x, σ

2gyroy

, σ2gyroz

])

Qaccel = diag([σ2accel,x, σ

2accely , σ

2accelz ]),

and where

Q = diag([σ2pn , σ

2pe , σ

2pd, σ2

u, σ2v , σ

2w, σ

2φ, σ

2θ , σ

2ψ, σ

2bx , σ

2by , σ

2bz , σ

2wn , σ

2we ])

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

hdiff(x) =1

2ρV 2

a ,

where

V 2a =

(v −R>(Θ)w

)> (v −R>(Θ)w

)w = (wn, we, 0)>.

Therefore

hdiff(x) =1

2ρ(v −R>(Θ)w

)> (v −R>(Θ)w

).

The associated Jacobian is given by

Cdiff(x) = ρ

03×1

v −R>(Θ)w

−∂[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 planeis given by

vg⊥ =[I2×2, 02×1

]R(Θ)v. (8.9)

The ground-speed is given by

Vg(v,Θ) = ‖vg⊥‖ .The model for the sensor is therefore

hGPS,Vg(x) = ‖PR(Θ)v‖ ,

where P =[I2×2, 02×1

], and the associated Jacobian is given by

CGPS,Vg (x) =(

01×3, v>R>(Θ)P>PR(Θ),∂Vg(v,Θ)

∂Θ

>, 01×3, 01×2

),

where ∂Vg(v,Θ)∂Θ

>is computed numerically as

∂Vg(v,Θ)

∂Θi=Vg(v,Θ + ∆ei)− Vg(v,Θ)

∆,

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

CGPS,χ(x) =(

01×3,∂χ(v,v)∂v

>, ∂χ(v,Θ)

∂Θ

>, 01×3, 01×2

),

where ∂χ(v,v)∂v

>and ∂χ(v,Θ)

∂Θ

>are computed numerically as

∂χ(v,Θ)

∂vi=χ(v + ∆ei,Θ)− χ(v,Θ)

∆

∂χ(v,Θ)

∂Θi=χ(v,Θ + ∆ei)− χ(v,Θ)

∆.

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 notobservable and can therefore drift. To help correct this situation, we can adda pseudo measurement on the side-slip angle by assuming that it is zero.

The side slip angle is given by

β =vrVa,

therefore an equivalent condition is that vr = 0. The airspeed vector is givenby

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

Cβ(x) =(

01×3,∂vr(v,Θ,w)

∂v

>, ∂vr(v,Θ,w)

∂Θ

>, 01×3,

∂vr(v,Θ,w)∂w

>,

),

where the partial derivatives are computed numerically as

∂vr(v,Θ,w)

∂vi=vr(v + ∆ei,Θ,w)− vr(v,Θ,w)

∆

∂vr(v,Θ,w)

∂Θi=vr(v,Θ + ∆ei,w)− vr(v,Θ,w)

∆

∂vr(v,Θ,w)

∂wi=vr(v,Θ,w + ∆ei)− vr(v,Θ,w)

∆.

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 sampledat rate Ts, which is the same rate that state estimates are required by thecontroller. 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 , e

2pe , e

2pd, e2u, e

2v, e

2w, e

2φ, e

2θ, e

2ψ, e

2bx , e

2by , e

2bz , e

2wn , e

2we ]),

where e∗ is the standard deviation of the expected error in ∗.1. At the fast sample rate Ts1.a. Propagate x and P according to

˙x = f(x, y)

P = A(x, y)P + PA>(x, y) +Gg(x)QgyrosG>g (x) +GaQaccelG

>a +Q

1.b. Update x and P with the static pressure sensor according to

L = P−C>static(x−)/(σ2

abs pres + Cstatic(x−)P−C>static(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−C>diff(x−)/(σ2

diff pres + Cdiff(x−)P−C>diff(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−C>GPS,n(x−)/(σ2GPS,n + CGPS,n(x−)P−C>GPS,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−C>GPS,e(x−)/(σ2

GPS,e + CGPS,e(x−)P−C>GPS,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−C>GPS,Vg(x−)/(σ2

GPS,Vg + CGPS,Vg(x−)P−C>GPS,Vg(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−C>GPS,χ(x−)/(σ2GPS,χ + CGPS,χ(x−)P−C>GPS,χ(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 observethe effect. Different states may require a different filter bandwidth.

8.3 Modify the observer file to implement the extended Kalman filter forroll and pitch angles described in Section 8.9. Tune the filter until youare satisfied with the performance.

8.4 Modify the observer file to implement the extended Kalman filter forposition, heading, and wind described in Section 8.10. Tune the filteruntil you are satisfied with the performance.

8.5 Change the simulation to use the estimated state in the autopilot asopposed to the true states. Tune autopilot and estimator gains if nec-essary. By changing the bandwidth of the low-pass filter, note that thestability 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 anarc. Define the two dimensional planeP as the plane containing the velocityvector 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-bodydiagram 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 angleof φ.

at an angle of φ, the projection of the lift vector onto P is Flift cosφ. Thecentripetal force due to the pull-up maneuver is mVgγ. Therefore, summingthe forces in the ib-kb plane gives

Flift cosφ = mVgγ + mg cos γ. (9.1)

88 DESIGN MODELS FOR GUIDANCE

Solving for γ gives

γ =g

Vg

(Flift

mgcosφ− cos γ

). (9.2)

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. Sincewind 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 theflight control system, rather than being considered during the path planning.It has been shown that vector-field-based path following methods, such asthose employed in this chapter, are particularly effective at rejecting winddisturbances [3]. Treating wind as a disturbance also allows paths to beplanned relative to the inertial environment, which is important as UAVsare flown in complex 3D terrain. Accordingly, when the Dubins airplanemodel is used for path planning, the effects of wind are not accounted forwhen formulating the equations of motion. In this case, the airspeed V isthe same as the groundspeed, the heading angle ψ is the same as the courseangle (assuming zero sideslip angle), and the flight-path angle γ is the sameas the air-mass-referenced flight-path angle [4].

Figure 9.2 depicts a UAV flying with airspeed V , heading angle ψ andflight-path angle γ. Denoting the inertial position of the UAV as p =(rn, re, rd)

>, the kinematic relationship between the inertial velocity, v =(rn, re, rd)

>, and the airspeed, heading angle, and flight-path angle can beeasily visualized as rnre

rd

=

V cosψ cos γV sinψ cos γ−V sin γ

,

where V = ‖v‖.

Design Models for Guidance 89

Flight path projected onto ground

horizontal component of airspeed vector

ki (down)

ji (east)

ii (north)

ii

V cos

rn = V cos cos re = V sin cos

rd = V sin

v

Figure 9.2: Graphical representation of aircraft kinematic model.

This chapter assumes that a low-level autopilot regulates the airspeed V toa constant commanded value V c, the flight-path angle γ to the commandedflight-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 areassumed to be sufficiently fast that they can be ignored for the purpose ofpath following. The relationship between the heading angle ψ and the bankangle φ is given by the coordinated turn condition [4]

ψ =g

Vtanφ,

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 responseto their commanded values, then the following kinematic model is a good

90 DESIGN MODELS FOR GUIDANCE

description of the UAV motion

rn = V cosψ cos γc

re = V sinψ cos γc (9.3)rd = −V sin γc

ψ =g

Vtanφc.

Physical capabilities of the aircraft place limits on the achievable bank andflight-path angles that can be commanded. These physical limits on theaircraft 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 modeloriginally proposed for the Dubins airplane in [?], which is given by

rn = V cosψ

re = V sinψ (9.6)rd = u1 |u1| ≤ 1

ψ = u2 |u2| ≤ 1.

Although (9.3) is similar to (9.6), it captures the aircraft kinematics withgreater accuracy and provides greater insight into the aircraft behavior, andis more consistent with commonly used aircraft guidance models. Notehowever, that (9.3) is only a kinematic model that does not include aero-dynamics, wind effects, or engine/thrust limits. While it is not sufficientlyaccurate for low-level autopilot design, it is well suited for for high levelpath planning and path following control design. In-depth discussions ofaircraft 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, priorto implementation on the full simulation model. We will focus primarily onthe models given in equations (??) and (??).

9.1 Create a Simulink S-function or Python/Matlab class that implementsthe model given in equation (??) and insert it in your MAV simulator.For different inputs χc, hc, and V c

a , compare the output of the twomodels, and tune the autopilot coefficients bVa , bh, bh, bχ, and bχ toobtain similar behavior. You may need to re-tune the autopilot gainsobtained 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 similartriangles, we have the relationship

hd + rd√s2n + s2

e

=−qd√q2n + q2

e

.

MODIFIED MATERIAL: From figure 10.1(b), the desired altitude for anaircraft at p following the straight-line path Pline(r,q) is given by

hd(r,p,q) = −rd −√s2n + s2

e

(qd√q2n + q2

e

). (10.5)

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

χc(t) = ϕ+ λ

[π

2+ tan−1

(korbit

(d− ρρ

))].

One of the disadvantages of this strategy is that if the roll angle is usedto command the course, and if the course angle is currently correct, then theroll 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 headingrate is given by

ψd = λVaR.

Assuming a coordinated turn condition, the kinematics of the UAV are givenby equation (9.14) as

ψ =g

Vatanφ.

Setting these expressions equal to each other, and solving for the roll angle,gives the feedforward term

φff = λ tan−1

(V 2a

gR

). (10.6)

In other words, if the UAV is currently on the orbit and is banked at φff , andassuming 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 willassume 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 describedby the expression

χd(t) = λVg(t)

R,

where Vg is the time varying ground speed. From equation (9.10) in thebook, the coordinated turn condition in wind is given by

χ =g

Vgtanφ cos(χ− ψ).

Equating these expressions and solving for φ results in the feedforward term

φff = λ tan−1

(V 2g

gR cos(χ− ψ)

).

From the wind triangle expression given in equation (2.12) we have that

sin(χ− ψ) =1

Va cos γa(we cosχ− wn sinχ).

Using a simple identity from trigonometry we get

cos(χ− ψ) =

√1−

(1

Va cos γa(we cosχ− wn sinχ)

)2

.

In a constant altitude orbit, the flight path angle γ = 0, and therefore equa-tion (2.11), which comes from the wind triangle, becomes

sin γa =wdVa,

from which we get that

cos γa =

√1−

(wdVa

)2

,

which implies that

cos(χ− ψ) =

√1− (we cosχ− wn sinχ)2

V 2a − w2

d

.

In a constant altitude orbit, the flight path angle γ = 0, and thereforeequation (2.10), which comes from the wind triangle, becomes

V 2g − 2 (wn cosχ+ we sinχ)Vg + (V 2

w − V 2a ) = 0.


Taking the positive root for Vg gives

Vg = (wn cosχ+ we sinχ) +

√(wn cosχ+ we sinχ)2 − V 2

w + V 2a

= (wn cosχ+ we sinχ) +√V 2a − (wn sinχ− we cosχ)2 − w2

d.

The term under the square root will be positive when the airspeed is greaterthan the windspeed, ensuring a positive groundspeed.

Therefore, the feedforward roll angle is given by

φff = λ tan−1

(

(wn cosχ+ we sinχ) +√V 2a − (wn sinχ− we cosχ)2 − w2

d

)2gR

√V 2a−(wn sinχ−we cosχ)2−w2

d

V 2a−w2

d

,

(10.7)

which depends only on the wind speed, the current course angle, and theairspeed. 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 ?? showshow 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 partialderivatives, and where r ∈ R3. An underlying assumption is that the pathgiven by the intersection is connected and one-dimensional. Defining thefunction

V (r) =1

2α2

1(r) +1

2α2

2(r),

gives

∂V

∂r= α1(r)

∂α1

∂r(r) + α2(r)

∂α2

∂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 ∂α1

∂r and ∂α2∂r . Following [8] the desired velocity vector u′ ∈ R3

can be chosen as

u′ = −K1∂V

∂r+K2

∂α1

∂r× ∂α2

∂r, (10.8)

where K1 and K2 are symmetric tuning matrices. It is shown in [8] that thedynamics r = u′ where u′ 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 thedirection of travel along the trajectory.

The problem with equation (10.8) is that the magnitude of the desiredvelocity u′ may not equal V , the velocity of the Dubin’s airplane. Thereforeu′ is normalized as

u = Vu′

‖u′‖ . (10.9)

Fortunately, the stability proof in [8] is still valid when u′ is normalized asin equation (10.9).

Setting the NED components of the velocity of the Dubins airplane modelgiven 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 headingangle ψd results in the expressions

γc = −satγ[sin−1

(u3

V

)](10.10)

ψd = atan2(u2, u1),

where atan2 is the four quadrant inverse tangent, and where the saturationfunction is defined as

sata[x] =

a if x ≥ a−a if x ≤ −ax 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

φ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 onthe line. Let c` = (cn, ce, cd)

> be an arbitrary point on the line, and let thedirection of the line be given by the desired heading angle from north ψ`,and the desired flight-path angle γ` measured from the inertial north-eastplane. Therefore

q` =

qnqeqd

4=

cosψ` cos γ`sinψ` cos γ`− 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

nlon4=

− sinψ`cosψ`

0

.

Similarly, a unit vector that is perpendicular to the lateral plane defined byq` is given by

nlat4= nlon × q` =

− cosψ` sin γ`− sinψ` sin γ`− cos γ`

.

It follows that Pline is given by the intersection of the surfaces defined by

αlon(r)4= n>lon(r− c`) = 0 (10.13)

αlat(r)4= 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.

nlon

nlat

c` q`

↵lon(r) = 0

↵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

∂r= nlon

∂αlat

∂r= nlat.

Therefore, before normalization, the desired velocity vector is given by

u′line = K1

(nlonn

>lon + nlatn

>lat

)(r− c`) +K2 (nlon × nlat) . (10.15)

10.3.3 Helical Paths

A time parameterized helical path is given by

r(t) = ch +

Rh cos(λht+ ψh)Rh sin(λht+ ψh)−tRh tan γh

, (10.16)

where r(t) =

rnrerd

(t) is the position along the path, ch = (cn, ce, cd)> is

the center of the helix, and the initial position of the helix is

r(0) = ch +

Rh cosψhRh 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 parameterizationin (10.16) needs to be eliminated. Equation (10.16) gives

(rn − cn)2 + (re − ce)2 = R2h.

In addition, divide the east component of r− ch by the north component toget

tan(λht+ ψh) =re − cern − cn

Solving for t and plugging into the third component of (10.16) gives

rd − cd = −Rh tan γhλh

(tan−1

(re − cern − cn

)− ψh

).

Therefore, normalizing these equations by Rh results in

αcyl(r) =

(rn − cnRh

)2

+

(re − ceRh

)2

− 1

αpl(r) =

(rd − cdRh

)+

tan γhλh

(tan−1

(re − cern − cn

)− ψh

).

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 forparameters ch = (0, 0, 0)>, Rh = 30 m, γh = 15π

180 rad, and λh = +1. Theassociated helical path is the intersection of the two surfaces.The gradients of αcyl and αpl are given by

∂αcyl

∂r=(2 rn−cnRh

, 2 re−ceRh, 0

)>∂αpl

∂r=(

tan γhλh

−(re−ce)(rn−cn)2+(re−ce)2 ,

tan γhλh

(rn−ce)(rn−cn)2+(re−ce)2 ,

1Rh

)>.

Before normalization, the desired velocity vector is given by

u′helix = K1

(αcyl

∂αcyl

∂r+ αpl

∂αpl

∂r

)+ λK2

(∂αcyl

∂r× ∂αpl

∂r

), (10.18)


Figure 10.3: A helical path for parameters ch = (0, 0, 0)>, Rh = 30 m, γh =15π180 rad, and λh = +1.

where

∂αcyl

∂r× ∂αpl

∂r=

2

Rh

( re−ceRh

, − rn−cnRh

, λh tan γh)>.



10.5 DESIGN PROJECT



Chapter Eleven

Path Manager

11.1 TRANSITIONS BETWEEN WAYPOINTS

MODIFIED MATERIAL: In other words, the guidance algorithm wouldswitch at the first time instant when

‖p(t)−wi‖ ≤ 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 waypointsegments.

is in state state=1, the MAV is commanded to follow the straight-linepath along wi−1wi, which is parameterized by r = wi−1, and q = qi−1,which are assigned in Lines ??–??. Lines ??–?? test to see if the MAVhas transitioned into the half plane shown as H1 in figure ??. If the MAVhas transitioned into H1, then the state machine is updated to state=2.MODIFIED MATERIAL: To be precise, let

|W| 4=N∑i=2

‖wi −wi−1‖

be defined as the length of the waypoint pathW . Define |W|F as the pathlength 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 lengthof the straight-line segment removed from |W| by traversing the fillet is2R/ tan %i

2 . Therefore,

|W|F = |W|+N∑i=2

(R(π − %i)−

2R

tan %i2

), (11.1)

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: ‖ps − pe‖ ≥ 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← minL1, L2, L3, L4.7: if arg minL1, L2, L3, L4 = 1 then8: cs ← crs, λs ← +1, ce ← cre, λe ← +19: q1 ← ce−cs

‖ce−cs‖ ,10: z1 ← cs +RRz

(−π

2

)q1,

11: z2 ← ce +RRz(−π

2

)q1

12: else if arg minL1, L2, L3, L4 = 2 then13: cs ← crs, λs ← +1, ce ← cle, λe ← −114: `← ‖ce − cs‖,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

20: else if arg minL1, L2, L3, L4 = 3 then21: cs ← cls, λs ← −1, ce ← cre, λe ← +122: `← ‖ce − cs‖,23: ϑ← angle(ce − cs),24: ϑ2 ← cos−1 2R

`

25: q1 ← Rz(ϑ+ ϑ2 − π

2

)e1,

26: z1 ← cs +RRz (ϑ+ ϑ2) e1,27: z2 ← ce +RRz (ϑ+ ϑ2 − π) e1

28: else if arg minL1, L2, L3, L4 = 4 then29: cs ← cls, λs ← −1, ce ← cle, λe ← −130: q1 ← ce−cs

‖ce−cs‖ ,31: z1 ← cs +RRz

(π2

)q1,

32: z2 ← ce +RRz(π2

)q1

33: end if34: z3 ← pe35: 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 valueis

σ∗ =(v1 − p)>(v1 − v2)

‖v1 − v2‖2,

and

D(v1,v2,p) =

√‖p− v1‖2 −

((v1 − p)>(v1 − v2))2

‖v1 − v2‖2.

If we define

D′(v1,v2,p)

4=

D(v1,v2,p) if σ∗ ∈ [0, 1]

‖p− v1‖ if σ∗ < 0

‖p− v2‖ if σ∗ > 1,

then the distance between the point setQ and the line segment v1v2 is givenby

D(v1,v2,Q) = minp∈Q

D′(v1,v2,p).

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 cameraframes 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 azimuthand elevation angles that will align the optical axis with ˇb

d. In the cameraframe, the optical axis is given by (0, 0, 1)c. Therefore, the objective is to

110 VISION-GUIDED NAVIGATION

select the commanded gimbal angles αcaz and αcel so that

ˇbd4=

ˇbxd

ˇbyd

ˇbzd

= Rbg(αcaz, αcel)Rgc

001

(13.1)

=

cosαcel cosαcaz − sinαcaz − sinαcel cosαcazcosαcel sinαcaz cosαcaz − sinαcel sinαcaz

sinαcel 0 cosαcel

0 0 11 0 00 1 0

001

(13.2)

=

cosαcel cosαcazcosαcel sinαcaz

sinαcel

. (13.3)

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 ANDROTATION 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 theimplementation 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 oneparticular way that the simulator can be constructed, as well as the specificdata 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

path planner

path manager

path following

autopilot

unmanned aircraft

waypoints

on-board sensors

position error

tracking error

status

destination,obstacles

servo commandsstate estimator

wind

path definition

airspeed,altitude,course

commands

map

state estimates

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

dt(t) = f(x(t), u(t)) (C.1)

with initial condition x(t0) = x0, where x(t) ∈ Rn, and u(t) ∈ Rm, andwhere we assume that f(x, u) is sufficiently smooth to ensure that uniquesolutions to the differential equation exist for every x0.

Integrating both sides of equation (C.1) from t0 to t gives

x(t) = x(t0) +

∫ t

t0

f(x(τ), u(τ)) dτ. (C.2)

The difficulty with solving equation (C.2) is that x(t) appears on both sidesof the equation, and cannot therefore be solved explicitly for x(t). Nu-merical solutions techniques for ordinary differential equations attempt toapproximate the solution by approximating the integral in equation (C.2).Most techniques break the solution into small time increments, usually equalto the sample rate, which we denote by Ts. Accordingly we write equa-tion (C.2) as

x(t) = x(t0) +

∫ t−Ts

t0

f(x(τ), u(τ)) dτ +

∫ t

t−Tsf(x(τ), u(τ)) dτ

= x(t− Ts) +

∫ t

t−Tsf(x(τ), x(τ)) dτ. (C.3)

The most simple form of numerical approximation for the integral inequation (C.3) is to use the left endpoint method as shown in figure C.2,where ∫ t

t−Tsf(x(τ), u(τ)) dτ ≈ Tsf(x(t− Ts), u(t− Ts)).

Using the notation xk4= x(t0 +kTs) we get the numerical approximation

to the differential equation in equation (C.1) as

xk = xk−1 + Tsf(xk−1, uk−1), x0 = x(t0). (C.4)

Equation (C.4) is called the Euler method, or the Runge-Kutta first ordermethod (RK1).

While the RK1 method is often effective for numerically solving ODEs, ittypically requires a small sample size Ts. The advantage of the RK1 methodis 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 inRunge-Kutta first order method (RK1).

To improve the numerical accuracy of the solution, a second order methodcan be derived by approximating the integral in equation (C.3) using thetrapezoidal rule shown in figure ??, where∫ b

af(ξ)dξ ≈ (b− a)

(f(a) + f(b)

2

).

Accordingly,

Figure C.3: Approximation of integral using the trapezoidal rule results in Runge-Kutta second order method (RK2).

∫ t

t−Tsf(x(τ), u(τ))dτ ≈ Ts

2

[f(x(t− Ts), u(t− Ts)) + f(x(t), u(t))

].


Since x(t) is unknown, we use the RK1 method to approximate it as

x(t) ≈ x(t− Ts) + Tsf(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

t−Tsf(x(τ), u(τ))dτ ≈ Ts

2

[f(x(t− Ts), u(t− Ts))

+ f(x(t− Ts) + Tsf(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 + TsX1, uk−1)

xk = xk−1 +Ts2

(X1 +X2) .

Equations (C.5) is the Runge-Kutta second order method or RK2. It requirestwo 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’sRule ∫ b

af(ξ)dξ ≈

(b− a

6

)(f(a) + 4f

(a+ b

2

)+ f(b)

),

which is derived by approximating the area under the integral using a parabola,as shown in figure ??. The standard strategy is to write the approximationas∫ b

af(ξ)ξ ≈

(b− a

6

)(f(a) + 2f

(a+ b

2

)+ 2f

(a+ b

2

)+ f(b)

),

and then to use

X1 = f(a) (C.6)

X2 = f(a+Ts2X1) ≈ f

(a+ b

2

)(C.7)

X3 = f(a+Ts2X2) ≈ f

(a+ b

2

)(C.8)

X4 = f(a+ TsX3) ≈ 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)

X2 = f(xk−1 +Ts2X1, uk−1) (C.10)

X3 = f(xk−1 +Ts2X2, uk−1)

X4 = f(xk−1 + TsX3, uk−1)

xk = xk−1 +Ts6

(X1 + 2X2 + 2X3 +X4) .

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. Higherorder methods can be derived, but generally do not result in significantlybetter numerical approximations to the ODE. The step size Ts must still bechosen to be sufficiently small to result in good solutions. It is also possibleto develop adaptive step size solvers. For example the ODE45 algorithm inMatlab, solves the ODE using both an RK4 and an RK5 algorithm. The twosolutions are then compared and if they are sufficiently different, then thestep size is reduced. If the two solutions are close, then the step size can beincreased.

A Python implementation of the RK4 method for the dynamics(x1

x2

)=

(x2

sin(x1) + u

),

y = x1


with initial conditions x = (0, 0)>, would be as follows:import numpy as npimport 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 i n i t ( s e l f , Ts ) :

s e l f . t s = Ts# s e t i n i t i a l c o n d i t i o n ss e l f . s t a t e = np . a r r a y ( [ [ 0 . 0 ] , [ 0 . 0 ] ] )

def u p d a t e ( s e l f , u ) :’ ’ ’ 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 ’ ’ ’t s = s e l f . t sX1 = 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 o u t p u t ( s e l f ) :’ ’ ’ R e t u r n s s y s t e m o u t p u t ’ ’ ’y = s e l f . s t a t e . i t em ( 0 )re turn y

def d e r i v a t i v e s ( s e l f , x , u ) :’ ’ ’ Re t u r n x d o t f o r t h e dynamics x d o t = f ( x , u ) ’ ’ ’x1 = x . i t em ( 0 )x2 = x . i t em ( 1 )x 1 d o t = x2x 2 d o t = np . s i n ( x1 ) + ux d o t = np . a r r a y ( [ [ x 1 d o t ] , [ x 2 d o t ] ] )re turn x d o t

# i n i t i a l i z e t h e s y s t e mTs = 0 . 0 1 # s i m u l a t i o n s t e p s i z esys tem = dynamics ( 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 da tas i m t i m e = 0 . 0t ime = [ s i m t i m e ]y = [ sys tem . o u t p u t ( ) ]

# main s i m u l a t i o n loopwhi le s i m t i m e < 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 )sys tem . u p d a t e ( u ) # p r o p a g a t e t h e s y s t e m dynamicss i m t i m e += Ts # i n c r e m e n t t h e s i m u l a t i o n t i m e

# u pd a t e da ta f o r p l o t t i n g


t ime . append ( s i m t i m e )y . append ( sys tem . o u t p u t ( ) )

# p l o t o u t p u t yp l t . p l o t ( t ime , y )p l t . x l a b e l ( ’ t ime ( s ) ’ )p l t . y l a b e l ( ’ o u t p u t y ’ )p l t . show ( )



Appendix D

Animation

In the study of aircraft dynamics and control, it is essential to be able tovisualize the motion of the airframe. In this appendix we describe how tocreate 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 tovisualize the motion of the airframe. In this section we describe how tocreate 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 ’ , cos ( t ) )

>> p l o t h a n d l e 1 = p l o t ( t , s i n ( t ) )>> hold on>> p l o t h a n d l e 2 = p l o t ( t , cos ( t ) )

>> s e t ( p l o t h a n d l e 2 , ’ YData ’ , cos (2∗ t ) )

D.5 ANIMATION EXAMPLE: INVERTED PENDULUM

f u n c t i o n h a n d l e = drawBase ( y , width , h e i g h t , gap , hand le , mode )X = [ y−wid th / 2 , y+ wid th / 2 , y+ wid th / 2 , y−wid th / 2 ] ;Y = [ gap , gap , gap+ h e i g h t , gap+ h e i g h t ] ;i f i sempty ( h a n d l e ) ,

h a n d l e = f i l l (X, Y, ’m’ , ’ EraseMode ’ , mode ) ;e l s e

126 ANIMATION

s e t ( hand le , ’ 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 , hand le , 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∗ cos ( t h e t a ) ] ;i f i sempty ( h a n d l e ) ,

h a n d l e = p l o t (X, Y, ’ g ’ , ’ EraseMode ’ , mode ) ;e l s e

s e t ( hand le , ’ XData ’ ,X, ’ YData ’ ,Y ) ;end

f u n c t i o n drawPendulum ( u )% p r o c e s s i n p u t s t o f u n c t i o ny = u ( 1 ) ;t h e t a = u ( 2 ) ;t = u ( 3 ) ;

% drawing p a r a m e t e r sL = 1 ;gap = 0 . 0 1 ;wid th = 1 . 0 ;h e i g h t = 0 . 1 ;

% d e f i n e p e r s i s t e n t v a r i a b l e sp e r s i s t e n t b a s e h a n d l ep e r s i s t e n t r o d h a n d l e

% f i r s t t i m e f u n c t i o n i s c a l l e d ,% 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 si f t ==0 ,

f i g u r e ( 1 ) , c l ft 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 ’ ) ;hold onb a s e h a n d l e = drawBase ( y , width , h e i g h t , gap , [ ] , ’ normal ’ ) ;r o d h a n d l e = drawRod ( y , t h e t a , L , gap , h e i g h t , [ ] , ’ normal ’ ) ;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 , redraw base and rode l s e

drawBase ( y , width , 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 ; . . . % p o i n t 11 −1 0 ; . . . % p o i n t 2−1 −1 0 ; . . . % p o i n t 3−1 1 0 ; . . . % p o i n t 4

1 1 0 ; . . . % p o i n t 11 1 − 2 ; . . . % p o i n t 51 −1 − 2 ; . . . % p o i n t 61 −1 0 ; . . . % p o i n t 21 −1 − 2 ; . . . % p o i n t 6−1 −1 − 2 ; . . . % p o i n t 7−1 −1 0 ; . . . % p o i n t 3−1 −1 − 2 ; . . . % p o i n t 7−1 1 − 2 ; . . . % p o i n t 8−1 1 0 ; . . . % p o i n t 4−1 1 − 2 ; . . . % p o i n t 8

1 1 − 2 ; . . . % p o i n t 51 1 0 ; . . . % p o i n t 11 . 5 1 . 5 0 ; . . . % p o i n t 91 . 5 −1.5 0 ; . . . % p o i n t 101 −1 0 ; . . . % p o i n t 21 . 5 −1.5 0 ; . . . % p o i n t 10−1.5 −1.5 0 ; . . . % p o i n t 11−1 −1 0 ; . . . % p o i n t 3−1.5 −1.5 0 ; . . . % p o i n t 11−1.5 1 . 5 0 ; . . . % p o i n t 12−1 1 0 ; . . . % p o i n t 4−1.5 1 . 5 0 ; . . . % p o i n t 12

1 . 5 1 . 5 0 ; . . . % p o i n t 9] ’ ;

f u n c t i o n XYZ= r o t a t e (XYZ, phi , t h e t a , p s i )% d e f i n e r o t a t i o n m a t r i xR r o l l = [ . . .

1 , 0 , 0 ; . . .0 , cos ( p h i ) , −s i n ( p h i ) ; . . .0 , s i n ( p h i ) , cos ( p h i ) ] ;

R p i t c h = [ . . .cos ( t h e t a ) , 0 , s i n ( t h e t a ) ; . . .0 , 1 , 0 ; . . .−s i n ( t h e t a ) , 0 , cos ( t h e t a ) ] ;

R yaw = [ . . .cos ( p s i ) , −s i n ( p s i ) , 0 ; . . .s i n ( 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 ;% r o t a t e v e r t i c e sXYZ = 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 + repmat ( [ pn ; pe ; pd ] , 1 , s i z e (XYZ, 2 ) ) ;

Drawing the spacecraft at the desired location is accomplished using thefollowing 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 , phi , t h e t a , p s i , hand le , 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 sNED = 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 phi , t h e t a , p s iNED = r o t a t e (NED, phi , 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 from NED t o XYZ ( f o r mat lab r e n d e r i n g )R = [ . . .

0 , 1 , 0 ; . . .1 , 0 , 0 ; . . .0 , 0 , − 1 ; . . .] ;

XYZ = R∗NED;% p l o t s p a c e c r a f ti f i sempty ( h a n d l e ) ,

h a n d l e = p l o t 3 (XYZ( 1 , : ) ,XYZ( 2 , : ) ,XYZ( 3 , : ) , ’ EraseMode ’ , mode ) ;e l s e

s e t ( hand le , ’ 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% D e f i n e t h e v e r t i c e s ( p h y s i c a l l o c a t i o n o f v e r t i c e s )V = [ . . .

1 1 0 ; . . . % p o i n t 11 −1 0 ; . . . % p o i n t 2−1 −1 0 ; . . . % p o i n t 3−1 1 0 ; . . . % p o i n t 4

1 1 − 2 ; . . . % p o i n t 51 −1 − 2 ; . . . % p o i n t 6−1 −1 − 2 ; . . . % p o i n t 7−1 1 − 2 ; . . . % p o i n t 8

1 . 5 1 . 5 0 ; . . . % p o i n t 91 . 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 as a l i s t o f v e r t i c e s numbered aboveF = [ . . .

1 , 2 , 6 , 5 ; . . . % f r o n t

Animation 129

4 , 3 , 7 , 8 ; . . . % back1 , 5 , 8 , 4 ; . . . % r i g h t2 , 6 , 7 , 3 ; . . . % l e f t5 , 6 , 7 , 8 ; . . . % t o p9 , 10 , 11 , 1 2 ; . . . % bot tom

] ;% d e f i n e c o l o r s f o r each f a c emyred = [ 1 , 0 , 0 ] ;mygreen = [ 0 , 1 , 0 ] ;myblue = [ 0 , 0 , 1 ] ;myyellow = [ 1 , 1 , 0 ] ;mycyan = [ 0 , 1 , 1 ] ;p a t c h c o l o r s = [ . . .

myred ; . . . % f r o n tmygreen ; . . . % backmyblue ; . . . % r i g h tmyyellow ; . . . % l e f tmycyan ; . . . % t o pmycyan ; . . . % bot tom

] ;end

The vertices are shown in figure ?? and are defined in Lines 3–16. Thefaces 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 thefaces has N columns, and the number of rows is the number of faces. Thecolor for each face is defined in Lines 32–39. Matlab code that draws thespacecraft 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 , p s i ,hand le , 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 tV = r o t a t e (V’ , phi , 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 gR = [ . . .

0 , 1 , 0 ; . . .1 , 0 , 0 ; . . .0 , 0 , − 1 ; . . .] ;

V = V∗R ;i f i sempty ( h a n d l e )h a n d l e = patch ( ’ V e r t i c e s ’ , V, ’ Faces ’ , F , . . .

’ FaceVer texCData ’ , p a t c h c o l o r s , . . .’ FaceCo lo r ’ , ’ f l a t ’ , . . .

130 ANIMATION

’ EraseMode ’ , mode ) ;e l s e

s e t ( hand le , ’ V e r t i c e s ’ ,V, ’ Faces ’ , F ) ;end

end


Appendix E

Airframe Parameters

Table E.1: Physical parameters for the Aerosonde UAV.

Physical Param Value Motor Param Valuem 11.0 kg Vmax 44.4 VJx 0.824 kg-m2 Dprop 0.5 mJy 1.135 kg-m2 KV 145 RPM/VJz 1.759 kg-m2 KQ 0.0659 V-s/radJxz 0.120 kg-m2 Rmotor 0.042 OhmsS 0.55 m2 i0 1.5 Ab 2.9 m CQ2 -0.01664c 0.19 m CQ1 0.004970ρ 1.2682 kg/m3 CQ0 0.005230e 0.9 CT2 -0.1079

CT1 -0.06044CT0 0.09357

132 AIRFRAME PARAMETERS

Table E.2: Aerodynamic coefficients for the Aerosonde UAV.

Longitudinal Coef. Value Lateral Coef. ValueCL0 0.23 CY0 0CD0 0.043 Cl0 0Cm0 0.0135 Cn0 0CLα 5.61 CYβ -0.83CDα 0.030 Clβ -0.13Cmα -2.74 Cnβ 0.073CLq 7.95 CYp 0CDq 0 Clp -0.51Cmq -38.21 Cnp -0.069CLδe 0.13 CYr 0CDδe 0.0135 Clr 0.25Cmδe -0.99 Cnr -0.095M 50 CYδa 0.075α0 0.47 Clδa 0.17ε 0.16 Cnδa -0.011

CDp 0 CYδr 0.19Clδr 0.0024Cnδ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 theJacobians ∂f

∂x and ∂f∂u , where

∂f

∂x=(∂f∂x1

∂f∂x2

. . . ∂f∂xn

),

where

∂f

∂xi=

∂f1∂xi∂f2∂xi...

∂fn∂xi

is a column vector. By definition we have that

∂f

∂xi(x) = lim

ε→0

f(x+ εei)− f(x)

ε,

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

∂xi(x) ≈ f(x+ εei)− f(x)

ε,

which can be computed numerically. Python code that computes the Jaco-bian of f at (x, u) is given below.

import numpy as npdef d f d x ( x , u ) :

# t a k e p a r t i a l o f f ( x , u ) w i t h r e s p e c t t o xeps = 0 . 0 1 # 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 wr t xf a t x = f ( x , u )f o r i in range ( 0 , 1 2 ) :

x e p s = np . copy ( x )x e p s [ i ] [ 0 ] += eps # add eps t o i t h s t a t ef 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 ) / epsA [ : , i ] = d f d x i [ : , 0 ]

re turn 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 spaceand 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,

T

pnpepduvwe0

exeyezpqr

=

pnpepduvw

Θφ(e0, ex, ey, ez)Θθ(e0, ex, ey, ez)Θψ(e0, ex, ey, ez)

pqr

,

where Θ(e) is given in equation (??).

Trim and Linearization 135

Since xe = T (xq) we have

xe =∂T

∂xqxq

=∂T

∂xqf(xq, u)

=∂T

∂xqf(T−1(xe), u),

where T−1(xe) is given in equation (??), and where

∂T

∂xq=

I3×3 03×3 03×4 03×3

03×3 I3×3 03×4 03×3

03×3 03×3∂Θ∂e 03×3

03×3 03×3 03×4 I3×3,

(F.1)

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], V ∗a sin γ∗, 0, 0, 0, 0, 0, V ∗a /R∗ cos(γ∗), 0, 0, 0)>,

therefore we letDX = [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 diagramis to be linearized, and [A,B,C,D] is the resulting state-space model. Ifthe 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 obtainequation (??) for example, you could use the following steps:


[A, B , C ,D]= l inmod ( 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 toxe. Suppose that the quaternion state space equations are given by

xq = fq(xq, u),

and that the euler state space equations are given by

xe = fe(xe, u),

and recall from the discussion above that xe = T (xq). Differentiating withrespect to time we get

xe =∂T

∂xqxq

=∂T

∂xqfq(xq, u)

=∂T

∂xqfq(T

−1(xe), u)

= fe(xe, u).

Therefore the Jacobian with respect to xe is given by

∂fe∂xe

=∂ ∂T∂xq

fq(T−1(xe), u)

∂xe

=∂T

∂xq

∂fq∂xq

∂T−1

∂xe,

where ∂T∂xq

given in equation (F.1) and where

∂T−1

∂xq=

I3×3 03×3 03×3 03×3

03×3 I3×3 03×3 03×3

04×3 04×3∂Q∂Θ 04×3

03×3 03×3 03×3 I3×3,

(F.2)

and where Q(Θ) is given by equation (??) and where ∂Q∂Θ can be computed

numerically.

Trim and Linearization 137

The Jacobian for the B matrix can be found similarly as

∂fe∂u

=∂T

∂xq

∂fq∂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 nonlinearfunctions with general constraints. For example, to minimize the functionJ(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 optimumgam = 3 ; % parame te r pa s s ed i n t o o b j e c t i v e f u n c t i o nxop t = fmincon ( @objec t ive , 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 df u n c t i o n J = o b j e c t i v e ( x , gam )

J = x∗x ’ − gam ;end

% n o n l i n e a r c o n s t r a i n t s f o r t r i m o p t i m i z a t i o nf u n c t i o n [ c , ceq ] = 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)<=0c ( 1 ) = s i n ( x ( 3 ) ) + 0 . 5 ;e q u a l i t y c o n s t r a i n t s ceq ( x )==0ceq ( 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 asfollows: 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 optimumx0 = np . a r r a y ( [ [ 1 0 ; 5 ; −20; 8 ] ] ) . T ;gam = 3 ; # parame te r p as se 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# second 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.5cons = ( ’ t y p e ’ : ’ eq ’ , ’ fun ’ : lambda x : x [0]−x [ 1 ] , ,


’ t y p e ’ : ’ i n e q ’ , ’ fun ’ : 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 problemr e s = min imize ( 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 ’ : 1e−10 , ’ d i s p ’ : True )

# e x t r a c t o p t i m a l s t a t exop 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 ddef o b j e c t i v e ( x , gam ) :

J = np . d o t ( x . T , x ) − gamre turn 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, AmericanInstitute 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, “VectorField Path Following for Miniature Air Vehicles,” IEEE Transactionson Robotics, vol. 37, pp. 519–529, jun 2007.

[4] R. W. Beard and T. W. McLain, Small Unmanned Aircraft: Theory andPractice. 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 RobotNavigation Along Time-Varying Curves in n-Dimensions,” IEEE Trans-actions on Robotics, vol. 26, pp. 647–659, aug 2010.



Index

Absolution Pressure Sensor, 57Accelerometers, 57, 75Aerodynamic Coefficients, 8AIRSPEED, 3Airspeed, 4, 13, 16Angle of Attack, 16Autopilot

Airspeed Hold using Throttle, 46Altitude Hold Using Pitch, 45Course Hold, 37Pitch Attitude Hold, 43Yaw damper, 38

Bandwidth Separation, 38

Coordinate Frames, 3, 109Body Frame, 3, 109Gimbal Frame, 109Inertial Frame, 3Stability Frame, 3Vehicle Frame, 3Vehicle-1 Frame, 3Vehicle-2 Frame, 3Wind Frame, 3

Coordinated Turn, 23, 87Course Angle, 3, 59Crab Angle, 3

Design Models, 1, 21, 87Differential Pressure Sensor, 57Dubins airplane, 90Dubins Path

Computation, 104Definition, 104RRT Paths through Obstacle Field, 108Tracking, 104

Dutch-roll Mode, 28Dynamics

Rotational Motion, 5

Ego Motion, 110Estiamtion

Heading, 75Estimation

Course, 75Position, 75Roll and Pitch Angles, 75Wind, 75

Flat Earth Model, 110Flight Path Angle

Inertial Referenced, 88

GPS, 57, 75GROUND SPEED, 3Ground Speed, 4, 13, 59

Heading Angle, 3helical path following, 99

Kalman FilterContinuous-discrete to Estimate Roll and

Pitch, 75Continuous-discrete to Position, Course,

Wind, Heading, 75Basic Explanation, 70Derivation, 70Geolocation, 110

KinematicsPosition, 22Rotation, 22

Lateral Motion, 23, 26, 35Linearization, 26Longitudinal Motion, 7, 23, 26, 43Low Pass Filter, 110Low-pass Filter, 61

path following, 96Phugoid Mode, 28

148 INDEX

PID: Digital Implementation, 48Precision Landing, 110

Rapidly Exploring Random Trees (RRT),108

3-D Terrain, 108Using Dubins Paths, 108

Rate Gyros, 57, 75Roll Mode, 28Rotation Matrices, 3

Short-period Mode, 28Side Slip Angle, 16Simulation Model, 1, 22, 133Spiral-divergence Mode, 28State-Space Models

Longitudinal, 26State-space Models

Lateral, 26straight-line path following, 98Successive Loop Closure, 33

Transfer FunctionsElevator to Pitch, 43Pitch to Altitude, 45Roll to Course, 37Throttle to Airspeed, 46

Trim, 23

vector field guidance model, 96Voronoi Path Planning, 107

Wind Gusts, 13Dryden Model, 14

WIND SPEED, 3Wind Speed, 4, 13Wind Triangle, 3, 4

Documents

Small Unmanned Aircraft - Brigham Young Universityuavbook.byu.edu/lib/exe/fetch.php?media=uavbook... · 2020. 3. 18. · 2.3 Airspeed, Wind Speed, and Ground Speed3 2.4 The Wind Triangle3