% Data for Symmetric Aircraft
close all; 
clear all
clc
% Mass and Dimensions
m = 10000; 
W = m * 9.81; 
m_F = 0.15 * m; 
m_W = 0.3 * m; 
m_C = 0.4 *m;
m_T = 0.15 * m; 
S_W = 30.0; 
S_T = 7.5; 
s = 7.5; 
c = 2.0; 
a_W = S_W;
a_T=S_T;
s_tp =3.0; 
c_tp = 1.25; 
l_W = 0.3*c;
l_T = 3.5 * c; 
l_A = 0.125 * c; 
l_E =0.125 * c; 
l_WM = l_W - l_A - l_E; 
l_M = 0.375 * c; 
l_F = (m_T *l_T- m_W * l_WM) / m_F; 
l_WT = l_W + l_T; 
l_N = l_F; 
l_M = 0.375 * c;
mu = m_W / 2 / s;
% Moments of Inertia
I_y_fuse = m_F * l_F^2 + m_T * l_T^2; 
I_y_W = m_W * (c / 3)^2; 
I_y =I_y_fuse + I_y_W + m_W * l_WM^2; 
l_y_W = sqrt(I_y_W / m_W); 
l_y =sqrt(I_y / m);

% Additional Data for Flexible Aircraft (if required) - Example of Fuselage Bending dominant
kappa_e0 = 1; 
gamma_e0 = 0;
A = 0; 
B = 0;
m=10000;
m_F=0.15*m;
m_W=0.3*m;
m_C=0.4*m;
m_T=0.15*m;

% Normalisation to 1 at wing tip trailing edge
kappa_tip = kappa_e0*(1 + A) + gamma_e0*(1 + B)*(c - c/4 - l_A);
kappa_e0 = kappa_e0/kappa_tip;

% Solution of equations to yield dominant Fuselage Bending mode shape
X = [m_F m_T; -m_F*l_F m_T*l_T]; 
Y = [-(m_W + m_C)*kappa_e0; m_W*l_WM*kappa_e0];
Z = X\Y
kappa_eC = 1;
kappa_eF = Z(1);
kappa_eT = Z(2); 
gamma_eT = 2*(kappa_eT - kappa_eC) / l_T - gamma_e0;

% Modal Mass, Natural Frequency and Modal Stiffness for Fuselage Bending dominant
m_e = m_F*kappa_eF^2 + m_W*kappa_e0^2 + m_C*kappa_eC^2 + m_T*kappa_eT^2;
f_e = 4.0; omega_e = 2*pi*f_e; k_e = omega_e^2*m_e;
% `J' Integrals for aerodynamic derivatives
J1 = gamma_e0*(1 + B / 2);
J2 = kappa_e0*(1 + A / 3) - l_A*gamma_e0*(1 + B / 2);
J3 = gamma_e0*kappa_e0*(1 + A / 3 + B / 2 + A*B / 4) - l_A*gamma_e0^2*(1 + B + B^2 / 3);

% Data for Flight Case-specify EAS (and relative density if not at sea level)
V0 = 150; % EAS
rho0 = 1.225;% Sea level density
root_sigma = 0.8;% Relative density at altitude
V = V0 / root_sigma;% Convert to TAS for gusts / ground manoeuvres
k_epsilon = 0.35;
alpha_0=-0.03;
a_E=1.5;
C_M0=-0.03;
C_D=0.0233;
% Derivatives evaluated using Equivalent Air Speed and sea level air density
% Aerodynamic Derivatives (inertial axes)- Heave DoF
Z_0 = -0.5*rho0*V0^2*[- S_W*a_W + S_T*a_T*k_epsilon]*alpha_0;
Z_alpha = -0.5*rho0*V0^2*[S_W*a_W + S_T*a_T*(1 - k_epsilon)];
Z_q = -0.5*rho0*V0*S_T*a_T*l_T;
Z_eta = -0.5*rho0*V0^2*S_T*a_E;
Z_zdot = -0.5*rho0*V0*(S_W*a_W + S_T*a_T*(1 - k_epsilon));
Z_gW = -0.5*rho0*V0*S_W*a_W;
Z_gT = -0.5*rho0*V0*S_T*a_T*(1 - k_epsilon);
% Aerodynamic Derivatives (inertial axes)- Pitch DoF
M_0W = 0.5*rho0*V0^2*S_W*c*C_M0 -0.5*rho0*V0^2*S_W*a_W*l_W*alpha_0;
M_0T = -0.5*rho0*V0^2*S_T*a_T*k_epsilon*l_T*alpha_0;
M_0 = M_0W + M_0T;
M_alpha = 0.5*rho0*V0^2*[S_W*a_W*l_W - S_T*a_T*(1 - k_epsilon)*l_T];
M_q = -0.5*rho0*V0*S_T*a_T*l_T^2;
M_eta = -0.5*rho0*V0^2*S_T*a_E*l_T;
M_zdot = 0.5*rho0*V0*(S_W*a_W*l_W - S_T*a_T*l_T*(1 - k_epsilon));
M_gW = 0.5*rho0*V0*S_W*a_W*l_W;
M_gT = -0.5*rho0*V0*S_T*a_T*l_T*(1 - k_epsilon);
% Additional aerodynamic derivatives for wind axes- Rigid DoF (if required)
Z_w = -0.5*rho0*V0*(S_W*a_W + S_T*a_T*(1 - k_epsilon)+ S_W*C_D);
M_w = 0.5*rho0*V0*(S_W*a_W*l_W - S_T*a_T*l_T*(1 - k_epsilon));
% Aerodynamic Derivatives- Elastic DoF (if required)
Z_e = 0.5*rho0*V0^2*(-S_W*a_W*J1 - S_T*a_T*gamma_eT);
Z_edot = -0.5*rho0*V0*S_T*a_T*kappa_eT;
M_e = 0.5*rho0*V0^2*(S_W*a_W*l_W*J1 - S_T*a_T*l_T*gamma_eT);
M_edot = -0.5*rho0*V0*S_T*a_T*l_T*kappa_eT;
Q_0 = 0.5*rho0*V0^2*(S_W*a_W*J2 - S_T*a_T*k_epsilon*kappa_eT)*alpha_0;
Q_alpha = 0.5*rho0*V0^2*(-S_W*a_W*J2 - S_T*a_T*(1 - k_epsilon)*kappa_eT);
Q_q = -0.5*rho0*V0*S_T*a_T*l_T*kappa_eT;
Q_eta = -0.5*rho0*V0^2*S_T*a_E*kappa_eT;
Q_e = 0.5*rho0*V0^2*(-S_W*a_W*J3 - S_T*a_T*gamma_eT*kappa_eT);
Q_zdot = 0.5*rho0*V0*(-S_W*a_W*J2 - S_T*a_T*(1 - k_epsilon)*kappa_eT);
Q_edot = -0.5*rho0*V0*S_T*a_T*kappa_eT^2;
Q_gW = -0.5*rho0*V0*S_W*a_W*J2;
Q_gT = -0.5*rho0*V0*S_T*a_T*kappa_eT;
% Additional aerodynamic derivative for wind axes-Elastic DoF
Q_w = 0.5*rho0*V0*(- S_W*a_W*J2 - S_T*a_T*(1 - k_epsilon)*kappa_eT);

% Load factor and steady pitch rate for equilibrium manoeuvre
n = 1.0; q_pr = 0;
% Trim response of a rigid aircraft- requires aircraft data, flight case and derivative codes
% Setting-up and Solving Equations of Motion for the Rigid Aircraft
ARigid = -[Z_eta Z_alpha; M_eta M_alpha];
CRigid = [1 ; 0]; DRigid = [Z_q; M_q]; ERigid = [Z_0; M_0];
BRigid = inv(ARigid)*(CRigid*(n*W) + DRigid*q_pr + ERigid);
Bdeg = BRigid*180 / pi;
Trim_Elevator_Rigid = Bdeg(1)
Trim_Incidence_Rigid = Bdeg(2)
% Trim response of an elastic aircraft-requires aircraft data, flight case, flexible mode and derivative codes
% Setting-up and Solving Equations of Motion for the Elastic Aircraft
AElastic= - [Z_eta Z_alpha Z_e; M_eta M_alpha M_e; Q_eta Q_alpha Q_e - k_e];

CElastic = [1 ; 0 ; 0]; DElastic = [Z_q ; M_q ; Q_q];
EElastic = [Z_0 ; M_0 ; Q_0];
BElastic = inv(AElastic)*(CElastic*(n*W) + DElastic*q_pr +EElastic); 
Bdeg = BElastic*180 / pi;
Trim_Elevator_Elastic = Bdeg(1)
Trim_Incidence_Elastic = Bdeg(2)