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

%ДедпмЭнб беспукЬцпхт Learjet 24 (American Imperial Units)
%2з ресЯрфщуз: Рлеэуз ме мЭгйуфп вЬспт
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%ГещмефсЯб БнбцпсЬт
S = 230;    c_mean = 7.0;   b = 34.0;

%УхниЮкет рфЮузт (Псйжьнфйб пмблЮ рфЮуз,Mach=0.7)
VTe = 677;   Ue = VTe;    We = 0;    Alt = 40000;   
thita_e = 0*pi/180;   ae = 2.7*pi/180;    gamma_e = thita_e - ae;

%ДедпмЭнб бфмьуцбйсбт,мЬжбт - Ущмбфьдефп уэуфзмб
Q = 134.6;  rho=0.0005909;  
g = 32.17;  Weight = 1.3*10^4;  m = Weight/g;   
Ix = 2.8*10^4;    Iy = 1.88*10^4;   Iz = 4.7*10^4;     Ixz = 0.13*10^4;     

%УхнфелеуфЭт мьнймзт кбфЬуфбузт
CLe = 0.41; CDe = 0.0335;   CTxe = 0.0335;  Cme = 0; CmTe = 0;

%ЕгкЬсуйпй-дйЭхихнузт ухнфелеуфЭт кбй рбсбгюгпй ехуфЬиейбт (БдйЬуфбфпй-ўопнет ехуфЬиейбт)
Clb = -0.110;   Clp = -0.450;   Clr = 0.160;
Cyb = -0.730;   Cyp = 0;        Cyr = 0.400;
Cnb = 0.127;    CnTb = 0;       Cnp = -0.008;   Cnr = -0.200;

%ЕгкЬсуйпй-дйЭхихнузт ухнфелеуфЭт кбй рбсбгюгпй елЭгчпх (БдйЬуфбфпй-ўопнет ехуфЬиейбт)
Clda = 0.178;    Cyda = 0;    Cnda = -0.020;
Cldr = 0.019;    Cydr = 0.140;    Cndr = -0.074;

%ЕгкЬсуйпй-дйЭхихнузт ухнфелеуфЭт кбй рбсбгюгпй ехуфЬиейбт (ДйбуфбфпЯ-ўопнет ехуфЬиейбт)
Yb = Q*S*Cyb/m;                Yv = Yb/VTe;    
Yp = 1/2*Q*S*b*Cyp/(m*VTe);    Yr = 1/2*Q*S*b*Cyr/(m*VTe);

Lb = Q*S*b*Clb/Ix;                Lv = Lb/VTe;    
Lp = 1/2*Q*S*b^2*Clp/(Ix*VTe);    Lr = 1/2*Q*S*b^2*Clr/(Ix*VTe);  

Nb = Q*S*b*Cnb/Iz;             Nv = Nb/VTe;
Np = 1/2*Q*S*b^2*Cnp/(Ix*VTe); Nr = 1/2*Q*S*b^2*Cnr/(Ix*VTe);

Lv_star = Lv / (1-Ixz^2/(Ix*Iz)); 
Lp_star = Lp / (1-Ixz^2/(Ix*Iz));
Lr_star = Lr / (1-Ixz^2/(Ix*Iz));


Nv_star = Nv / (1-Ixz^2/(Ix*Iz));
Np_star = Np / (1-Ixz^2/(Ix*Iz));
Nr_star = Nr / (1-Ixz^2/(Ix*Iz));

%Lateral control stability derivatives -DIMENSIONAL - BODY AXIS
Yda = -Q*S*Cyda/m;   Ydr = Q*S*Cydr/m; 

Lda = -Q*S*b*Clda/Ix;  Ldr = Q*S*b*Cldr/Ix;

Nda = -Q*S*b*Cnda/Iz;  Ndr = Q*S*b*Cndr/Iz;

Lda_star = Lda / (1-Ixz^2/(Ix*Iz));
Ldr_star = Ldr / (1-Ixz^2/(Ix*Iz));

Nda_star = Nda / (1-Ixz^2/(Ix*Iz));
Ndr_star = Ndr / (1-Ixz^2/(Ix*Iz));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Suntetmhmenes egkarsies aerodynamikes paragwgoi eusta8eias
%sunarthsei twn DIASTATWN paragwgwn eusta8eias
yv = Yv;
yp = Yp;
yr = Yr-Ue;
yf = g*cos(thita_e);

lv = Lv_star+Ixz/Ix*Nv_star;
lp = Lp_star+Ixz/Ix*Np_star;
lr = Lr_star+Ixz/Ix*Nr_star;
lf = 0;

nv = Nv_star+Ixz/Ix*Lv_star;
np = Np_star+Ixz/Ix*Lp_star;
nr = Nr_star+Ixz/Ix*Lr_star;
nf = 0;

%Suntetmhmenes egkarsies paragwgoi elegxou
%sunarthsei twn DIASTATWN paragwgwn elegxou kai aerodunamikwn
yda = Yda ;
ydr = Ydr ;

lda = Lda_star+Ixz/Ix*Nda_star;
ldr = Ldr_star+Ixz/Ix*Ndr_star;

nda = Nda_star+Ixz/Ix*Lda_star;
ndr = Ndr_star+Ixz/Ix*Ldr_star;

%Pinakes GXAS
A = [yv yp yr yf;
     lv lp lr lf;
     nv np nr nf;
     0 1 0 0];

B = [yda ydr;
     lda ldr;
     nda ndr;
     0 0];

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

D = [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)
 [num2,den2]=ss2tf(A,B,C,D,2)

 TF_v_da = tf(num1(1,:),den1)
 TF_p_da = tf(num1(2,:),den1)
 TF_r_da = tf(num1(3,:),den1)
 TF_f_da = tf(num1(4,:),den1)
 TF_b_da = tf(num1(5,:),den1)
 
 TF_v_dr = tf(num2(1,:),den2)
 TF_p_dr = tf(num2(2,:),den2)
 TF_r_dr = tf(num2(3,:),den2)
 TF_f_dr = tf(num2(4,:),den2)
 TF_b_dr = tf(num2(5,:),den2)
 
 approxTF_p_da = tf([0 lda],[1 -lp])
 %paragontopoihsh arithmhtwn kai xarakthristikhs
rizes_da = roots(den1);
rizes_Nv_da = roots(num1(1,:));
rizes_Np_da = roots(num1(2,:));
rizes_Nr_da = roots(num1(3,:));
rizes_Nf_da = roots(num1(4,:));
rizes_Nb_da = roots(num1(5,:));

rizes_dr = roots(den2);
rizes_Nv_dr = roots(num2(1,:));
rizes_Np_dr = roots(num2(2,:));
rizes_Nr_dr = roots(num2(3,:));
rizes_Nf_dr = roots(num2(4,:));
rizes_Nb_dr = roots(num2(5,:));

 %Pinakes idiodianusmatwn-idiotimwn
 A_epau = [yv yp/VTe yr/VTe yf/VTe;
        lv*VTe lp lr lf;
        nv*VTe np nr nf;
        0 1 0 0];
 [V,Lamda]=eig(A_epau);
 for i=1:4
     for j=1:4
         V_magn(i,j)=abs(V(i,j));
     end
 end
 
e=2;
 for i=1:3000
     t(i)=(i-1)/100;
     if and(ge(t,0),le(t,e));
        da(i)=1*pi()/180;
     else gt(t,e);
         da(i)=0;
     end
 end
 
 %Apokriseis se eisodo palmou twn ailerons

figure(10)
subplot(5,1,1)
[v_da,t_da]=lsim(0.3048*TF_v_da,da,t);
plot(t_da,v_da,'LineWidth',1.5)
title('БрпксЯуейт уе еЯупдп рблмпэ 2 дехфесплЭрфщн фщн рздблЯщн клЯузт','FontSize',14)
axonasy=ylabel('v [m/sec]');
set(axonasy,'FontSize',12)
grid on

subplot(5,1,2)
[p_da,t_da]=lsim(TF_p_da,da,t);
plot(t_da,p_da,'LineWidth',1.5)
axonasy=ylabel('p [rad/sec]');
set(axonasy,'FontSize',12)
grid on

subplot(5,1,3)
[r_da,t_da]=lsim(TF_r_da,da,t);
plot(t_da,r_da,'LineWidth',1.5)
axonasy=ylabel('r [rad/sec]');
set(axonasy,'FontSize',12)
grid on

subplot(5,1,4)
[f_da,t_da]=lsim(TF_f_da,da,t);
plot(t_da,f_da,'LineWidth',1.5)
axonasy=ylabel('ц [rad]');
set(axonasy,'FontSize',12)
grid on

subplot(5,1,5)
[b_da,t_da]=lsim(TF_b_da,da,t);
plot(t_da,b_da,'LineWidth',1.5)
axonasx=xlabel('t [sec]');
set(axonasx,'FontSize',12)
axonasy=ylabel('в [rad]');
set(axonasy,'FontSize',12)
grid on

%Apokriseis se eisodo va8midas tou rudder

figure(11)
subplot(5,1,1)
[v_dr,t_dr]=step(0.3048*TF_v_dr*pi/180,20);
plot(t_dr,v_dr,'LineWidth',1.5)
title('БрпксЯуейт уе еЯупдп мпнбдйбЯбт вбимЯдбт фпх рздблЯпх екфспрЮт','FontSize',14)
axonasy=ylabel('v [m/sec]');
set(axonasy,'FontSize',12)
grid on

subplot(5,1,2)
[p_dr,t_dr]=step(TF_p_dr*pi/180,20);
plot(t_dr,p_dr,'LineWidth',1.5)
axonasy=ylabel('p [rad/sec]');
set(axonasy,'FontSize',12)
grid on

subplot(5,1,3)
[r_dr,t_dr]=step(TF_r_dr*pi/180,20);
plot(t_dr,r_dr,'LineWidth',1.5)
axonasy=ylabel('r [rad/sec]');
set(axonasy,'FontSize',12)
grid on

subplot(5,1,4)
[f_dr,t_dr]=step(TF_f_dr*pi/180,20);
plot(t_dr,f_dr,'LineWidth',1.5)
axonasy=ylabel('ц [rad]');
set(axonasy,'FontSize',12)
grid on

subplot(5,1,5)
[b_dr,t_dr]=step(TF_b_dr*pi/180,20);
plot(t_dr,b_dr,'LineWidth',1.5)
axonasx=xlabel('t [sec]');
set(axonasx,'FontSize',12)
axonasy=ylabel('в [rad]');
set(axonasy,'FontSize',12)
grid on

figure(13)
[p_fo,t_da] = step(TF_p_da*pi()/180,20);
plot(t_da,p_fo,'LineWidth',1.5);
hold on
[p_app,t_da] = step(approxTF_p_da*pi()/180,20);
plot(t_da,p_app,'--','LineWidth',1.5);
title('Брьксйуз схимпэ ресйуфспцЮт','FontSize',14)
axonasx=xlabel('t');
set(axonasx,'FontSize',12)
axonasy=ylabel('p [rad/sec]');
set(axonasy,'FontSize',12)
legend('РлЮсет мпнфЭлп','РспуЭггйуз ресйуфспцЮт')
grid on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%ДйбгсЬммбфб Bode

rizes = roots(den1);
%Ухчньфзфб ПллбндйкЮт ресйуфспцЮт
rizes_d = [rizes(1,1) rizes(2,1)];
dutch = poly(rizes_d);
wd = sqrt(dutch(1,3))
%ЧспнйкЮ уфбиесЬ фпх roll
Tr = 1/(-rizes(3,1))
wr = 2*pi/Tr;
%ЧспнйкЮ уфбиесЬ фпх урейспейдпэт
Ts = 1/(-rizes(4,1))
ws = 2*pi/Ts;

band = logspace(-2,1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Щт рспт фб рздЬлйб клЯузт %%%%

[magn,phase] = bode(TF_b_da,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);
    wd_m_vec(1,i) = wd;
end
for i=1:size_p
    phase_y_vec(1,i) = phase(:,:,i);
    wd_p_vec(1,i) = wd;
end

figure(20)

subplot(2,3,1)
semilogx(wd_m_vec,magn_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(band,magn_y_vec,'LineWidth',1.5)
title('ДйЬгсбммб Bode фзт в(s)/д_a(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(wd_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_p_da,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);
    wd_m_vec(1,i) = wd;
end
for i=1:size_p
    phase_y_vec(1,i) = phase(:,:,i);
    wd_p_vec(1,i) = wd;
end



subplot(2,3,2)
semilogx(wd_m_vec,magn_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(band,magn_y_vec,'LineWidth',1.5)
title('ДйЬгсбммб Bode фзт p(s)/д_a(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(wd_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_r_da,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);
    wd_m_vec(1,i) = wd;
end
for i=1:size_p
    phase_y_vec(1,i) = phase(:,:,i);
    wd_p_vec(1,i) = wd;
end


subplot(2,3,3)
semilogx(wd_m_vec,magn_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(band,magn_y_vec,'LineWidth',1.5)
title('ДйЬгсбммб Bode фзт r(s)/д_a(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(wd_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_b_dr,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);
    wd_m_vec(1,i) = wd;
end
for i=1:size_p
    phase_y_vec(1,i) = phase(:,:,i);
    wd_p_vec(1,i) = wd;
end

figure(30)

subplot(2,3,1)
semilogx(wd_m_vec,magn_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(band,magn_y_vec,'LineWidth',1.5)
title('ДйЬгсбммб Bode фзт в(s)/д_r(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(wd_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_p_dr,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);
    wd_m_vec(1,i) = wd;
end
for i=1:size_p
    phase_y_vec(1,i) = phase(:,:,i);
    wd_p_vec(1,i) = wd;
end


subplot(2,3,2)
semilogx(wd_m_vec,magn_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(band,magn_y_vec,'LineWidth',1.5)
title('ДйЬгсбммб Bode фзт p(s)/д_r(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(wd_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_r_dr,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);
    wd_m_vec(1,i) = wd;
end
for i=1:size_p
    phase_y_vec(1,i) = phase(:,:,i);
    wd_p_vec(1,i) = wd;
end



subplot(2,3,3)
semilogx(wd_m_vec,magn_y_vec,'r--','LineWidth',1.5)
hold on
semilogx(band,magn_y_vec,'LineWidth',1.5)
title('ДйЬгсбммб Bode фзт r(s)/д_r(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(wd_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