clear all;clc;close all
M = .4;
g = 32.1737; %ft/s^2
a = 1058; %ft/s
u_0 = M*a; %ft/s
Xu = -.000877;%
Xw = .052;%
Zu = -.0704;%
Zw = -.535;%
Mu = 0.00253;%
Mw = -.0131;%
Mwdot = -.000476;%
Mq = -.67;%
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, 16000, 200);
subplot(1, 2, 1)
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
subplot(1, 2, 2)
t = linspace(0, 14, 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')