clear;clc;
% constants
g = 9.81; % N/s2
Ma = 5805; % kg
% Inertias
Ixx=9638; % kg.m2
Iyy=33240;
Izz=25889;
Ixz=2226;
% 1500 ft/min = 7.62 m/s
% 1 m/s = 196.85039 ft/min
% 1 m/s = 1.94385 knots
% 1 knot = 1.15078 mph
% Trimmed Variables
Ue=52;
Ve=0;
We=0;
Pe=0;
Qe=0;
Re=0;
THe=0;
PHIe=0;
OMEGAa = 27; %rad/s
% Stability Derivatives
Xu=-0.0275;
Xv=0.006;
Xw=0.004;
Xp=0.33;
Xq=1.5;
Xr=0;
Yu=-0.005;
Yv=-0.133;
Yw=-0.0225;
Yp=-1.5;
Yq=0.33;
Yr=-52;
Zu=-0.026;
Zv=0.03;
Zw=0.004;
Zp=0.85;
Zq=1.5;
Zr=0;
Lu=-0.005;
Lv=-0.045;
Lw=-0.055;
Lp=-1.66;
Lq=1;
Lr=0.35;
Mu=0.03;
Mv=-0.004;
Mw=-0.025;
Mp=-0.22;
Mq=-0.75;
Mr=0;
Nu=0.008;
Nv=0.02;
Nw=0.035;
Np=0;
Nq=-0.25;
Nr=-0.625;
% Control Derivatives
Xth0=-2.5;
Xth1s=-9.4;
Xth1c=0.5;
Xth0T=0;
Yth0=-3.2;
Yth1s=-0.7;
Yth1c=10;
Yth0T=5;
Zth0=-115;
Zth1s=-41;
Zth1c=0;
Zth0T=0;
Lth0=-7.5;
Lth1s=-3;
Lth1c=25;
Lth0T=5.3;
Mth0=2.5;
Mth1s=7.5;
Mth1c=0;
Mth0T=0;
Nth0=-5;
Nth1s=1.67;
Nth1c=0;
Nth0T=-9.5;
% semi-normalize forces
Xu=Xu/Ma;Xv=Xv/Ma;Xw=Xw/Ma;Xp=Xp/Ma;Xq=Xq/Ma;Xr=Xr/Ma;
% Dependent Variables
Lu_pri = (Izz/(Ixx*Izz-Ixz^2))*Lu + (Ixz/(Ixx*Izz-Ixz^2))*Nu;
Lv_pri = (Izz/(Ixx*Izz-Ixz^2))*Lv + (Ixz/(Ixx*Izz-Ixz^2))*Nv;
Lw_pri = (Izz/(Ixx*Izz-Ixz^2))*Lw + (Ixz/(Ixx*Izz-Ixz^2))*Nw;
Lp_pri = (Izz/(Ixx*Izz-Ixz^2))*Lp + (Ixz/(Ixx*Izz-Ixz^2))*Np;
Lq_pri = (Izz/(Ixx*Izz-Ixz^2))*Lq + (Ixz/(Ixx*Izz-Ixz^2))*Nq;
Lr_pri = (Izz/(Ixx*Izz-Ixz^2))*Lr + (Ixz/(Ixx*Izz-Ixz^2))*Nr;
Nu_pri = (Ixz/(Ixx*Izz-Ixz^2))*Lu + (Ixx/(Ixx*Izz-Ixz^2))*Nu;
Nv_pri = (Ixz/(Ixx*Izz-Ixz^2))*Lv + (Ixx/(Ixx*Izz-Ixz^2))*Nv;
Nw_pri = (Ixz/(Ixx*Izz-Ixz^2))*Lw + (Ixx/(Ixx*Izz-Ixz^2))*Nw;
Np_pri = (Ixz/(Ixx*Izz-Ixz^2))*Lp + (Ixx/(Ixx*Izz-Ixz^2))*Np;
Nq_pri = (Ixz/(Ixx*Izz-Ixz^2))*Lq + (Ixx/(Ixx*Izz-Ixz^2))*Nq;
Nr_pri = (Ixz/(Ixx*Izz-Ixz^2))*Lr + (Ixx/(Ixx*Izz-Ixz^2))*Nr;
Lth0_pri = (Izz/(Ixx*Izz-Ixz^2))*Lth0 + (Ixz/(Ixx*Izz-Ixz^2))*Nth0;
Lth1s_pri = (Izz/(Ixx*Izz-Ixz^2))*Lth1s + (Ixz/(Ixx*Izz-Ixz^2))*Nth1s;
Lth1c_pri = (Izz/(Ixx*Izz-Ixz^2))*Lth1c + (Ixz/(Ixx*Izz-Ixz^2))*Nth1c;
Lth0T_pri = (Izz/(Ixx*Izz-Ixz^2))*Lth0T + (Ixz/(Ixx*Izz-Ixz^2))*Nth0T;
Nth0_pri = (Ixz/(Ixx*Izz-Ixz^2))*Lth0 + (Ixx/(Ixx*Izz-Ixz^2))*Nth0;
Nth1s_pri = (Ixz/(Ixx*Izz-Ixz^2))*Lth1s + (Ixx/(Ixx*Izz-Ixz^2))*Nth1s;
Nth1c_pri = (Ixz/(Ixx*Izz-Ixz^2))*Lth1c + (Ixx/(Ixx*Izz-Ixz^2))*Nth1c;
Nth0T_pri = (Ixz/(Ixx*Izz-Ixz^2))*Lth0T + (Ixx/(Ixx*Izz-Ixz^2))*Nth0T;
k1 = Ixz*(Izz+Ixx-Iyy)/(Ixx*Izz-Ixz^2);
k2 = (Izz*(Izz-Iyy)+Ixz^2)/(Ixx*Izz-Ixz^2);
k3 = (Ixx*(Iyy-Ixx)+Ixz^2)/(Ixx*Izz-Ixz^2);
% A & B Matrices
A = [Xu Xw-Qe Xq-We -g*cos(THe) Xv+Re Xp 0 Xr-Ve;...
Zu-Qe Zw Zq -g*cos(PHIe)*sin(THe) Zv-Pe Zp-Ve -g*sin(PHIe)*sin(THe) Zr
Mu Mw Mq 0 Mv Mp+2*Pe*Ixz/Iyy-Re*(Ixx-Izz)/Iyy 0 Mr+2*Re*Ixz/Iyy-Pe*(Ixx-Izz)/Iyy
0 0 cos(THe) 0 0 0 -OMEGAa*cos(THe) -sin(THe)
Yu-Re Yw+Pe Yq -g*sin(PHIe)*sin(THe) Yv Yp+We g*cos(PHIe)*cos(THe) Yr-Ue
Lu_pri Lw_pri Lq_pri-k1*Pe-k2*Re 0 Lv_pri Lp_pri-k1*Qe 0 Lr_pri-k2*Qe
0 0 sin(PHIe)*tan(THe) OMEGAa*sec(THe) 0 1 0 cos(PHIe)*tan(THe)
Nu_pri Nw_pri Nq_pri-k1*Re-k3*Pe 0 Nv_pri Np_pri-k3*Qe 0 Nr_pri-k1*Qe];
B = [Xth0 Xth1s Xth1c Xth0T;...
Zth0 Zth1s Zth1c Zth0T
Mth0 Mth1s Mth1c Mth0T
0 0 0 0
Yth0 Yth1s Yth1c Yth0T
Lth0_pri Lth1s_pri Lth1c_pri Lth0T_pri
0 0 0 0
Nth0_pri Nth1s_pri Nth1c_pri Nth0T_pri];
C = diag([ones(1,8)]);
D=zeros(8,4);
[K]= lqr (A,B,C,diag([1 1 1 1]))
[n1,d]=ss2tf(A,B,C,D,1);
[n2,d]=ss2tf(A,B,C,D,2);
[n3,d]=ss2tf(A,B,C,D,3);
[n4,d]=ss2tf(A,B,C,D,4);
p=85;
[n1,d]=ss2tf(A-B*K,B,p*C,D,1);
[n2,d]=ss2tf(A-B*K,B,p*C,D,2);
[n3,d]=ss2tf(A-B*K,B,p*C,D,3);
[n4,d]=ss2tf(A-B*K,B,p*C,D,4);
tf(n1(2,:),d)
step(n1(2,:),d)
