16.333: Lecture # 13
Aircraft Longitudinal Autopilots
Altitude Hold and Landing
0
? ?
?
Fall 2004 16.333 11–1
Altitude Controller
? In linearized form, we know from 1–5 that the change of altitude h
can be written as the ?ight path angle times the velocity, so that
h
˙
≈ U
0
sin γ = U
0
(θ ? α) = U
0
θ ? U
0
w
= U
0
θ ? w
U
0
– For ?xed U
0
, h
˙
determined by variables in short period model
? Use short period model augmented with θ state
? ? ?
w
? x˙ = A
?
sp
x +
?
B
sp
δ
e
x =
?
q
?
θ
?
?
h
˙
=
?
?1 0 U
0
x
where
? ?
? ?
0
?
A
?
sp
=
A
sp
, B
sp
=
B
sp
[ 0 1 0 ]
0
? In transfer function form, we get
h K(s + 4)(s ? 3.6)
=
δ
e
s
2
(s
2
+ 2ζ
sp
ω
sp
s + ω
2
sp
)
?5 ?4 ?3 ?2 ?1 0 1 2 3 4 5?3
?2
?1
0
1
2
3
0.945
0.89
0.30.50.680.81
5
0.81
0.976
0.994
0.30.50.68
0.994
0.89
0.945
0.976
4 123
Pole?Zero Map
Real Axis
Imaginary Axis
Figure 1: Altitude root locus #1
? ?
? ?
Fall 2004 16.333 11–2
?5 ?4 ?3 ?2 ?1 0 1 2 3 4?4
?3
?2
?1
0
1
2
3
4 0.68 0.54 0.38 0.18
5
0.18
0.986
0.95
0.89
0.8
0.8 0.54 0.38
3
0.68
0.89
4 2
0.95
0.986
1
Altitude Gain Root Locus: k>0
Real Axis
Imaginary Axis
?5 ?4 ?3 ?2 ?1 0 1 2 3 4?4
?3
?2
?1
0
1
2
3
4 0.68 0.54 0.38 0.18
5
0.18
0.986
0.95
0.89
0.8
0.8 0.54 0.38
3
0.68
0.89
4 2
0.95
0.986
1
Altitude Gain Root Locus: k<0
Real Axis
Imaginary Axis
Figure 2: Altitude root locus #2
? Root locus versus h feedback clearly NOT going to work!
? Would be better o? designing an inner loop ?rst. Start with short
period model augmented with the θ state
δ
e
= ?k
w
w ?k
q
q ?k
θ
θ +δ
e
c
= k
w
k
q
k
θ
x+δ
c
= ?K
IL
x+δ
e
c
e
?
– Target pole locations s = ?1.8 ± 2.4i, s = ?0.25
– Gains: K
IL
= ?0.0017 ?2.6791 ?6.5498
?5 ?4 ?3 ?2 ?1 0 1 2 3 4?4
?3
?2
?1
0
1
2
3
4 0.8 0.68 0.54 0.38 0.18
0.54 0.18
0.986
0.95
0.89
45
0.38
0.89
0.68
2
0.8
3
0.95
0.986
1
Inner loop target poles
Real Axis
Imaginary Axis
Figure 3: Inner loop target pole locations – won’t get there with only a gain.
?
Fall 2004 16.333 11–3
? Giving the closed-loop dynamics
x˙ = A
sp
x +
??
B
sp
(?K
IL
x + δ
e
c
)
δ
c
= (A
?
sp
?
?
B
sp
e
B
sp
K
?
IL
)x +
?
h
˙
= ?1 0 U
0
x
In transfer function form ?
?
h K(s + 4)(s ?3.6)
=
δ
c
s(s + 0.25)(s
2
+ 3.6s + 9)
e
with ζ
d
and ω
d
being the result of the inner loop control.
?6 ?5 ?4 ?3 ?2 ?1 0 1?4
?3
?2
?1
0
1
2
3
4
6
0.58 0.44 0.3 0.14
0.3
0.98
0.92
0.84 0.72
4
0.44
5
0.140.84 0.58
2
0.72
3
0.92
0.98
1
CLPzeros
pole locations with inner loop
Altitude Gain Root Locus: with inner loop
Real Axis
Imaginary Axis
?0.5 ?0.4 ?0.3 ?0.2 ?0.1 0 0.1 0.2?0.5
?0.4
?0.3
?0.2
?0.1
0
0.1
0.2
0.3
0.4
0.5
0.4
0.66 0.52 0.4 0.26 0.12
0.52 0.26 0.12
0.97
0.9
0.8
0.2
0.66
0.3
0.4
0.97
0.8
0.4
0.9
0.1
0.1
0.2
0.3
CLPzeros
pole locations with inner loop
Altitude Gain Root Locus: with inner loop
Real Axis
Imaginary Axis
Figure 4: Root loci versus altitude gain K
h
< 0 with inner loop added (zoomed on
right). Much better than without inner loop, but gain must be small (K
h
≈?0.01).
? Final step then is to select the feedback gain on the altitude K
h
and
implement (h
c
is the commanded altitude.)
δ
c
= ?K
h
(h
c
?h)
e
– Design inner loop to damp the short period poles and move one
of the poles near the origin.
– Then select K
h
to move the 2 poles near the origin.
Fall 2004 16.333 11–4
? Poles near origin dominate response s = ?0.1056 ± 0.2811i
? ω
n
= 0.3, ζ = 0.35
– Rules of thumb for 2nd order response:
1 + 1.1ζ + 1.4ζ
2
10-90% rise time t
r
=
ω
n
3
Settling time (5%) t
s
=
ζω
n
Time to peak amplitude t
p
=
?
π
ω
n
1 ? ζ
2
Peak overshoot M
p
= e
?ζω
n
t
p
0 10 20 30 40 50 60?0.2
0
0.2
0.4
0.6
0.8
1
1.2
1.4 Altitude controller
height
time
Figure 5: Time response for the altitude controller.
? Predictions: t
r
= 5.2sec, t
s
= 28.4sec, t
p
= 11.2sec, M
p
= 0.3
? Now with h feedback, things are a little bit better, but not much.
– Real design is complicated by the location of the zeros
– Any actuator lag is going to hinder the performance also.
? Typically must tweak inner loop design for altitude controller to
work well.
Fall 2004 16.333 11–5
0 50 100 150 200 250 300?20
0
20
40
60
80
100
120 Altitude controller
height
time
hc
h
Figure 6: Altitude controller time response for more complicated input.
? The gain K
h
must be kept very small since the low frequency poles
are heading into the RHP.
? Must take a di?erent control approach to design a controller for the
full dynamics.
– Must also use the throttle to keep tighter control of the speed.
– Example in Etkin and Reid, page 277. Simulink and Matlab code
for this is on the Web page.
Fall 2004 16.333 11–6
Altitude Controller
Completes Figure 8.17 in Etkin and Reid
use with altit_simul.m
T=alt(:,1);u=alt(:,2);w=alt(:,3);
q=alt(:,4);theta=alt(:,5);h=alt(:,6);
dele=alt(:,7);delt=alt(:,8);href=alt(:,9);
K_h*Khn(s)
Khd(s)
height Lead
alt
To Workspace
Sum4
Signal 2
Signal Builder
Signal
Generator
Scope1
Scope
Saturation
Sat
Ramp
Mux
Mux
Mux
Dele
Delt
u
w
q
theta
h
all
Longitudinal Model
-K-
K_u
K_th
K_q
Kun(s)
Kud(s)
Engine Lead
JNt(s)
JDt(s)
Engine Dynamics
JNe(s)
JDe(s)
Elevator Lag
0
Constant1
0
Clock
u
u_ref
Figure 7: Simulink block diagram for implementing the more advanced version of
the altitude hold controller. Follows and extends the example in Etkin and Reid,
page 277.
Fall 2004
?3 ?2.5 ?2 ?1.5 ?1 ?0.5 0?3
?2
?1
0
1
2
3
0.82
0.66 0.52 0.4 0.28 0.18 0.09
3
0.52 0.4 0.28 0.18 0.09
0.94
0.5
0.82
0.66
2
2.5
0.94
1.5
1
1
2
1.5
0.5
2.5
3with q and theta FB to de
Real Axis
Imaginary Axis
16.333 11–7
Figure 8: Root loci associated for simulation (q, θ to δ
e
).
?2 ?1.8 ?1.6 ?1.4 ?1.2 ?1 ?0.8 ?0.6 ?0.4 ?0.2 0?2
?1.5
?1
?0.5
0
0.5
1
1.5
2
0.94
0.82
0.66 0.52 0.4 0.28 0.18 0.09
0.94
0.66 0.52 0.4 0.28 0.18 0.09 1.75
0.25
2
0.82
1
0.5
1.25
0.75
1.25
0.75
1.5
1.5
1
0.5
1.75
2
0.25
with u FB to dt
Real Axis
Imaginary Axis
Figure 9: Root loci associated with simulation (u to δ
t
). There is more authority in
the linear control analysis, but this saturates the nonlinear simulation.
?1 ?0.9 ?0.8 ?0.7 ?0.6 ?0.5 ?0.4 ?0.3 ?0.2 ?0.1 0 0.1?2
?1.5
?1
?0.5
0
0.5
1
1.5
2
0.82
0.6
0.42 0.31 0.22 0.16 0.1 0.045
0.6
0.31 0.22 0.16 0.1 0.0451.75
0.82
2
0.42
0.75
0.25
1.25
0.5
1
0.75
1.5
1.25
0.25
0.5
1
2
1.5
1.75
with h FB to de command
Real Axis
Imaginary Axis
Figure 10: Root loci associated with simulation (h to δ
e
). Lead controller
Fall 2004 16.333 11–8
0 2 4 6 8 10 12 14 16 18?20
0
20
40
60
80
100
Initial Condition response to 90m altitude error. E?FB
H
5*U
q deg
0 2 4 6 8 10 12 14 16 18?20
?15
?10
?5
0
5
10
15
20
Commands (degs)
Initial Condition response to 90m altitude error
de input
20*dt input
Figure 11: Initial condition response using elevator controller. Note the initial ele-
vator e?ort and that the throttle is scaled back as the speed picks up.
Fall 2004 16.333 11–9
0 50 100 150 200 250 300
0
200
400
600
800
1000
1200
1400
1600
time
height
Altitude controller: elevator FB
hc
h
Figure 12: Command following ramps up and down. O?set, but smooth transitions.
0 50 100 150 200 250 300?5
0
5
Linear Response ? elevator?FB
U
a deg
q deg
0 50 100 150 200 250 300?3
?2
?1
0
1
2
3
de
10*dt
Figure 13: Elevator controller – Overall response and inputs
Fall 2004 16.333 11–10
0 50 100 150 200 250 300?10
?5
0
5
U
a deg
q deg
0 50 100 150 200 250 300
0
200
400
600
800
1000
1200
1400
1600
Href
H
0 50 100 150 200 250 300?4
?3
?2
?1
0
1
2
3
4
de
10*dt
Figure 14: Nonlinear simulation - Note: throttle at the limits, speed not well main-
tained, but path similar to linear simulation.
K_hu*Khnu(s)
Khdu(s)
height Lead
altu
To Workspace
Sum4
Signal 2
Signal Builder
Signal
Generator
Scope1
Scope
Saturation
Sat
Ramp
Mux
Mux
Mux
Dele Delt
u
w
q
theta
h
all
Longitudinal Model
-K-
K_u
K_th
K_q
Kun(s) Kud(s)
Engine Lead
JNt(s) JDt(s)
Engine Dynamics
JNe(s) JDe(s)
Elevator Lag
0
Constant1
Clock
u
Fall 2004 16.333 11–11
? Recall: could have used throttle to control γ directly (see 6–9)
– Alternative altitude control strategy
– However, engine lag dynamics are much slower.
Figure 15: Altitude Controller using FB to the thrust.
?1 ?0.9 ?0.8 ?0.7 ?0.6 ?0.5 ?0.4 ?0.3 ?0.2 ?0.1 0 0.1?2
?1.5
?1
?0.5
0
0.5
1
1.5
2
0.82
0.6
0.42 0.31 0.22 0.16 0.1 0.045
0.6
0.31 0.22 0.16 0.1 0.045 1.75
0.82
2
0.42
0.75
0.25
1.25
0.5
1
0.75
1.5
1.25
0.25
0.5
1
2
1.5
1.75
with h FB to dt command
Real Axis
Imaginary Axis
Figure 16: Root loci associated with simulation (h to δ
t
). Lead controller
Fall 2004 16.333 11–12
0 2 4 6 8 10 12 14 16 18?40
?20
0
20
40
60
80
100
Initial Condition response to 90m altitude error. U ? FB
H
U
q deg
0 2 4 6 8 10 12 14 16 18?20
?15
?10
?5
0
5
10
15
20
Command inputs (degs)
de input
dt input
Figure 17: Initial condition response (linear sim) using thrust controller. Slows down
to descend. Much slower response, but throttle commands still quite large (beyond
saturation of ±0.2)
Fall 2004 16.333 11–13
0 50 100 150 200 250 300
0
200
400
600
800
1000
1200
1400
1600
time
height
Altitude controller: thrust FB
hc
h
Figure 18: Command following ramps up and down. O?set, but smooth transitions.
0 50 100 150 200 250 300?40
?20
0
20
40
Linear Response ? thrust?FB
U
5*a deg
5*q deg
0 50 100 150 200 250 300?2
?1
0
1
2
de
dt
Figure 19: Thrust controller – Overall response and inputs. Linear response looks
good, but commands are very large.
Fall 2004 16.333 11–14
0 50 100 150 200 250 300?30
?20
?10
0
10
20
30
U
a deg
q deg
0 50 100 150 200 250 300
0
200
400
600
800
1000
1200
1400
1600
Href
H
0 50 100 150 200 250 300?2
?1.5
?1
?0.5
0
0.5
1
1.5
2
de
dt
Figure 20: Nonlinear simulation - throttle saturations limit path following perfor-
mance.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
Fall 2004 16.333 11–15
%
% altitude hold controller
%
clear all
prt=1;
for ii=1:9
figure(ii);clf;
set(gcf,’DefaultLineLineWidth’,2);
set(gcf,’DefaultlineMarkerSize’,10)
end
Xu=-1.982e3;Xw=4.025e3;
Zu=-2.595e4;Zw=-9.030e4;Zq=-4.524e5;Zwd=1.909e3;
Mu=1.593e4;Mw=-1.563e5;Mq=-1.521e7;Mwd=-1.702e4;
g=9.81;theta0=0;S=511;cbar=8.324;
U0=235.9;Iyy=.449e8;m=2.83176e6/g;cbar=8.324;rho=0.3045;
Xdp=.3*m*g;Zdp=0;Mdp=0;
Xde=-3.818e-6*(1/2*rho*U0^2*S);Zde=-0.3648*(1/2*rho*U0^2*S);
Mde=-1.444*(1/2*rho*U0^2*S*cbar);;
%
%
% use the full longitudinal model
%
% x=[u w q theta h];
% u=[de;dt];
sen_u=1;sen_w=2;sen_q=3;sen_t=4;sen_h=5;sen_de=6;sen_dt=7;
act_e=1;act_t=2;
%
% dot h = U0 (theta - alpha) = U0 theta - w
%
A=[Xu/m Xw/m 0 -g*cos(theta0) 0;[Zu Zw Zq+m*U0 -m*g*sin(theta0)]/(m-Zwd) 0;
[Mu+Zu*Mwd/(m-Zwd) Mw+Zw*Mwd/(m-Zwd) Mq+(Zq+m*U0)*Mwd/(m-Zwd) ...
-m*g*sin(theta0)*Mwd/(m-Zwd) 0]/Iyy;
[ 0 0 1 0 0];[0 -1 0 U0 0]];
B=[Xde/m Xdp/m;Zde/(m-Zwd) Zdp/(m-Zwd);(Mde+Zde*Mwd/(m-Zwd))/Iyy ...
(Mdp+Zdp*Mwd/(m-Zwd))/Iyy;0 0;0 0];
C=[eye(5);zeros(2,5)];
D=[zeros(5,2);[eye(2)]]; %last 2 outputs are the controls
%
% add actuator dynamics
%
% elevator
%tau_e=.1;%sec
tau_e=.25;%sec
JNe=1;JDe=[tau_e 1];
syse=tf(JNe,JDe);
%
% thrust
%
tau_t=3.5;%sec
JNt=1;JDt=[tau_t 1];
syst=tf(JNt,JDt);
sysd=append(syse,syst);
syslong=ss(A,B,C,D);
syslong2=series(sysd,syslong);
[A,B,C,D]=ssdata(syslong2);
na=size(A);
%
% q and theta loops
%
% inner loop on theta/q
% u=kq*q+Kth*theta + de_c
K_th=1;K_q=1.95*K_th;
if 1
figure(1);clf;%orient tall
sgrid([.5 .707]’,[1]);
rlocus(tf(-[1.95 1],1)*tf(syslong2(sen_t,act_e)))
r_th=rlocus(tf(-[1.95 1],1)*tf(syslong2(sen_t,act_e)),K_th)
hold on
plot(r_th+eps*j,’bd’,’MarkerFace’,’b’);axis([-3,1,-3,3]);hold off
axis([-3 .1 -3 3])
Fall 2004 16.333 11–16
73 title(’with q and theta FB to \delta_e’);
74 if prt
75 print -depsc alt_sim1.eps
76 jpdf(’alt_sim1’)
77 end
78 end
79 % close inner loop
80 syscl=feedback(syslong2,[K_q K_th],act_e,[sen_q sen_t],1)
81 [eig(syscl) [nan;r_th;nan]]
82 [Acl,Bcl,Ccl,Dcl]=ssdata(syscl);
83 %
84 % engine loops
85 % mostly the phugoid ==> design on top of the short period
86 %
87 % system with q/th loop feedback
88 figure(9);clf
89 % interested in vel output and engine input
90 sys_u=syscl(sen_u,act_t);
91 Kun=[1/0.2857 1]; % comp zero on plant pole
92 Kud=[1/(0.2857*5) 1];
93 [Au,Bu,Cu,Du]=tf2ss(Kun,Kud);
94 %K_u=.075;
95 %K_u=.35;
96 K_u=.1;
97 rlocus(sys_u*tf(Kun,Kud));
98 rr_u=rlocus(sys_u*tf(Kun,Kud),K_u);
99 hold on;plot(rr_u+eps*j,’bd’,’MarkerFace’,’b’);hold off;
100
101 if 1
102 na=size(Acl,1);
103 Aclt=[Acl Bcl(:,act_t)*Cu;zeros(1,na) Au];
104 Bclt=[[Bcl(:,act_e);0] [Bcl(:,act_t)*Du;Bu]];
105 Cclt=[Ccl zeros(size(Ccl,1),1)];
106 Dclt=[zeros(size(Ccl,1),2)];
107 figure(2);clf;axis([-.5 .05,-.4,.4]);sgrid([.5 .707]’,[.05]);hold on;
108 rlocus(Aclt,Bclt(:,act_t),Cclt(sen_u,:),Dclt(sen_u,act_t));
109 axis([-2 .05,-2,2])
110 r_u=rlocus(Aclt,Bclt(:,act_t),Cclt(sen_u,:),Dclt(sen_u,act_t),K_u)’;
111 [[r_th;NaN;NaN;NaN] r_u]
112 hold on;plot(r_u+eps*j,’bd’,’MarkerFace’,’b’),hold off
113 title(’with u FB to \delta_t’)
114 if prt
115 print -depsc alt_sim2.eps
116 jpdf(’alt_sim2’)
117 end
118 end
119 Acl2=Aclt-Bclt(:,act_t)*K_u*Cclt(sen_u,:);
120 Bcl2=Bclt;
121 Ccl2=Cclt;
122 Dcl2=Dclt;
123 sysclt=ss(Acl2,Bcl2,Ccl2,Dcl2);
124 [eig(sysclt) rr_u]
125 %
126 % now close loop on altitude
127 %
128 % de_c = kh*(h_c-h)
129 if 1
130 % design lead by canceling troubling plant pole
131 % zero located p*8
132 tt=eig(sysclt);[ee,ii]=min(abs(tt+.165));
133 Khn=[1/abs(tt(ii)) 1];Khd=[1/(8*abs(tt(ii))) 1];
134 K_h=-1*.00116;
135 Gc_eng=tf(Khn,Khd);
136 Loopt=series(append(Gc_eng,tf(1,1)),sysclt);
137 figure(3);clf;
138 sgrid([.5 .707]’,[.1:.1:1]);
139 hold on;
140 rlocus(sign(K_h)*Loopt(sen_h,act_e));
141 axis([-1 .1, -2 2])
142 r_h=rlocus(Loopt(sen_h,act_e),K_h)
143 hold on;plot(r_h+eps*j,’bd’,’MarkerFace’,’b’),hold off
144 title(’with h FB to \delta_e command’)
Fall 2004 16.333 11–17
145 if prt
146 print -depsc alt_sim22.eps
147 jpdf(’alt_sim22’)
148 end
149 end
150 syscl3=feedback(series(append(tf(K_h,1),tf(1,1)),Loopt),[1],act_e,sen_h,-1);
151 [r_h eig(syscl3)]
152 %
153 % try using the thrust act to control altitude
154 %
155 if 1
156 % K_hu=.154;
157 K_hu=0.025;
158 tt=eig(sysclt);[ee,ii]=min(abs(tt+.165));
159 Khnu=[1/abs(tt(ii)) 1];Khdu=[1/(5*abs(tt(ii))) 1];
160 Gc_engu=tf(Khnu,Khdu);
161 Loopt=series(append(tf(1,1),Gc_engu),sysclt);
162 figure(7);clf;axis([-.6 .1,-.5,.5]);sgrid([.5 .707]’,[.05]);hold on;
163 rlocus(sign(K_hu)*Loopt(sen_h,act_t));
164 axis([-1 .1,-2,2])
165 r_hu=rlocus(Loopt(sen_h,act_t),K_hu)
166 hold on;plot(r_hu+eps*j,’bd’,’MarkerFace’,’b’),hold off
167 title(’with h FB to \delta_t command’)
168 if prt
169 print -depsc alt_sim22a.eps
170 jpdf(’alt_sim22a’)
171 end
172 end
173 syscl4=feedback(series(append(tf(1,1),tf(K_hu,1)),Loopt),[1],act_t,sen_h,-1);
174 [r_hu eig(syscl4)]
175
176 t=[0:.01:18]’;
177 [ystep,t]=initial(syscl3,[0 0 0 0 90 0 0 0 0],t);
178 figure(4);clf;orient tall;
179 subplot(211)
180 ll=1:length(t);
181 U=ystep(:,sen_u);W=ystep(:,sen_w);q=ystep(:,sen_q);
182 TH=ystep(:,sen_t);H=ystep(:,sen_h);
183 DELe=ystep(:,sen_de);DELt=ystep(:,sen_dt);
184 plot(t(ll),H(ll),t(ll),5*U(ll),t(ll),TH(ll)*180/pi);setlines(2)
185 title(’Initial Condition response to 90m altitude error. E-FB’)
186 legend(’H’,’5*U’,’\theta deg’);grid
187 subplot(212)
188 plot(t(ll),DELe(ll)*180/pi,t(ll),20*DELt(ll));
189 setlines(2)
190 legend(’\delta_e input’,’20*\delta_t input’);grid
191 axis([0 18 -20 20])
192 ylabel(’Commands (degs)’)
193 title(’Initial Condition response to 90m altitude error’)
194 if prt
195 print -depsc alt_sim3.eps
196 jpdf(’alt_sim3’)
197 end
198 %
199 t=[0:.01:18]’;
200 [ystep,t]=initial(syscl4,[0 0 0 0 90 0 0 0 0],t);
201 figure(14);clf;orient tall;
202 subplot(211)
203 ll=1:length(t);
204 U=ystep(:,sen_u);W=ystep(:,sen_w);q=ystep(:,sen_q);
205 TH=ystep(:,sen_t);H=ystep(:,sen_h);
206 DELe=ystep(:,sen_de);DELt=ystep(:,sen_dt);
207 plot(t(ll),H(ll),t(ll),U(ll),t(ll),TH(ll)*180/pi);setlines(2)
208 title(’Initial Condition response to 90m altitude error. U - FB’)
209 legend(’H’,’U’,’\theta deg’);grid
210 subplot(212)
211 plot(t(ll),DELe(ll)*180/pi,t(ll),DELt(ll));
212 setlines(2)
213 legend(’\delta_e input’,’\delta_t input’);grid
214 axis([0 18 -20 20])
215 ylabel(’Command inputs (degs)’)
216 if prt
1
2
3
4
5
6
7
8
Fall 2004 16.333 11–18
217 print -depsc alt_sim3a.eps
218 jpdf(’alt_sim3a’)
219 end
220
221 figure(5);clf
222 %Path to follow
223 Hmax=1500;
224 Hc=[zeros(10,1);Hmax/100*[0:1:100]’;Hmax*ones(50,1);Hmax/100*[100:-1:0]’;zeros(50,1)];
225 T=0:length(Hc)-1;
226 [Yh,T]=lsim(syscl3(:,1),Hc,T);
227 U=Yh(:,sen_u);W=Yh(:,sen_w);q=Yh(:,sen_q);TH=Yh(:,sen_t);H=Yh(:,sen_h);
228 DELe=Yh(:,sen_de);DELt=Yh(:,sen_dt);
229 plot(T,[Hc],T,H,’--’);setlines(2)
230 axis([0 300 -100 1600])
231 legend(’h_c’,’h’)
232 title(’Altitude controller: elevator FB’)
233 ylabel(’height’)
234 xlabel(’time’)
235 if prt
236 print -depsc alt_sim4.eps
237 jpdf(’alt_sim4’)
238 end
239
240 figure(6);clf;
241 subplot(211)
242 ll=1:length(T);
243 plot(T(ll),U(ll),T(ll),W(ll)/U0*180/pi,T(ll),TH(ll)*180/pi);setlines(2)
244 title(’Linear Response - elevator-FB’)
245 legend(’U’,’\alpha deg’,’\theta deg’);grid
246 axis([0 300 -5 5])
247 subplot(212)
248 plot(T(ll),DELe(ll)*180/pi,T(ll),10*DELt(ll));
249 setlines(2)
250 axis([0 300 -3 3])
251 legend(’\delta_e’,’10*\delta_t’);grid
252 %axis([0 250 -4 4])
253 if prt
254 print -depsc alt_sim5.eps
255 jpdf(’alt_sim5’)
256 end
257
258 figure(15);clf
259 %Path to follow
260 Hmax=1500;
261 Hc=[zeros(10,1);Hmax/100*[0:1:100]’;Hmax*ones(50,1);Hmax/100*[100:-1:0]’;zeros(50,1)];
262 T=0:length(Hc)-1;
263 [Yh,T]=lsim(syscl4(:,2),Hc,T);
264 U=Yh(:,sen_u);W=Yh(:,sen_w);q=Yh(:,sen_q);TH=Yh(:,sen_t);H=Yh(:,sen_h);
265 DELe=Yh(:,sen_de);DELt=Yh(:,sen_dt);
266 plot(T,[Hc],T,H,’--’);setlines(2)
267 axis([0 300 -100 1600])
268 legend(’h_c’,’h’)
269 title(’Altitude controller: thrust FB’)
270 ylabel(’height’)
271 xlabel(’time’)
272 if prt
273 print -depsc alt_sim4a.eps
274 jpdf(’alt_sim4a’)
275 end
276
277 figure(16);clf;
278 subplot(211)
279 ll=1:length(T);
280 plot(T(ll),U(ll),T(ll),5*W(ll)/U0*180/pi,T(ll),5*TH(ll)*180/pi);setlines(2)
28 title(’Linear Response - thrust-FB’)
28 legend(’U’,’5*\alpha deg’,’5*\theta deg’);grid
28 axis([0 300 -40 40])
28 subplot(212)
28 plot(T(ll),DELe(ll)*180/pi,T(ll),DELt(ll));
28 setlines(2)
28 legend(’\delta_e’,’\delta_t’);grid
28 axis([0 300 -2 2])
1
2
3
4
5
6
7
8
9
1
2
3
4
5
Fall 2004 16.333 11–19
289 %axis([0 250 -4 4])
290 if prt
291 print -depsc alt_sim5a.eps
292 jpdf(’alt_sim5a’)
293 end
294
295 return
296
297 sT=alt(:,1);u=alt(:,2);w=alt(:,3);
298 q=alt(:,4);theta=alt(:,5);h=alt(:,6);
299 dele=alt(:,7);delt=alt(:,8);href=alt(:,9);
300
301 figure(17);clf;orient tall
302 ll=1:length(sT)-2;
303 subplot(311)
304 title(’Simulink hard path following: elevator FB’)
305 plot(sT(ll),u(ll),sT(ll),w(ll)/U0*180/pi,sT(ll),theta(ll)*180/pi);setlines(2)
306 legend(’U’,’\alpha deg’,’\theta deg’);grid
307 subplot(312)
308 plot(sT(ll),[href(ll) h(ll)])
309 setlines(2)
310 legend(’H_{ref}’,’H’);grid
311 axis([0 300 -100 1600])
312 subplot(313)
313 plot(sT(ll),dele(ll)*180/pi,sT(ll),10*delt(ll));
314 setlines(2)
315 legend(’\delta_e’,’10*\delta_t’);grid
316 %axis([0 60 -1000 600])
317 if prt
318 print -depsc alt_sim6.eps
319 jpdf(’alt_sim6’)
320 end
321
322 sTu=altu(:,1);uu=altu(:,2);wu=altu(:,3);
323 qu=altu(:,4);thetau=altu(:,5);hu=altu(:,6);
324 deleu=altu(:,7);deltu=altu(:,8);hrefu=altu(:,9);
325
326 figure(18);clf;orient tall
327 ll=1:length(sTu)-2;
328 subplot(311)
329 title(’Simulink hard path following: thruster FB’)
330 plot(sTu(ll),uu(ll),sTu(ll),wu(ll)/U0*180/pi,sTu(ll),thetau(ll)*180/pi);setlines(2)
33 legend(’U’,’\alpha deg’,’\theta deg’);grid
33 subplot(312)
33 plot(sTu(ll),[hrefu(ll) hu(ll)])
33 setlines(2)
33 legend(’H_{ref}’,’H’);grid
33 axis([0 300 -100 1600])
33 subplot(313)
33 plot(sTu(ll),deleu(ll)*180/pi,sTu(ll),deltu(ll));
33 setlines(2)
340 legend(’\delta_e’,’\delta_t’);grid
34 %axis([0 60 -1000 600])
34 if prt
34 print -depsc alt_sim6a.eps
34 jpdf(’alt_sim6a’)
34 end
Fall 2004 16.333 11–20
Summary
? Multi-loop closure complicated
– Requires careful consideration of which sensors and actuators to
use together.
– Feedback gain selection often unclear until full controller is done -
balance between performance and control authority (saturation).
– Design of successive loops can interact.
? Could design it by hand, identify good closed-loop pole locations,
and then use state feedback controller to design a fully integrated
controller.
? Nonlinear simulations important to at least capture impact of the
saturations.
Fall 2004 16.333 11–21
Autolanding Controller
? A related, but even more complicated scenario is the problem of land-
ing an aircraft onto a runway
– Consider the longitudinal component only, but steering to get
aligned with the runway is challenging too.
u
o
?
d
R
?
u
o
Sin
u
D
Glide path centerline
Figure 21: Glide slope basic terminology. Runway is to the right, and the glide slope
intersects the ground there. Aircraft is distance R from the runway.
? Aircraft below intended ?ight path (i.e. o?set d > 0)
? Desired glide slope γ
r
< 0, and actual glide slope given by γ < 0
? Di?erence between desired and actual glide slopes:
Γ = γ ?γ
r
In this con?guration, γ
r
≈?3
?
and γ ≈?2
?
, so Γ ≈ 1
?
> 0.
? Deviation d grows as a function of Γ – projection of U
0
onto a normal
of the desired glide slope
d
˙
= ?U
0
sin Γ = ?U
0
sin(γ ?γ
r
) ≈ U
0
(γ
r
?γ)
– Γ > 0 reduces d
Fall 2004 16.333 11–22
? Along the glide path, we must control the pitch attitude and speed
– Large number of loops must be closed.
– Similar to altitude controller, but now concerned about how well
we track the ramp slope down ? will also use feedback of sepa-
ration distance d.
Figure 22: Coded signals from two stations at 90 & 150 MHz indicate whether
you are above or below the desired glide slope. The 75 MHz signals (outer/inner
markers) give a 4 mile and 3500 ft warning.
? Can measure the angle deviation from the glide slope
– Crudely using lights.
– More precisely using an ILS (instrument landing system).
? If d not directly measurable, the can write that
d d
sin Γ =
R
? Γ ≈
R
and do feedback on Γ, which is measurable from ILS.
– Controller gains must then change with range R. Complex, but
feasible (gain scheduled control)
? With GPS can measure the vehicle X, Y, Z position to (2-5m)
Fall 2004 16.333 11–23
Glide
Glide path
Glide slope
angle
slope transmitter
Runway
Flare path
Figure 23: Flare path.
? Other complications in this control problem:
– We cannot just ?y into the ground along this glide slope. The
vertical velocity (sink rate) is too high (10 ft/sec).
? Need to ?are to change the sink rate to a level that is consistent
with the capabilities of the landing gear (2 ft/sec).
– Aircraft con?guration changes when we deploy the ?aps and slats,
so we need to change our model of the dynamics.
? Flare Control: At some decision height (≈70 ft) we will change the
desired trajectory away from glide slope following to direct control of
the altitude.
– Want the height h(t) to follow a smooth path to the ground
h
ref
(t) = h
0
e
?t/τ
, τ = 3 ? 10 sec
which implies that h
˙
ref
= ?h
ref
/τ .
– Need to ?nd the decision height h
0
.
t=Lan(:,1);u=Lan(:,2);w=Lan(:,3);q=Lan(:,4);
th=Lan(:,5);d=Lan(:,6);gam=Lan(:,7);
ele=Lan(:,8);thr=Lan(:,9);H=Lan(:,10);Hr=Lan(:,11);
figure(1);plot(U0*t,[H Hr]);setlines;xlabel('Position');ylabel('height')
title(['tau_f= ',num2str(tau_f)]);legend('height','ref height')
Landing glide slope control
with the Flare added at the end
For use with land2.m
Prof How, 16.333, Fall 2004
Lan
Work
Switch
Saturation
K_th*Ktn(s)
Ktd(s)
Pitch controller
Mux
Mux1
Mux Mux
Dele Delt gam_ref
u
w
q
theta
d
gama
all
Longitudinal Model
1 s
1 s
Href
Hdot
Gamma_ref for glide slope
K_q
U0
2
U0
In1
In3
Ou t1
FLARE
K_u*Kun(s)
Kud(s)
Engine controller
JNt(s) JDt(s)
Engine Dynamics
JNe(s) JDe(s)
Elevator Lag
K_d*Kdn(s)
Kdd(s)
D controller
0
0
Clock1
Clock
d
d_c
11
u
6
6
h
h
slope
Href
Href
2
Fall 2004 16.333 11–24
? We start the ?are when the height above the runway matches the
value we would get from the ?are calculation using the current descent
rate h
˙
, i.e. when
h(t) = ?τh
˙
(t) = τ U
0
sin(?γ
r
) > 0
– De?nes the decision height h
0
– Ensures a smooth transition at the start of the ?are.
? Landing the aircraft requires a complex combination of δ
e
and δ
t
to
coordinate the speed/pitch/height control.
Figure 24: Simulink block diagram for implementing the autoland controller. Follows
and extends the example in Etkin and Reid, page 277.
Fall 2004 16.333 11–25
? Simulation based on the longitudinal model with elevator and throttle
lags. (See also Stevens and Lewis, page 300)
– Tight q and θ loops
– Engine loop designed using Phugoid model, tries to maintain con-
stant speed.
? Simulation explicitly switches the reference commands to the con-
troller when we reach the decision height.
– Could change the controller gains, but current code does not.
– Above decision height, feedback is on d with the commands used
to generate reference pitch commands.
? At the decision height, the control switches in two ways:
– Flare program initiated ? generates h
ref
.
– Switch from d feedback to feedback on altitude error h ? h
ref
.
? With τ
f
= 8, start ?are at about 77 ft (60 sec). Need tighter control
to lower τ
f
further.
Fall 2004 16.333 11–26
0 10 20 30 40 50 60 70 80 90 100?100
0
100
200
300
400
500
600
700
time
Href
H
Figure 25: Typical results from the simulink block diagram. Slight initial error, but
0 10 20 30 40 50 60 70 80 90 100?8
?6
?4
?2
0
2
4
6
8
10
12
time
Control Signals
tauf= 10
de degs
10*dt
then the aircraft locks onto the glideslope. Follows ?are ok, but not great.
Figure 26: Control signals. Positive elevator pitches aircraft over to initiate descent,
and then elevator up (negative) at 60sec pitches nose up as part of the ?are.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
Fall 2004 16.333 11–27
Landing Code
%
% landing Case I (etkin and reid)
% for use within landing3.mdl
%
% from reid
clear all
prt=1;
for ii=1:9
figure(ii);clf;
set(gcf,’DefaultLineLineWidth’,2);
set(gcf,’DefaultlineMarkerSize’,10)
end
% case 1, page 371 reid
Xub=-3.661e2;Xwb=2.137e3;Xqb=0;Xwdb=0;
Zub=-3.538e3;Zwb=-8.969e3;Zqb=-1.090e5;Zwdb=5.851e2;
Mub=3.779e3;Mwb=-5.717e4;Mqb=-1.153e7;Mwdb=-7.946e3;
Xdeb=1.68e4;Zdeb=-1.125e5;Mdeb=-1.221e7;
g=32.2;U0=221;Iyy=3.23e7;theta0=0;S=5500;m=5.640e5/g;cbar=27.31;
alphae=-8.5*pi/180;
CA=cos(alphae);SA=sin(alphae);
%
% convert to stability axes - page 356
%
Xu=Xub*CA^2+Zwb*SA^2+(Xwb+Zub)*SA*CA;
Xw=Xwb*CA^2-Zub*SA^2-(Xub-Zwb)*SA*CA;
Zu=Zub*CA^2-Xwb*SA^2-(Xub-Zwb)*SA*CA;
Zw=Zwb*CA^2+Xub*SA^2-(Xwb+Zub)*SA*CA;
Mu=Mub*CA+Mwb*SA;
Mw=Mwb*CA-Mub*SA;
Xq=Xqb*CA+Zqb*SA;
Zq=Zqb*CA-Xqb*SA;
Mq=Mqb;
Xwd=Xwdb*CA^2+Zwdb*SA*CA;
Zwd=Zwdb*CA^2-Xwdb*SA*CA;
Mwd=Mwdb*CA;
Xde=Xdeb*CA+Zdeb*SA;
Zde=Zdeb*CA-Xdeb*SA;
Mde=Mdeb;
Xdp=.3*m*g;Zdp=0;Mdp=0;
%x=[u w q th d]
%
% full longitudinal model
%
AL=[Xu/m Xw/m Xq/m -g*cos(theta0) 0;[Zu Zw Zq+m*U0 -m*g*sin(theta0)]/(m-Zwd) 0;
[Mu+Zu*Mwd/(m-Zwd) Mw+Zw*Mwd/(m-Zwd) Mq+(Zq+m*U0)*Mwd/(m-Zwd) ...
-m*g*sin(theta0)*Mwd/(m-Zwd)]/Iyy 0;
[ 0 0 1 0 0];[0 1 0 -U0 0]]; %changed
BL=[Xde/m Xdp/m 0;Zde/(m-Zwd) Zdp/(m-Zwd) 0;(Mde+Zde*Mwd/(m-Zwd))/Iyy ...
(Mdp+Zdp*Mwd/(m-Zwd))/Iyy 0;0 0 0;0 0 U0]; %changed
if 1
[V,ev]=eig(AL);ev=diag(ev);D=V;
%
% Short-period Approx
%
Asp=[Zw/m U0;
[Mw+Zw*Mwd/m Mq+U0*Mwd]/Iyy];
Bsp=[Zde/m;(Mde+Zde/m*Mwd)/Iyy];
[nsp,dsp]=ss2tf(Asp,Bsp,eye(2),zeros(2,1));
[Vsp,evsp]=eig(Asp);evsp=diag(evsp);
Ap=[Xu/m -g;-Zu/(m*U0) 0];
Bp=[(Xde-(Xw/Mw)*Mde)/m Xdp/m-Xw/Mw*Mdp/m;(-Zde+(Zw/Mw)*Mde)/m/U0 (-Zdp+(Zw/Mw)*Mdp)/m/U0];
[Vp,evp]=eig(Ap);evp=diag(evp);
end
% x=[u w q theta d];
1
2
3
4
5
6
7
8
9
Fall 2004 16.333 11–28
68 % u=[de;dt];
69 sen_u=1;sen_w=2;sen_q=3;sen_t=4;sen_d=5;sen_g=6;
70 act_e=1;act_t=2;act_gr=3;
71
72 CL=[eye(5);0 -1/U0 0 1 0]; %adds a gamma output
73 DL=zeros(6,3);
74 %
75 % y=[u w q th d gamma]
76 %
77 % CONTROL
78 % add actuator dynamics
79 %
80 % elevator
81 tau_e=.1;%sec
82 JNe=1;JDe=[tau_e 1];
83 syse=tf(JNe,JDe);
84 %
85 % thrust
86 %
87 tau_t=3.5;%sec
88 JNt=1;JDt=[tau_t 1];
89 syst=tf(JNt,JDt);
90 sysd=append(syse,syst,1);
91 syslong=ss(AL,BL,CL,DL);
92 syslong2=series(sysd,syslong);
93 [A,B,C,D]=ssdata(syslong2);
94 na=size(A);
95 %
96 % q and theta loops
97 % using full model
98 % inner loop on theta/q
99 % u=kq*q+Kth*theta + de_c
100 %
101 % higher BW than probably needed
102 % numerator from zero placement
103 % and den from pole at 0 and one at 10
104 %
105 K_th=-6;K_q=-1.5;
106 if 1
107 Ktn=[1 1.4 1];Ktd=conv([1/5 1],[1 0]);
108 [Ath,Bth,Cth,Dth]=tf2ss(Ktn,Ktd);
109 na=size(A,1);nt=size(Ath,1);
110 At=[A-B(:,act_e)*K_q*C(sen_q,:) B(:,act_e)*Cth;zeros(nt,na) Ath];
111 Bt=[[B(:,act_e)*Dth;Bth] [B(:,[act_t act_gr]);zeros(nt,2)]];
112 Ct=[C zeros(size(C,1),nt)];
113 Dt=[zeros(size(C,1),size(B,2))];
114
115 figure(1);clf;sgrid([.5 .707]’,[1]);hold on;
116 rlocus(At,-Bt(:,act_e),Ct(sen_t,:),Dt(sen_t,act_e));
117 r_th=rlocus(At,Bt(:,act_e),Ct(sen_t,:),Dt(sen_t,act_e),K_th)’;
118 plot(r_th+eps*j,’bd’,’MarkerFace’,’b’);axis([-1.5,.1,-.1,1.2]);hold off
119 title(’with q and theta FB to dele’)
120 if prt
121 print -depsc land2_1.eps
122 jpdf(’land2_1’)
123 end
124 end
125 Acl=At-Bt(:,act_e)*K_th*Ct(sen_t,:);
126 Bcl=[Bt(:,act_e) Bt(:,[act_t act_gr])]; % act_e now a ref in for theta
127 Ccl=Ct;
128 Dcl=[Dt(:,act_e) Dt(:,[act_t act_gr])];
129
130 if 0
13 figure(3);clf
13 K_th=-6;K_q=-1.5;
13 Ktn=[1 1.4 1];Ktd=conv([1/5 1],[1 0]);
13 rlocus([K_q K_th*ss(tf(Ktn,Ktd))]/norm(K_th)*syslong2([sen_q sen_t],act_e));axis([-2 1 -1 1]) ;
13 rr_qq=rlocus([K_q K_th*ss(tf(Ktn,Ktd))]*syslong2([sen_q sen_t],act_e),1);
13 hold on;
13 plot(rr_qq+eps*sqrt(-1),’gd’)
13 plot(eig(Acl)+eps*sqrt(-1),’bs’)
13 hold off
Fall 2004 16.333 11–29
140
141
142 figure(4);clf
143 Ktn=[1];Ktd=[1];
144 Ktn=[1 2*0.7*2 4];Ktd=conv([1/5 1],[1 0]);
145 K_th=-6;K_q=-3;
146 rlocus([K_q K_th*ss(tf(Ktn,Ktd))]/norm(K_th)*syslong2([sen_q sen_t],act_e));axis([-2 1 -1 1]*2) ;
147 rr_qq=rlocus([K_q K_th*ss(tf(Ktn,Ktd))]*syslong2([sen_q sen_t],act_e),1);
148 hold on;
149 plot(rr_qq+eps*sqrt(-1),’gd’)
150 plot(eig(Acl)+eps*sqrt(-1),’bs’)
151 hold off
152
153 end
154
155
156 %
157 %
158 % engine loops
159 % mostly the phugoid ==> design on top of the short period
160 %
161 syst=tf(JNt,JDt);
162 Cp=[1 0]; % pulls u out of phugiod model
163 syslongp=ss(Ap,Bp(:,act_t),Cp,0);
164 syslongp2=series(syst,syslongp);
165 [Apt,Bpt,Cpt,Dpt]=ssdata(syslongp2);
166 %
167 % model will have a zero at the origin
168 % so cancel that with a compensator pole
169 % cancel pole in eng dynamics
170 % put a zero at 0.1 to pull in the phugoid
171 %
172 K_u=.005;
173 if 1
174 Kun=conv([1/.2857 1],[1/.1 1]);Kud=conv([1 0],[1/1 1]);
175 % Kun=1;Kud=1;
176 [Au,Bu,Cu,Du]=tf2ss(Kun,Kud);
177 na=size(Apt,1);nu=size(Au,1);
178 Aclt=[Apt Bpt*Cu;zeros(nu,na) Au];
179 Bclt=[[Bpt*Du;Bu]];
180 Cclt=[Cpt zeros(size(Cpt,1),nu)];
181 Dclt=[zeros(size(Cclt,1),size(Bclt,2))];
182 figure(3);clf;axis([-.5 .05,-.05,.6]);sgrid([.5 .707]’,[.05]);hold on;
183 rlocus(Aclt,Bclt,Cclt,Dclt);
184 axis([-.5 .05,-.05,.6])
185 r_u=rlocus(Aclt,Bclt,Cclt,Dclt,K_u)’
186 hold on;plot(r_u+eps*j,’rd’),hold off
187 title(’with u FB to delt’)
188 end
189
190 % standard so far, except revised to account for changes in the dynamics
191 % Now close the loop on d
192
193 %close loop on q
194 syscl=feedback(syslong2,diag([K_q]),[act_e],[sen_q]);
195 %close loop on theta and engine
196 syst=tf(K_th*Ktn,Ktd);
197 sysu=tf(K_u*Kun,Kud);
198 sysc=append(syst,sysu,1);
199 syslong3=series(sysc,syscl);
200 syscl2=feedback(syslong3,diag([1 1]),[act_e act_t],[sen_t sen_u]);
201 [Acl2,Bcl2,Ccl2,Dcl2]=ssdata(syscl2);
202 %
203 % pole at origin for tracking perf
204 % lead at 1 and 2
205 % zero at 0.05 to catch low pole and pull in the ones at DC
206 %
207 if 1
208 %Kdn=conv([1/.11 1],[1/.05 1]);Kdd=conv([1/.1 1],[1 0]); K_d=.0001;
209 Kdn=conv([1/1 1],[1/.05 1]);Kdd=conv([1/2 1],[1 0]); K_d=-.0001;
210 [Ad,Bd,Cd,Dd]=tf2ss(Kdn,Kdd);
211 na=size(Acl2,1);nd=size(Ad,1);
Fall 2004 16.333 11–30
212 Acl2t=[Acl2 Bcl2(:,act_e)*Cd;zeros(nd,na) Ad];
213 Bcl2t=[[Bcl2(:,act_e)*Dd;Bd] [Bcl2(:,[act_t act_gr]);zeros(nd,2)]];
214 Ccl2t=[Ccl2 zeros(size(Ccl2,1),nd)];
215 Dcl2t=[zeros(size(Ccl2t,1),size(Bcl2t,2))];
216 figure(4);clf;axis([-1 .05,-.05,1]);sgrid([.5 .707]’,[.05]);hold on;
217 rlocus(Acl2t,sign(K_d)*Bcl2t(:,act_e),Ccl2t(sen_d,:),sign(K_d)*Dcl2t(sen_d,act_e),[0:.000005:.0005])
218 %,[0:.0000005:.0001]);
219 axis([-4 .05,-.05,2])
220 r_d=rlocus(Acl2t,Bcl2t(:,act_e),Ccl2t(sen_d,:),Dcl2t(sen_d,act_e),K_d)’
221 hold on;plot(r_d+eps*j,’rd’),hold off
222 title(’with d FB to dele’)
223 end
224 Acl3=Acl2t-Bcl2t(:,act_e)*Ccl2t(sen_d,:)*K_d;
225 Bcl3=[Bcl2t(:,act_e)*K_d Bcl2t(:,[act_t act_gr])];
226 Ccl3=Ccl2t;
227 Dcl3=Dcl2t;
228 %
229 % add h for simulations
230 % dot h = U0 (theta - alpha) = U0 theta - w
231 na=size(Acl3,1);
232 Acl3t=[Acl3 zeros(na,1);[0 -1 0 U0 zeros(1,na-4+1)]];
233 syscl3=ss(Acl3t,[Bcl3;[0 0 0]],[[Ccl3 zeros(size(Ccl3,1),1)];...
234 [zeros(1,na) 1]],[Dcl3;[0 0 0]]);
235
236 t=[0:1:200]’;
237 D0=0;%just above the glide slope;
238 H0=600;
239 % U=[d_c U_c Gam_r]
240 Gam_r=-2.5*pi/180;
241 INP=[zeros(size(t,1),2) Gam_r*[zeros(10,1);ones(size(t,1)-10,1)]];
242 % x=[u w q theta d f1 f2 f3 f4 h];
243 X0=[ zeros(1,na) H0];
244 [ystep,t]=lsim(syscl3,INP,t,X0);
245 figure(5)
246 % y=[u w q th d gamma h]
247 U=ystep(:,sen_u);W=ystep(:,sen_w);
248 q=ystep(:,sen_q);TH=ystep(:,sen_t);d=ystep(:,sen_d);
249 Gam=ystep(:,sen_g);H=ystep(:,sen_g+1);
250 plot(t,[H0+d H],[t(11);max(t)],[H0;H0+U0*(max(t)-t(11))*tan(Gam_r)]);
251
252 figure(6)
253 plot(t,[Gam]*180/pi)
254
255
256 return
257 f=logspace(-3,2,400);
258 g=freqresp(Acl3,Bcl3(:,3),Ccl3,Dcl3,1,2*pi*f*j);
259 loglog(f,abs(g(:,sen_d)))
260
261 % phugoid approx from reid
262 a1=U0*(-Mw*Xdp/m);
263 a0=0;
264 AA=-U0*Mw;
265 BB=g*Mu+U0/m*(Xu*Mw-Mu*Xw);
266 CC=g/m*(Zu*Mw-Mu*Zw);
267 rlocus(conv([a1 a0],JNt),conv([AA
268
269
270 return
271 t=Lan(:,1);u=Lan(:,2);
272 w=Lan(:,3);q=Lan(:,4);
273 th=Lan(:,5);d=Lan(:,6);
274 gam=Lan(:,7);ele=Lan(:,8);
275 thr=Lan(:,9);H=Lan(:,10);
276 Hr=Lan(:,11);
277
278 figure(6)
279 plot(t,[Hr H]);setlines(2)
280 xlabel(’time’);
281 legend(’H_{ref}’,’H’)
282 print -depsc land3_1.eps
283 jpdf(’land3_1’)
BB CC],JDt))
Fall 2004 16.333 11–31
284
285 figure(1);plot(t,ele*180/pi,t,10*thr);setlines;xlabel(’time’);
286 ylabel(’Control Signals’)
287 title([’tau_f= ’,num2str(tau_f)]);legend(’\delta_e degs’,’10*\delta_t’)
288 setlines(2)
289 print -depsc land3_2.eps
290 jpdf(’land3_2’)
291
292 return
293 if 1
294
Kdn=conv([1/.11 1],[1/.05 1]);Kdd=conv([1/.1 1],[1 0]);[Ad,Bd,Cd,Dd]=tf2ss(Kdn,Kdd);
295 K_d=.00005;
296 na=size(Acl2,1);nd=size(Ad,1);
297 Acl2t=[Acl2 Bcl2(:,act_e)*Cd;zeros(nd,na) Ad];
298 Bcl2t=[[Bcl2(:,act_e)*Dd;Bd] [Bcl2(:,[act_t act_gr]);zeros(nd,2)]];
299 Ccl2t=[Ccl2 zeros(size(Ccl2,1),nd)];
300 Dcl2t=[zeros(size(Ccl2t,1),size(Bcl2t,2))];
301
302 figure(4);clf;axis([-1 .05,-.05,1]);sgrid([.5 .707]’,[.05]);hold on;
303 rlocus(Acl2t,Bcl2t(:,act_e),Ccl2t(sen_d,:),Dcl2t(sen_d,act_e),[0:.000005:.0005])
304 %,[0:.0000005:.0001]);
305 axis([-1 .05,-.05,1])
306 r_d=rlocus(Acl2t,Bcl2t(:,act_e),Ccl2t(sen_d,:),Dcl2t(sen_d,act_e),K_d)’
307 hold on;plot(r_d+eps*j,’rd’),hold off
308 title(’with d FB to dele’)
309 end