Lecture # 12

Aircraft Lateral Autopilots

Multiloop closure
Heading Control: linear
Heading Control: nonlinear
 Fall 2004 16.333 1026

1 % newr.m 53 kk=1;
2 % Analyze tracking algorithm by Park et al 54 while (~isempty(iii)) & (kk< length(t)1)
3 % AIAA GNC 2004 55 kk=kk+1;
4 % 56 aim_point=path(iii,:);
5 % Assumes that ac3.m has been run to generate syscl 57
6 % Jonathan How 58 xedot=U0*cos(X(5))X(1)*cos(X(4))*sin(X(5));
7 % MIT 16.333 Fall 2004 59 yedot=U0*sin(X(5))+X(1)*cos(X(4))*cos(X(5));
8 % 60
9 % 61 v1=[xedot yedot];
10 close all 62 v2=[aim_point(1)xe aim_point(2)ye];
11 dt=1; % time step for the simulation 63 v1=v1/norm(v1);
12 U0=235.9; 64 v2=v2/norm(v2);
13 path=[]; 65 [v1 v2];
14 66 temp=cross([v1;0],[v2;0]);
15 jcase=1; 67 eta=acos(v1*v2)*sign(temp(3));
16 % 2 path cases considered 68 phi_d=atan(2*U0^2/L1*sin(eta)/9.81);
17 if jcase==1 69 phi_d=max(min(phi_d,philim*pi/180),philim*pi/180);
18 t=[0:5*dt:2500]; 70
19 omp=.0025; 71 store=[store;[t(kk) X xe ye phi_d v2]];
20 path=24000*[sin(omp*t) (1cos(omp*t))]; 72 % propagate forward a step
21 xe=0;ye=1500; 73 X=Ad*X+Bd*phi_d;
22 X=[.1 0 0 0*pi/180 0*pi/180 0 0 0]; 74 xe=xe+xedot*dt;
23 else 75 ye=ye+yedot*dt;
24 t=[0:dt:1350]; 76
25 path(:,1)=U0*t; 77 ii=find((xepath(:,1)).^2+(yepath(:,2)).^2 < L1^2);
26 omp=.005; 78 iii=max(ii);
27 path(:,2)=500*(cos(2*pi*omp*t)+1).*exp(.002*t); 79 end
28 xe=0;ye=1000; 80
29 X=[.1 0 0 15*pi/180 15*pi/180 0 0 0]; 81 figure(1);clf
30 end 82 plot(store(:,11),store(:,10),m);
31 83 hold on;plot(path(:,2),path(:,1),g);
32 % Discretize the dynamics with time step dt 84 legend(veh,Path);xlabel(Y_e);
33 % system has the inner yaw and roll loops closed 85 ylabel(x_e);setlines(2);hold off
34 [A,B,C,D]=ssdata(syscl); 86 if jcase==1
35 syscld=c2d(ss(A,B,C,D),dt); 87 axis(square);axis(equal)
36 [Ad,Bd,Cd,Dd]=ssdata(syscld); 88 print depsc park_1; jpdf(park_1)
37 Bd=Bd(:,1);Dd=Dd(:,1); % only need first input 89 else
38 90 orient tall
39 % bank angle limit 91 print depsc park_1a; jpdf(park_1a)
40 philim=30; 92 end
41 % 93
42 %inputs are phi_d and 0 94 figure(2);clf
43 %state x=[v p r phi Psi xx xx xx] 95 plot(store(:,1),store(:,[5 12])*180/pi);
44 L1=2000; % look ahead distance 96 axis([0 t(kk) philim*1.1 philim*1.1])
45 store=[]; 97 xlabel(time);ylabel(\phi and \phi_d);
46 98 legend(\phi,\phi_d);setlines(2)
47 % find the point on the path L1 ahead 99 if jcase==1
48 ii=find((xepath(:,1)).^2+(yepath(:,2)).^2 < L1^2); 100 print depsc park_2; jpdf(park_2)
49 iii=max(ii); 101 else
50 % 102 print depsc park_2a; jpdf(park_2a)
51 % 103 end
52 % 104 return
Fall 2004 16.333 101
Lateral Autopilots

We can stabilize/modify the lateral dynamics using a variety of dif

ferent feedback architectures.
a -
p- 1 -
s
Glat (s)
r -
r -
1 -
s

Look for good sensor/actuator pairings to achieve desired behavior.

Example: Yaw damper
Can improve the damping on the Dutchroll mode by adding a
feedback on r to the rudder: rc = kr (rc r)
3.33
Servo dynamics Hr = s+3.33 added to rudder ra = Hr rc
System:
1.618s3 + 0.7761s2 + 0.03007s + 0.1883
Grcr = 5
s + 3.967s4 + 3.06s3 + 3.642s2 + 1.71s + 0.01223
Lateral autopilot: r to rudder
2
0.86 0.76 0.64 0.5 0.34 0.16

1.5
0.94

0.985
0.5
Imaginary Axis

3.5 3 2.5 2 1.5 1 0.5

0

0.5
0.985

0.94
1.5

0.86 0.76 0.64 0.5 0.34 0.16

2
3.5 3 2.5 2 1.5 1 0.5 0
Real Axis

Figure 2: Lateral polezero map Grc r

Fall 2004 16.333 102

Note that the gain of the plant is negative (Kplant < 0), so if kr < 0,
then K = Kplantkr > 0, so must draw a 180 locus (neg feedback)
Lateral autopilot: r to r with k>0 Lateral autopilot: r to r with k<0
2 2

1.5 1.5

0.25 1
1 1 0.25
0.5 0.5
0.75 0.75
0.5
0.5 0.5
0.25
Imaginary Axis

Imaginary Axis
0 1 0.5 0.25
0

0.25

0.5 0.5
0.5
0.75 0.75

0.5 0.5
1 0.25 1 0.25
1

1.5 1.5

2 2
2 1.5 1 0.5 0 0.5 1 1.5 2 3.5 3 2.5 2 1.5 1 0.5 0
Real Axis Real Axis

Figure 3: Lateral polezero map. Denitely need kr < 0

Root locus with kr < 0 looks pretty good as we have authority over
the four poles.
kr = 1.6 results in a large increase in the Dutchroll damping
and spiral/roll modes have combined into a damped oscillation.

Yaw damper looks great, but this implementation has a problem.

There are various ight modes that require a steady yaw rate
0). For example, steady turning ight.
(rss =
Our current yaw damper would not allow this to happen it would
create the rudder inputs necessary to cancel out the motion !!
Exact opposite of what we want to have happen, which
is to damp out any oscillations about the steady turn.
Fall 2004 16.333 103

Yaw Damper: Part 2

Can avoid this problem to some extent by ltering the feedback signal.
Feedback only a high pass version of the r signal.
High pass cuts out the low frequency content in the signal
steady state value of r would not be fed back to the controller.
s
New yaw damper: rc = kr (rc Hw (s)r) where Hw (s) = s+1 is the
washout lter.
Washout filter with =4.2

0
10
|H (s)|
w

1
10

2
10
2 1 0 1
10 10 10 10
Freq (rad/sec)

Figure 4: Washout lter with = 4.2

New control picture

a -
p- 1 -
s
Glat (s)
rc rc- r- 1 -
- kr Hr (s) -
6 s

Hw (s) 
Fall 2004 16.333 104

Lateral autopilot: r to rudder WITH washout filter

1.5

1 0.25
0.5

0.75

0.5
Imaginary Axis

1 0.5 0.25
0

0.5

0.75

0.5
1 0.25

1.5
2 1.5 1 0.5 0 0.5 1
Real Axis

Figure 5: Root Locus with the washout lter included.

Zero in Hw (s) traps a pole near the origin, but it is slow enough that
it can be controlled by the pilot.
Obviously has changed the closed loop pole locations ( ), but
kr = 1.6 still seems to give a well damped response.
Fall 2004 16.333 105

1
Without Washout
With Washout

0.5
Response r

0.5
0 5 10 15 20 25 30
Time

1
Without Washout
With Washout

0.5
Control r

0.5
0 5 10 15 20 25 30
Time

Figure 6: Impulse response of closed loop system with and without the Washout
lter ( = 4.2). Commanded rc = 0, and both have (r )ss = 0, but without the
0.
lter, rss = 0, whereas with it, rss =

For direct comparison with and without the lter, applied impulse as
rc to both closedloop systems and then calculated r and r .
Bottom plot shows that control signal quickly converges to zero in
both cases, i.e., no more control eort is being applied to correct the
motion.
But only the one with the washout lter produces a zero control input
even though the there is a steady turn the controller will not try
to ght a commanded steady turn.
Fall 2004 16.333 106

Heading Autopilot Design

So now have the yaw damper added correctly and want to control
the heading .
Need to bank the aircraft to accomplish this.
Result is a coordinated turn with angular rate

Aircraft banked to angle so that vector sum of mg and mU0 is

along the body zaxis
Summing in the body yaxis direction gives mu0 cos = mg sin
U0
tan =
g
Since typically 1, we have
U0

g
gives the desired bank angle for a specied turn rate.
Fall 2004 16.333 107

Problem now is that tends to be a noisy signal to base out bank

angle on, so we generate a smoother signal by ltering it.
Assume that the desired heading is known d and we want to
follow d relatively slowly
Choose dynamics 1 + = d

=
d 1s + 1
with 1=1520sec depending on the vehicle and the goals.
A low pass lter that eliminates the higher frequency noise.

Filtered heading angle satises

1
= (d )
1
which we can use to create the desired bank angle:
U0 U0
d = = (d )
g 1 g
Fall 2004 16.333 108

Roll Control

Given this desired bank angle, we need a roll controller to ensure

that the vehicle tracks it accurately.
Aileron is best actuator to use: a = k(d ) kpp
To design k and kp, can just use the approximation of the roll mode


Ixx p = Lpp + La a
I Lp = La a
= p xx

which gives
La
= sL )
a s(Ixx p

For the design, add the aileron servo dynamics

1
Ha(s) = , aa = Ha(s)ac
0.15s + 1
PD controller ac = k(s + 1) + kd, adds zero at s = 1/
Pick = 2/3
Root Locus
10
10
0.52 0.38 0.28 0.2 0.12 0.06

8
8
0.68

6
6

4
4
0.88

2
2
Imaginary Axis

2
2

0.88
4
4

6
6

0.68
8
8

0.52 0.38 0.28 0.2 0.12 0.06

10
7 6 5 4 3 2 1 10 0
Real Axis

Figure 7: Root Locus for roll loop closed Loop poles for Kp = 20, K = 30
Fall 2004 16.333 109

Reponse to initial roll of 15 degs

0.3

p

0.2

0.1

0.1

0.2

0.3

0.4
0 1 2 3 4 5 6 7 8 9 10

x 10
3 Reponse to initial roll of 15 degs
10

r

2
0 1 2 3 4 5 6 7 8 9 10

Figure 8: Roll response to an initial roll oset

Fall 2004 16.333 1010

Reponse to step input c=15 degs (0.26 rad)

0.3

p

0.25

0.2

0.15

0.1

0.05

0.05
0 5 10 15 20 25 30
Time (sec)

Reponse to step input c=15 degs (0.26 rad)

0.35

r
0.3

0.25

0.2

0.15

0.1

0.05

0.05

0.1
0 5 10 15 20 25 30
Time (sec)

Figure 9: Roll response to a step command for c . Note the adverse yaw eects.
Fall 2004 16.333 1011

Heading Autopilot
Putting the pieces together we get the following autopilot controller

Kp

? c
? a-
1
- K Ha (s) - - -
+6 p s
d Glat (s)
0 rc- r- 1 -
- Kr Hs (s) -
6 s

s

s + 1
U0  

1 g + 6
d

Last step: analyze eect of closing the d loop

Heading Command Loop 1=12 sec

0.8 0.68 0.5 0.25

0.88
1

0.94

0.5
0.975

0.994
Imaginary Axis

2 1.75 1.5 1.25 1 0.75 0.5 0.25

0

0.994

0.975
0.5

0.94

1
0.88

0.8 0.68 0.5 0.25

2 1.5 1 0.5 0 0.5 1 1.5 2

Real Axis

Figure 10: Heading loop root locus. Closed loop poles for gain U0 /(g1 ). 8th order
system, but RL fairly well behaved.
Fall 2004 16.333 1012

Measure with a vertical gyro and with a directional gyro

Add a limiter to d or else we can get some very large bank angles

Design variables are K, Kp, 1, , and Kr

Multiloop closure must be done carefully
Must choose the loop gains carefully so that each one is slower
than the one inside

can lead to slow overall response

Analysis on fully coupled system might show that the controllers

designed on subsystem models interact with other modes (poles)

several iterations might be required

Now need a way to specify d

Fall 2004 16.333 1013

0.6

p

0.5

0.4

0.3

0.2

0.1

0.1
0 5 10 15 20 25 30 35 40 45 50
Time (sec)
0.3

r

0.25 c

0.2

0.15

0.1

0.05

0.05

0.1
0 5 10 15 20 25 30 35 40 45 50
Time (sec)

Figure 11: Response to 15 deg step in c . Note the bank angle is approximately 30

degs, which is about the maximum allowed. Decreasing 1 tends to increase max .

Fall 2004 16.333 1014

Ground Track Control

Consider scenario shown use this to determine desired heading d

Figure 12: Heading denitions

Are initially at (0,0), moving along Xe (0 = 0)

Want to be at (0,1000) moving along dashed line angled at ref
Separation distance d = Yref Ya/c (d0 = 1000)
Desired inertial y position y position of aircraft
d = U0 sin(ref ) U0(ref )
Want to smoothly decrease d to zero, use a lter so that
1
d = d d = 30 sec
d
1
d = U0(ref )
d
1 1
= = ref + d = ref + (Yref Ya/c)
d U0 dU0

which includes a feedback on the aircraft inertial Y position.

Use this as the input d.

 Fall 2004

Mux ac3out

To Workspace

desired path
time
U0
Clock
Signal
Generator

2*pi/180 all data

desired
Psi_ref -5000
ye
path offset
Path1
desired path -K-

psi_ref Aileron Dynamics Kp

Psi_d -K- JNa(s) v

Kphi JDa(s)
Saturation p
Banking mix Gain
Rudder Dynamics Mux syslat2 emu
all data r
0 JNr(s) phi
Lateral
Constant JDr(s) 1
-K-
-K- s
psi
Washout FIlter
Integrator
psi Banking mix1
Nwash(s)
Dwash(s)

ye

Figure 13: Simulink implementation run ac3.m rst

16.333 1015

 4
Fall 2004 1.5
x 10
Y
16.333 1016
ref
Y
a/c

0.5

e
Path Y
0

0.5

1.5
0 100 200 300 400 500 600 700 800 900 1000

Figure 14: Path following for ground track controller

0.6

c

0.4

0.2

0
and c

0.2

0.4

0.6

0.8
0 100 200 300 400 500 600 700 800 900 1000

Figure 15: Roll command following for ground track controller

3

c

1
and c

3
0 100 200 300 400 500 600 700 800 900 1000

Figure 16: Heading following for ground track controller

Fall 2004 16.333 1017

Alternative Strategy

Sanghyuk Park developed an alternative tracking algorithm that he

presented this past summer1
Guidance logic selects a reference point on the desired trajectory and
uses it to generate a lateral acceleration command.
Selection of Reference Point Reference point is on the desired
path at a distance (L1) forward of the vehicle Figure 17.
2
Lateral Acceleration Command determined by ascmd = 2 VL1 sin

aircraft
V

aS L1
cmd

desired path reference point

R

Figure 17: Diagram for Guidance Logic

Direction of acceleration depends on sign of angle between the L1

line segment and the vehicle velocity vector.
If selected reference point to the right of the vehicle velocity vec
tor, then the vehicle will be commanded to accelerate to the right
(see Figure 17) vehicle will tend to align its velocity direction
with the direction of the L1 line segment.
1 Sanghyuk Park, John Deyst, and Jonathan How, A New Nonlinear Guidance Logic for Trajectory Tracking, AIAA GNC 2004.
Fall 2004 16.333 1018

as
= t
V reference point
: velocity direction change
due to the acceleration
L1
s=Vt

Figure 18: One Time Step

Vehicle Position
Reference Point
50 L1

40

Desired Path
30

20
Y

10

0 At some point, acceleration sign changes

Smooth convergence to desired path
10
0 20 40 60 80
X
Figure 19: Step by Step (t=1, V =10, and L1 =40)

Figure 18 shows evolution of the guidance logic in one time step and
Figure 19 shows the trajectory of the vehicle over several time steps.
Vehicle initially starts from a location far away from the desired
path, and eventually converges to it.
If vehicle far from the desired path, then the guidance logic rotates
the vehicle so that its velocity direction approaches the desired
path at a large angle.
If vehicle close to the desired path, then the guidance logic rotates
the vehicle so its velocity direction approaches the desired path at
a small angle.
Fall 2004 16.333 1019

Figure 20 denes the notation used for a linearization.

V
2

L1
d as 1 reference point
cmd 1
desired flight path

Figure 20: Linear Model for Straightline Following Case

L1 is the distance from the vehicle to the reference point.

d is the crosstrack error
V is the vehicle nominal speed.

Assuming is small sin = 1 + 2 and

d d
1 , 2
L1 V

Combining the above gives

2

V V V
ascmd = 2 sin 2 d + d (1)
L1 L1 L1
Linearization yields a PD controller for the crosstrack error.
Ratio of V and separation distance L1 is an important factor in
determining the proportional and derivative controller gains.
Key points: NL form works signicantly better than a PD and
is much more tolerant to winds disturbances.
Fall 2004 16.333 1020

Implementation

Can implement this command by assuming that vehicle maintains

sucient lift to balance weight, even though banked at angle .
Requires that the vehicle speed up and/or change to a larger .
Lift increment
L mg W
CL = (n 1)
QS QS

where n L/W is the load factor.

Assume that L cos = W , then L sin = mas, so that

as

tan =
g
We can use this to develop d that we apply to the roll controller.

Kp

? c
? a-
1
- K Ha (s) - - -
+6 p s
d Glat (s)
0 rc- r- 1 -
- Kr Hs (s) -
6 s

s

s + 1

Some simulations shown

Works well hardest part is determining where to place the ref
erence point, which is L1 ahead along the path.
Recall that L1 acts like a gain in the controller making it too
small can drive the aircraft unstable.
Fall 2004 16.333 1021

4
x 10
veh
Path
2

1.5

0.5

0
xe

0.5

1.5

0 1 2 3 4 5
Ye x 10
4

Figure 21: Simulation #1: path. Turn radius R V 2 /(g tan )

30

d

20

10
and d

10

20

30

0 500 1000 1500 2000

time

Figure 22: simulation #1: bank angle. Limited to 30 degs here.

Fall 2004 16.333 1022

5
x 10
3.5
veh
Path

2.5

2
xe

1.5

0.5

0
2000 0 2000 4000 6000 8000 10000 12000 14000
Ye

30

d

20

10
and d

10

20

30

0 200 400 600 800 1000 1200

time

Figure 23: simulation #2

Fall 2004 16.333 1023

Flight Test

Guidance algorithm implemented and tested with two UAVs [Parent

Child Unmanned Air Vehicle (PCUAV) project by Prof. Deyst]
Required lateral acceleration achieved using bank angle control
Nominal ight velocity of about 25 m/s, the choice of L1=150 m
results in the associated crossover frequency at 0.4 rad/s.

Figure 24 shows the ight data for the Mini vehicle using the guidance
logic in the lateral dynamics.
Plot shows the 2dimensional trajectory of the Mini vehicle ()
with a commanded desired trajectory ( ).
Small numbers along the trajectory are the ight times recorded
in the onboard avionics.
Lateral displacement between the vehicle and the desired path
within 2 meters for the 75% of its ight time and within 3
meters for 96% of the ight time
.

A similar ight test was performed for the OHS Parent (see Figure 25)
After the transient period, the trajectory of the vehicle followed
the commanded path within 2 meters for the 78% of its ight
time and within 3 meters for 97% of the ight time.
Fall 2004 16.333 1024

300
Desired Path
190 Position Trajectory
200
180

100
200

0
North [m]

170
230
100

210

200
220

300 160

150
141
400
300 200 100 0 100 200 300 400
East [m]

Figure 24: Flight Data of MINI Trajectory Following

500
Desired Path
Position Trajectory
90

400
170

300 150
North [m]

200 110 76

100

0
130

100
700 600 500 400 300 200 100
East [m]
Figure 25: Flight Data of OHS Parent Trajectory Following

Fall 2004 16.333 1025

Then demonstrated rendezvous from any arbitrary initial positions to

a conguration of tight formation ight.
Figures 26 show the positions of the Parent and the Mini in the
northeast 2D map every 10 seconds.
OHS Parent vehicle follows the circular ight path, with no knowl
edge of the Mini vehicles location.
Mini vehicle schedules its ight path and performs formation ight
by receiving position information from the OHS Parent.
300 300

200 40 [sec] 200 120 [sec]

100 O 100
NORTH [m]

NORTH [m]

0 M 0

100 100
M
O

200 200

300 300

400 400
300 200 100 0 100 200 300 400 300 200 100 0 100 200 300 400
EAST [m] EAST [m]

300 300

200 70 [sec] 200 90 [sec]

O
M

100 100
NORTH [m]

NORTH [m]

0 0

100 100

200 O 200
M

300 300

400 400
300 200 100 0 100 200 300 400 300 200 100 0 100 200 300 400
EAST [m] EAST [m]

Figure 26: Flight Data Rendezvous Trajectories of OHS and Mini (O:OHS, M:Mini)