%DIAMHKEIS E3ISWSEIS KINHSHS STH MORFH STATE-SPACE
clc
clear all

%Aircraft Data (American Imperial Units) 
%Dimensional Stability Derivatives 
%Boeing 747-100 
%TL sxhmatizei gwnia 2,5deg me thn FRL sthn opoia keitai o x a3onas
%Den dinontai dedomena gia tis paragwgous ths wshs, opote 8ewroume sta8erh
%wsh kai epomenws oi paragwgoi ths = 0
%Oi ropes adraneias dinontai ws pros tous BODY AXIS(Omws Iy,body = Iy,wind)
%Flight Condition: Steady,Level flight in all 3 cases (We=0 -> Ue=V)
%Case I: Approach - Flaps 30deg , Landing system enabled , V=1,2*Vstall
%Case II: Cruise - Landing system disabled , flaps 0deg
%Case III: Cruise - Higher altitude , greater speed

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Flight Condition Data (ex. CASE III)
V = 871;   Ue = V;    We = 0;    Alt = 40000;   
thita_e = 0*pi/180;   ae = 0*pi/180;    gamma_e = thita_e - ae;

%Mass Data - BODY AXIS
g = 32.17;  Weight = 6.366*10^5;  m = Weight/g;   Iy = 3.31*10^7;
%Longitudinal aerodynamic stability derivatives -DIMENSIONAL - BODY AXIS
Xu = -3.954*10^2;  Xw = -3.144*10^2;  Xq = 0;  Xwdot = 0;

Zu = -8.383*10^2;  Zw = -7.928*10^3;  Zq = -1.327*10^5;  Zwdot = 1.214*10^2;

Mu = -2.062*10^3;  Mw = -6.829*10^4;  Mq = -1.327*10^7;  Mwdot = -5.269*10^3;

%Longitudinal control stability derivatives -DIMENSIONAL - BODY AXIS
Xde = 1.544*10^4;   Xt = 0; 

Zde = -3.667*10^5;  Zt = 0;

Mde = -4.038*10^7;  Mt = 0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%METASXHMATISMOS APO BODY AXIS SE WIND AXES (Ean xreiazetai)
%Longitudinal aerodynamic stability derivatives -DIMENSIONAL - WIND AXES
Xu = Xu*cos(ae)^2+Zw*sin(ae)^2+(Xw+Zu)*sin(ae)*cos(ae);
Xw = Xw*cos(ae)^2-Zu*sin(ae)^2-(Xu-Zw)*sin(ae)*cos(ae);
Xq = Xq*cos(ae)+Zq*sin(ae);
Xwdot = Xwdot*cos(ae)^2+Zwdot*sin(ae)*cos(ae);

Zu = Zu*cos(ae)^2-Xw*sin(ae)^2-(Xu-Zw)*sin(ae)*cos(ae);
Zw = Zw*cos(ae)^2+Xu*sin(ae)^2-(Xw+Zu)*sin(ae)*cos(ae);
Zq = Zq*cos(ae)-Xq*sin(ae);
Zwdot = Zwdot*cos(ae)^2-Xwdot*sin(ae)*cos(ae);

Mu = Mu*cos(ae)+Mw*sin(ae);
Mw = Mw*cos(ae)-Mu*sin(ae);
Mq = Mq;
Mwdot = Mwdot*cos(ae);

%Longitudinal control stability derivatives -DIMENSIONAL - WIND AXES
Xde = Xde*cos(ae)+Zde*sin(ae);
Xt = Xt*cos(ae)+Zt*sin(ae);

Zde = Zde*cos(ae)-Xde*sin(ae);
Zt = Zt*cos(ae)-Xt*sin(ae);

Mde = Mde;
Mt = Mt;

%Suntetmhmenes diamhkeis aerodynamikes paragwgoi eusta8eias
%sunarthsei twn DIASTATWN paragwgwn eusta8eias
xu = Xu/m + Xwdot*Zu/(m*(m-Zwdot));
xw = Xw/m + Xwdot*Zw/(m*(m-Zwdot));
xq = (Xq-m*We)/m + (Zq+m*Ue)*Xwdot/(m*(m-Zwdot));
x8 = -g*cos(thita_e) - Xwdot*g*sin(thita_e)/(m-Zwdot);

zu = Zu/(m-Zwdot);
zw = Zw/(m-Zwdot);
zq = (Zq+m*Ue)/(m-Zwdot);
z8 = -m*g*sin(thita_e)/(m-Zwdot);

mu = Mu/Iy + Zu*Mwdot/(Iy*(m-Zwdot));
mw = Mw/Iy + Zw*Mwdot/(Iy*(m-Zwdot));
mq = Mq/Iy + (Zq+m*Ue)*Mwdot/(Iy*(m-Zwdot));
m8 = -Mwdot*m*g*sin(thita_e)/(Iy*(m-Zwdot));

%Suntetmhmenes diamhkeis paragwgoi elegxou
%sunarthsei twn DIASTATWN paragwgwn elegxou kai aerodunamikwn
xde = Xde/m + Xwdot*Zde/(m*(m-Zwdot));
xt = Xt/m + Xwdot*Zt/(m*(m-Zwdot));

zde = Zde/(m-Zwdot);
zt = Zt/(m-Zwdot);

mde = Mde/Iy + Mwdot*Zde/(Iy*(m-Zwdot));
mt = Mt/Iy + Mwdot*Zt/(Iy*(m-Zwdot));

%Pinakes GXAS
A = [xu xw xq x8;
     zu zw zq z8;
     mu mw mq m8;
     0 0 1 0];

B = [xde xt;
     zde zt;
     mde mt;
     0 0];

C = [1 0 0 0;
     0 1 0 0;
     0 0 1 0;
     0 0 0 1;
     0 1/Ue 0 0;
     0 -1/Ue 0 1];
     

D = [0 0;
     0 0;
     0 0;
     0 0;
     0 0;
     0 0];
    

 %Dhmiourgia Xwrou Katastashs
 system = ss(A,B,C,D);
 
 %Sunarthseis metaforas
 [num1,den1]=ss2tf(A,B,C,D,1)

 TF_u_de = tf(num1(1,:),den1)
 TF_w_de = tf(num1(2,:),den1)
 TF_q_de = tf(num1(3,:),den1)
 TF_8_de = tf(num1(4,:),den1)
 TF_a_de = tf(num1(5,:),den1)
 TF_g_de = tf(num1(6,:),den1)
 
 %paragontopoihsh ariumhtvn kai xarakthristikhs
rizes = roots(den1)
rizes_Nu = roots(num1(1,:))
rizes_Nw = roots(num1(2,:))
rizes_Nq = roots(num1(3,:))
rizes_N8 = roots(num1(4,:))
rizes_Na = roots(num1(5,:))
rizes_Ng = roots(num1(6,:))

 %Pinakes idiodianusmatwn-idiotimwn
 [V,Lamda]=eig(A);
 for i=1:4
     for j=1:4
         V_magn(i,j)=abs(V(i,j));
     end
 end
 
%Apokriseis se Vhmatikh eisodo      
    
figure(1)
subplot(6,1,1)
[u_de,t_de]=step(TF_u_de*pi/180,400);
plot(t_de,u_de*0.3048,'LineWidth',1.5)
title('Áðïêñßóåéò óå åßóïäï âáèìßäáò ôïõ ðçäáëßïõ áíüäïõ-êáèüäïõ')
axonasx=xlabel('t');
set(axonasx,'FontSize',12)
axonasy=ylabel('u [m/sec]');
set(axonasy,'FontSize',12)
grid minor

subplot(6,1,2)
[w_de,t_de]=step(TF_w_de*pi()/180,20);
plot(t_de,w_de*0.3048,'LineWidth',1.5)
title('')
axonasy=ylabel('w [m/sec]');
set(axonasy,'FontSize',12)
grid minor

subplot(6,1,3)
[q_de,t_de]=step(TF_q_de*pi()/180,20);
plot(t_de,q_de,'LineWidth',1.5)
title('')
axonasy=ylabel('q [rad/sec]');
set(axonasy,'FontSize',12)
grid minor

subplot(6,1,4)
[theta_de,t_de]=step(TF_8_de*pi()/180,20);
plot(t_de,theta_de*180/pi,'LineWidth',1.5)
title('')
axonasy=ylabel('è [deg]');
set(axonasy,'FontSize',12)
grid minor

subplot(6,1,5)
[a_de,t_de]=step(TF_a_de*pi()/180,20);
plot(t_de,a_de*180/pi,'LineWidth',1.5)
title('')
axonasy=ylabel('á [deg]');
set(axonasy,'FontSize',12)
grid minor

subplot(6,1,6)
[g_de,t_de]=step(TF_g_de*pi()/180,20);
plot(t_de,g_de*180/pi,'LineWidth',1.5)
title('')
axonasx=xlabel('t');
set(axonasx,'FontSize',12)
axonasy=ylabel('ã [deg]');
set(axonasy,'FontSize',12)
grid minor

%Proseggish mikrhs periodou
Asp = [zw zq;
     mw mq];

Bsp = [zde;
     mde;];

Csp = [1 0;
     0 1];
     

Dsp = [0;
       0];
 
%Dhmiourgia Xwrou Katastashs
 sp_mode = ss(Asp,Bsp,Csp,Dsp);
 
 %Sunarthseis metaforas
 [num_sp,den_sp]=ss2tf(Asp,Bsp,Csp,Dsp,1)

 TF_w_de_sp = tf(num_sp(1,:),den_sp)
 TF_q_de_sp = tf(num_sp(2,:),den_sp)
 TF_a_de_sp = tf(num_sp(1,:),den_sp)/Ue
 
hold on
figure(3)
subplot(3,1,1)
step(TF_w_de*pi()/180*0.3048,'b-',TF_w_de_sp*pi()/180*0.3048,'r--',50)
title('')
axonasx=xlabel('t');
set(axonasx,'FontSize',12)
axonasy=ylabel('w [m/sec]');
set(axonasy,'FontSize',12)
legend('Full Order','Reduced Order')
grid on
hold on
subplot(3,1,2)
step(TF_q_de*pi()/180,'b-',TF_q_de_sp*pi()/180,'r--',50)
title('')
axonasx=xlabel('t');
set(axonasx,'FontSize',12)
axonasy=ylabel('q [rad/sec]');
set(axonasy,'FontSize',12)
grid on
hold on
subplot(3,1,3)
step(TF_a_de,'b-',TF_a_de_sp,'r--',50)
title('')
axonasx=xlabel('t');
set(axonasx,'FontSize',12)
axonasy=ylabel('alpha [deg]');
set(axonasy,'FontSize',12)
grid on

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%ÄéáãñÜììáôá Bode
%Óõ÷íüôçôá ÌéêñÞò Ðåñéüäïõ
rizes_sp = [rizes(1,1) rizes(2,1)];
shortp = poly(rizes_sp);
wsp = sqrt(shortp(1,3));

%Óõ÷íüôçôá Öõãïåéäïýò
rizes_ph = [rizes(3,1) rizes(4,1)];
phugoid = poly(rizes_ph);
wph = sqrt(phugoid(1,3));

band = logspace(-2,1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Ùò ðñïò ôï ðçäÜëéï áíüäïõ-êáèüäïõ %%%%

[magn,phase] = bode(TF_u_de,band);
magn = 20*log10(magn);
size_m = size(magn);
size_m = size_m(1,3);
size_p = size(phase);
size_p = size_p(1,3);
for i=1:size_m
    magn_y_vec(1,i) = magn(:,:,i);
    wsp_m_vec(1,i) = wsp;
    wph_m_vec(1,i) = wph;
end
for i=1:size_p
    phase_y_vec(1,i) = phase(:,:,i);
    wsp_p_vec(1,i) = wsp;
    wph_p_vec(1,i) = wph;
end

figure(20)

subplot(2,3,1)
semilogx(wsp_m_vec,magn_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(wph_m_vec,magn_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(band,magn_y_vec,'LineWidth',1.5)
title('ÄéÜãñáììá Bode ôçò u(s)/ä_e(s)','FontSize',14)
axonasx=xlabel('Óõ÷íüôçôá ù [rad/sec]');
axonasy=ylabel('ÊÝñäïò [dB]');
axis ([min(band) max(band) min(magn) max(magn)])
grid on

subplot(2,3,4)
semilogx(wsp_p_vec,phase_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(wph_p_vec,phase_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(band,phase_y_vec,'LineWidth',1.5)
axonasx=xlabel('Óõ÷íüôçôá ù [rad/sec]');
axonasy=ylabel('ÖÜóç [deg]');
axis ([min(band) max(band) min(phase) max(phase)])
grid on

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[magn,phase] = bode(TF_8_de,band);
magn = 20*log10(magn);
size_m = size(magn);
size_m = size_m(1,3);
size_p = size(phase);
size_p = size_p(1,3);
for i=1:size_m
    magn_y_vec(1,i) = magn(:,:,i);
    wsp_m_vec(1,i) = wsp;
    wph_m_vec(1,i) = wph;
end
for i=1:size_p
    phase_y_vec(1,i) = phase(:,:,i);
    wsp_p_vec(1,i) = wsp;
    wph_p_vec(1,i) = wph;
end

figure(20)

subplot(2,3,2)
semilogx(wsp_m_vec,magn_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(wph_m_vec,magn_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(band,magn_y_vec,'LineWidth',1.5)
title('ÄéÜãñáììá Bode ôçò è(s)/ä_e(s)','FontSize',14)
axonasx=xlabel('Óõ÷íüôçôá ù [rad/sec]');
axonasy=ylabel('ÊÝñäïò [dB]');
axis ([min(band) max(band) min(magn) max(magn)])
grid on

subplot(2,3,5)
semilogx(wsp_p_vec,phase_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(wph_p_vec,phase_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(band,phase_y_vec,'LineWidth',1.5)
axonasx=xlabel('Óõ÷íüôçôá ù [rad/sec]');
axonasy=ylabel('ÖÜóç [deg]');
axis ([min(band) max(band) min(phase) max(phase)])
grid on

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[magn,phase] = bode(TF_a_de,band);
magn = 20*log10(magn);
size_m = size(magn);
size_m = size_m(1,3);
size_p = size(phase);
size_p = size_p(1,3);
for i=1:size_m
    magn_y_vec(1,i) = magn(:,:,i);
    wsp_m_vec(1,i) = wsp;
    wph_m_vec(1,i) = wph;
end
for i=1:size_p
    phase_y_vec(1,i) = phase(:,:,i);
    wsp_p_vec(1,i) = wsp;
    wph_p_vec(1,i) = wph;
end

figure(20)

subplot(2,3,3)
semilogx(wsp_m_vec,magn_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(wph_m_vec,magn_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(band,magn_y_vec,'LineWidth',1.5)
title('ÄéÜãñáììá Bode ôçò á(s)/ä_e(s)','FontSize',14)
axonasx=xlabel('Óõ÷íüôçôá ù [rad/sec]');
axonasy=ylabel('ÊÝñäïò [dB]');
axis ([min(band) max(band) min(magn) max(magn)])
grid on

subplot(2,3,6)
semilogx(wsp_p_vec,phase_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(wph_p_vec,phase_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(band,phase_y_vec,'LineWidth',1.5)
axonasx=xlabel('Óõ÷íüôçôá ù [rad/sec]');
axonasy=ylabel('ÖÜóç [deg]');
axis ([min(band) max(band) min(phase) max(phase)])
grid on