aclear
close all

% Flight conditions and atmospheric parameters (sea level)
a = 1116.45;        % Speed of sound (ft/s)
rho = 0.0023769;    % Density (slugs/ft^3)
g = 32.2;           % Gravitational acceleration (ft/s^2)

Ma = 0.158;
u0 = Ma*a;
theta0 = 0;

% Moments of inertia
Ix = 1048;
Iy = 3000;
Iz = 3530;
Ixz = 0;

S = 184;    % Wing planform (ft^2)
b = 33.4;   % Wing span (ft)
c = 5.7;    % Wing mean aerodynamic chord (ft)

% Aircraft parameters
W = 2750;   % Weight (lbs)
m = W/g;    % Mass (slugs)
Pdyn = (1/2)*rho*u0^2;
CW = W/(Pdyn*S);

% Longitudinal nondimensional stability derivatives
CLalpha = 4.44;
CDalpha = 0.33;
Cmalpha = -0.683;
CLalphadot = 0.0;
Cmalphadot = -4.36;
CLq = 3.8;
Cmq = -9.96;
CLM = 0;
CDM = 0;
CmM = 0;

CW0 = W/((1/2)*rho*u0^2*S);
CD0 = 0.05;
%CL0 = CW0*cos(theta0);
CL0 = 0.41;
CT0 = CD0 + CW0*sin(theta0);

CTu = -3*CT0;   % Constant power assumption

CDu = CDM*Ma;    % Compressibility only.  Ignore aeroelasticity and other effects.
CLu = CLM*Ma;
Cmu = CmM*Ma;

% Longitudinal dimensional stability derivatives

Xu = (1/2)*rho*u0*S*(2*(-CD0+CT0) + (-CDu + CTu));
Xw = (1/2)*rho*u0*S*(-CDalpha + CL0);
Zu = (1/2)*rho*u0*S*(-2*CL0 - CLu);
Zw = (1/2)*rho*u0*S*(-CD0 - CLalpha);
Zq = (1/4)*rho*u0*c*S*(-CLq);
Zwdot = (1/4)*rho*c*S*(-CLalphadot);
Mu = (1/2)*rho*u0*c*S*Cmu;
Mw = (1/2)*rho*u0*c*S*Cmalpha;
Mq = (1/4)*rho*u0*c^2*S*Cmq;
Mwdot = (1/4)*rho*c^2*S*Cmalphadot;

AL = [Xu/m, Xw/m, 0, -g*cos(theta0);
    Zu/(m-Zwdot), Zw/(m-Zwdot), (Zq + m*u0)/(m-Zwdot), -(m*g*sin(theta0))/(m-Zwdot);
    (Mu + (Mwdot*Zu)/(m-Zwdot))/Iy, (Mw + (Mwdot*Zw)/(m-Zwdot))/Iy, ...
        (Mq + (Mwdot*(Zq+m*u0))/(m-Zwdot))/Iy, ((Mwdot*(-m*g*sin(theta0)))/(m-Zwdot))/Iy;
    0, 0, 1, 0];

[VL,DL] = eig(AL)


% Compare true and approximate modal values
% SHORT PERIOD MODE:
omegan_SP = abs(DL(1,1))
zeta_SP = -real(DL(1,1))/omegan_SP
% t_half_SP = abs(log(0.5)/real(DL(1,1)))
% N_half_SP = abs((log(0.5)/(2*pi))*(imag(DL(1,1))/real(DL(1,1))))

% approximate_omegan_SP = sqrt((1/(m*Iy))*Zw*Mq - (u0/Iy)*Mw)
% approximate_zeta_SP = -(1/(2*approximate_omegan_SP))*((1/m)*Zw+(1/Iy)*(Mq+u0*Mwdot))
% approximate_t_half_SP = abs(log(0.5)/(approximate_zeta_SP*approximate_omegan_SP))
% approximate_N_half_SP = abs(log(0.5)/(2*pi))*...
%     sqrt((1-approximate_zeta_SP^2)/(approximate_zeta_SP^2))

% PHUGOID MODE:
omegan_Phugoid = abs(DL(3,3))
zeta_Phugoid = -real(DL(3,3))/omegan_Phugoid
% t_half_Phugoid = abs(log(0.5)/real(DL(3,3)))
% N_half_Phugoid = abs((log(0.5)/(2*pi))*(imag(DL(3,3))/real(DL(3,3))))

approximate_omegan_Phugoid = sqrt(-(Zu*g)/(m*u0))
approximate_zeta_Phugoid = -Xu/(2*m*approximate_omegan_Phugoid)
% approximate_t_half_Phugoid = ...
%     abs(log(0.5)/(approximate_zeta_Phugoid*approximate_omegan_Phugoid))
% approximate_N_half_Phugoid = abs(log(0.5)/(2*pi))*...
%     sqrt((1-approximate_zeta_Phugoid^2)/(approximate_zeta_Phugoid^2))