clear all;clc
M = 0.84;
g = 32.1737; %ft/s^2
a = 995; %ft/s
u_0 = M*a; %ft/s
Xu = -.0106;
Zu = -.0688;
Mu = 0;
Xw = .0234;
Zw = -.504;
Mw = -.0142;
Mwdot = -.000239;
Mq = -.412;
Malpha = u_0*Mw;
Malphadot = u_0*Mwdot;
Zalpha = u_0*Zw;

A_L = [Xu Xw 0 -g;Zu Zw u_0 0;Mu+Mwdot*Zu Mw+Mwdot*Zw Mq+Mwdot*u_0 0;0 0 1 0]
E_L = eig(A_L)

lambdasp = E_L(1:2)
wnsp = sqrt(Zalpha*Mq/u_0-Malpha);
zetasp = (Mq+Malphadot+Zalpha/u_0)/(2*wnsp);
lambdasp_approx(:,1) = [zetasp*wnsp + sqrt(-1)*wnsp*sqrt(1-zetasp^2);zetasp*wnsp - sqrt(-1)*wnsp*sqrt(1-zetasp^2)];
zeta_sp = sqrt(1./(1+(imag(lambdasp_approx)./real(lambdasp_approx)).^2))
omega_sp = -real(lambdasp_approx)./zeta_sp

lambdaphugoid = E_L(3:4)
wnp = sqrt(-Zu*g/u_0);
zetap = -Xu/(2*wnp);
lambdaphugoid_approx(:,1) = [-zetap*wnp + sqrt(-1)*wnp*sqrt(1-zetap^2);-zetap*wnp - sqrt(-1)*wnp*sqrt(1-zetap^2)]
zeta_ph = sqrt(1./(1+(imag(lambdaphugoid_approx)./real(lambdaphugoid_approx)).^2))
omega_ph = -real(lambdaphugoid_approx)./zeta_ph

t = linspace(0, 1500, 100);
figure
plot(t, 1 - 1./sqrt(1-zeta_ph(1).^2).*exp(-zeta_ph(1).*omega_ph(1).*t).*cos(omega_ph(1).*sqrt(1-zeta_ph(1).^2).*t - atan(zeta_ph(1)./(1-zeta_ph(1).^2))))
title('Phugoid')
grid minor
figure
t = linspace(0, 15, 100);
plot(t, 1 - 1./sqrt(1-zeta_sp(1).^2).*exp(-zeta_sp(1).*omega_sp(1).*t).*cos(omega_sp(1).*sqrt(1-zeta_sp(1).^2).*t - atan(zeta_sp(1)./(1-zeta_sp(1).^2))))
grid on
title('Short Period')

figure
s = tf('s');
G = omega_sp(1)^2/(s*(s^2 + 2*omega_sp(1)*zeta_sp(1)*s + omega_sp(1)));
T = feedback(ss(G),1);
step(T)