%

% B747 longitudinal dynamics

% AA271a
% B747 at Mach 0.8, 40,000ft, level-flight
% From Etkin and Reid page 166
% Modified: 15 Nov. 2016, Tri D. Ngo
%
clear;
clc;
if 1
% metric
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);

else
%english
Xu=-1.358e2;Xw=2.758e2;cbar=27.31;
Zu=-1.778e3;Zw=-6.188e3;Zq=-1.017e5;Zwd=1.308e2;
Mu=3.581e3;Mw=-3.515e4;Mq=-1.122e7;Mwd=-3.826e3;

g=32.2;U0=774;Iyy=.331e8;m=636636/g;cbar=27.31;rho=0.0005909
;
theta0=0;S=5500;
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);

end

A=[Xu/m Xw/m 0 -g*cos(theta0);[Zu Zw Zq+m*U0

-m*g*sin(theta0)]/(m-Zwd);
[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 1 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];
C=[eye(3,4)];

wsp=sqrt(-U0*Mw/Iyy)
zetasp=-Mq/2*sqrt(-1/U0/Mw/Iyy)

wsp=sqrt(Zw*Mq/m/Iyy-U0*Mw/Iyy)
zetasp=-(Zw/m+Mq/Iyy+Mwd/Iyy*U0)/2/wsp

[V,ev]=eig(A);ev=diag(ev);D=V;
%rifd(ev)

% nondimensionalization of the eigenvetors

V(1,:)=V(1,:)/U0;
V(2,:)=V(2,:)/U0;
V(3,:)=V(3,:)/(2*U0/cbar);
for i=1:4
V(:,i)=V(:,i)/V(4,i);
end

figure(5)
subplot(221)
polar(angle(V(:,1)),abs(V(:,1)))
axis('square')
xlabel(num2str(abs(ev(1))))
subplot(222)
polar(angle(V(:,3)),abs(V(:,3)))
axis('square')
xlabel(num2str(abs(ev(3))))
%
% initial condition response using the original EVectors
% as the the IC
%
sys=ss(A,zeros(size(A,1),1),eye(4,4),zeros(4,1));
[y1,t1,x1]=initial(sys,real(D(:,1)));
[y3,t3,x3]=initial(sys,real(D(:,3)));

subplot(223)
plot(t1,y1(:,1),'-',t1,y1(:,2),'--',t1,y1(:,3),'-.',t1,y1(:,
4),':')
ylabel('Perturbation States u,w,q,\theta');xlabel('time
(sec)');axis([0 15 -1 1])
legend({'$\hat{u}$','$\hat{w}$','$\hat{q}
$','$\theta$'},'Interpreter','latex'); %'w','q','\theta',
subplot(224)
plot(t3,y3(:,1),'-',t3,y3(:,2),'--',t3,y3(:,3),'-.',t3,y3(:,
4),':')
ylabel('Perturbation States u,w,q,\theta');xlabel('time
(sec)');axis([0 600 -1 1])
%%
%return
% real imaginary frequency damping
%
% -3.2889e-003 -6.7202e-002 6.7282e-002 4.8882e-002
% -3.7168e-001 8.8692e-001 9.6166e-001 3.8650e-001
%V
% 0.0156 + 0.0244i -0.0254 - 0.6165i
% 1.0202 + 0.3553i 0.0045 - 0.0356i
% -0.0066 + 0.0156i -0.0001 - 0.0012i
% 1.0000 1.0000

%
% step response to elevator - scaled to del_e=1 deg input
%
C=[eye(4,4);0 -1/U0 0 1];
sys_sc=ss(A,[B(:,1)/180*pi],C,zeros(5,1));
[Y,T]=step(sys_sc,[0:1:600]);
[Y2,T2]=step(sys_sc,[0:.1:40]);
%
% step response to thrust - scaled to del_p=1/6
%
sys_sc=ss(A,[B(:,2)/6],C,zeros(5,1));
[Yt,Tt]=step(sys_sc,[0:1:600]);
[Yt2,Tt2]=step(sys_sc,[0:.1:40]);
%
%final value theorem:
Yfinal=-C*inv(A)*[B(:,1)/180*pi B(:,2)/6];
Yfinal(2,:)=Yfinal(2,:)/U0 % switch the w to w/U0=alpha
%
% freq response
%
freq=logspace(-2,1,400);
sys=ss(A,B,C,zeros(5,2));
G=freqresp(sys,freq);

clear Gude Gwde Gtde Gudp Gwdp Gtdp Ggde Ggdp

Gude(:,1)=G(1,1,:);
Gwde(:,1)=G(2,1,:);
Gtde(:,1)=G(4,1,:);
Ggde(:,1)=G(5,1,:);
Gudp(:,1)=G(1,2,:);
Gwdp(:,1)=G(2,2,:);
Gtdp(:,1)=G(4,2,:);
Ggdp(:,1)=G(5,2,:);

figure(3);clf
orient('landscape')
subplot(231)
loglog(freq,abs(Gude),min(abs(ev))*[1 1],[.1
1e4],'--',max(abs(ev))*[1 1],[.1 1e4],'--')
axis([.01 5 1 1e4])
ylabel('|G_u_d_e|')
xlabel('Freq (rad/sec)')
subplot(232)
loglog(freq,abs(Gwde)/U0,min(abs(ev))*[1 1],[.1
1e4],'--',max(abs(ev))*[1 1],[.1 1e4],'--')
axis([.01 5 .1 1e4])
ylabel('|G_{\alpha_d_e}|')
xlabel('Freq (rad/sec)')
title('Transfer function from elevator to flight variables')
subplot(233)
loglog(freq,abs(Ggde),min(abs(ev))*[1 1],[.1
1e4],'--',max(abs(ev))*[1 1],[.1 1e4],'--')
ylabel('|G_{\gamma_d_e}|')
xlabel('Freq (rad/sec)')
axis([.01 5 .1 1e4])
subplot(234)
semilogx(freq,180/pi*unwrap(angle(Gude.')),min(abs(ev))*[1
1],[-360 0],'--',max(abs(ev))*[1 1],[-360 0],'--')
ylabel('arg G_u_d_e')
xlabel('Freq (rad/sec)')
axis([.01 5 -360 0])
%
subplot(235)
semilogx(freq,180/pi*unwrap(angle(Gwde.')),min(abs(ev))*[1
1],[-360 0],'--',max(abs(ev))*[1 1],[-360 0],'--')
ylabel('arg G_{\alpha_d_e}')
xlabel('Freq (rad/sec)')
% axis([.01 5 -360 0])
%
subplot(236)
semilogx(freq,180/pi*unwrap(angle(Ggde.')),min(abs(ev))*[1
1],[-360 0],'--',max(abs(ev))*[1 1],[-360 0],'--')
ylabel('arg G_{\gamma_d_e}')
xlabel('Freq (rad/sec)')
axis([.01 5 -360 0])
print -depsc ac_fig1
figure(4);clf
orient('landscape')
subplot(231)
loglog(freq,abs(Gudp),min(abs(ev))*[1 1],[.01
1e4],'--',max(abs(ev))*[1 1],[.01 1e4],'--')
% axis([.01 5 .01 1e3])
ylabel('|G_u_d_p|')
xlabel('Freq (rad/sec)')
subplot(232)
loglog(freq,abs(Gwdp)/U0,min(abs(ev))*[1 1],[.01
1e4],'--',max(abs(ev))*[1 1],[.01 1e4],'--')
% axis([.01 5 .01 1e3])
ylabel('|G_{\alpha_d_p}|')
xlabel('Freq (rad/sec)')
title('Transfer function from thrust to flight variables')
subplot(233)
loglog(freq,abs(Ggdp),min(abs(ev))*[1 1],[.01
1e4],'--',max(abs(ev))*[1 1],[.01 1e4],'--')
ylabel('|G_{\gamma_d_p}|')
xlabel('Freq (rad/sec)')
% axis([.01 5 .01 1e3])
subplot(234)
semilogx(freq,180/pi*unwrap(angle(Gudp.')),min(abs(ev))*[1
1],[-360 360],'--',max(abs(ev))*[1 1],[-360 360],'--')
ylabel('arg G_u_d_p')
xlabel('Freq (rad/sec)')
axis([.01 5 -180 180])
subplot(235)
semilogx(freq,180/pi*unwrap(angle(Gwdp.')),min(abs(ev))*[1
1],[-360 360],'--',max(abs(ev))*[1 1],[-360 360],'--')
ylabel('arg G_{\alpha_d_p}')
xlabel('Freq (rad/sec)')
% axis([.01 5 -180 180])
subplot(236)
semilogx(freq,180/pi*unwrap(angle(Ggdp.')),min(abs(ev))*[1
1],[-360 360],'--',max(abs(ev))*[1 1],[-360 360],'--')
ylabel('arg G_{\gamma_d_p}')
xlabel('Freq (rad/sec)')
axis([.01 5 -360 0])
print -depsc ac_fig2
%
% plot time stuff
%
figure(1);clf;orient('landscape')
LL1=1:400;LL2=1:600;
subplot(231)
plot(T(LL2),Y(LL2,1,1),'LineWidth',2)
ylabel('u');xlabel('time')
hold on;plot([400 600],[1 1]*Yfinal(1,1),'--');hold off
axis([0 600 0 30])
subplot(232)
plot(T(LL2),Y(LL2,2,1)/U0,'LineWidth',2)
ylabel('\alpha (rad)');xlabel('time')
hold on;plot([400 600],[1 1]*Yfinal(2,1),'--');hold off
axis([0 600 -.03 0])
title('Step response to 1 deg elevator perturbation')
subplot(233)
plot(T(LL2),Y(LL2,5,1),'LineWidth',2)
ylabel('\gamma');xlabel('time')
hold on;plot([400 600],[1 1]*Yfinal(5,1),'--');hold off
axis([0 600 -.1 .1])
subplot(234)
plot(T2(LL1),Y2(LL1,1,1),'LineWidth',2)
ylabel('u');xlabel('time')
subplot(235)
plot(T2(LL1),Y2(LL1,2,1)/U0,'LineWidth',2)
ylabel('\alpha (rad)');xlabel('time')
subplot(236)
plot(T2(LL1),Y2(LL1,5,1),'LineWidth',2)
ylabel('\gamma');xlabel('time')
print -depsc ac_fig3
%
%
%
figure(2);clf;orient('landscape')
LL1=1:400;LL2=1:600;
subplot(231)
plot(Tt(LL2),Yt(LL2,1,1),'LineWidth',2)
ylabel('u');xlabel('time')
hold on;plot([400 600],[1 1]*Yfinal(1,2),'--');hold off
axis([0 600 -15 15])
%
subplot(232)
plot(Tt(LL2),Yt(LL2,2,1)/U0,'LineWidth',2)
ylabel('\alpha (rad)');xlabel('time')
hold on;plot([400 600],[1 1]*Yfinal(2,2),'--');hold off
axis([0 600 -.02 .02])
title('Step response to 1/6 thrust perturbation')
%
subplot(233)
plot(Tt(LL2),Yt(LL2,5,1),'LineWidth',2)
ylabel('\gamma');xlabel('time')
hold on;plot([400 600],[1 1]*Yfinal(5,2),'--');hold off
axis([0 600 0 .1])
%
subplot(234)
plot(Tt2(LL1),Yt2(LL1,1,1),'LineWidth',2)
ylabel('u');xlabel('time')
%
subplot(235)
plot(Tt2(LL1),Yt2(LL1,2,1)/U0,'LineWidth',2)
ylabel('\alpha (rad)');xlabel('time')
%
subplot(236)
plot(Tt2(LL1),Yt2(LL1,5,1),'LineWidth',2)
ylabel('\gamma');xlabel('time')
print -depsc ac_fig3a

!ps2pdf ac_fig1.eps
!ps2pdf ac_fig2.eps
!ps2pdf ac_fig3.eps
!ps2pdf ac_fig3a.eps

!cp ac_fig1.eps.pdf ac_fig1.pdf

!cp ac_fig2.eps.pdf ac_fig2.pdf
!cp ac_fig3.eps.pdf ac_fig3.pdf
!cp ac_fig3a.eps.pdf ac_fig3a.pdf