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