clc
clear all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Height hold system 
K8 = 1;
Kq = 1;
Kh = 10^-3;

g = 32.17; Ue = 221; thita_e = 0;
Weight = 5.640*10^5;  m = Weight/g;   Iy = 3.23*10^7;

Zw = -8.969*10^3;  Zq = -1.090*10^5;  Zwdot = 5.851*10^2;

Mw = -5.717*10^4;  Mq = -1.153*10^7;  Mwdot = -7.946*10^3;

Zde = -1.125*10^5;  

Mde = -1.221*10^7;  

zw = Zw/(m-Zwdot);
zq = (Zq+m*Ue)/(m-Zwdot);
z8 = -m*g*sin(thita_e)/(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));

zde = Zde/(m-Zwdot);
mde = Mde/Iy + +Mwdot*Zde/(Iy*(m-Zwdot));
%Pinakes GXAS: Short period approximation augmented with и and h 


A = [zw zq z8;
     mw mq m8;
     0 1 0];
    

B = [zde;
     mde;
     0];

C = [-1 0 Ue];

D = [0];

LongDyn = ss(A,B,C,D)

[Nhdot_de,Dhdot_de]=ss2tf(A,B,C,D,1);

Ghdot_de = tf(Nhdot_de,Dhdot_de);
integrator = tf([0 1],[1 0]);
Ghde = series(integrator,Ghdot_de)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%Dianusma Kerdous K'={Kw Kq K8} gia epi8umhtous polous:
%%s=-1.8+-2.4i kai s=-0.25 ara h epi8umhth xarakthristikh:
%%s^3+3.85s^2+9.95s+2.25
Pin = [zde mde 0;
       zq*mde-zde*mq zde*mw-zw*mde mde;
       z8*mde-zde*m8 0 zde*mw-zw*mde];

Const = [3.85+mq+zw;
         9.95+m8-zw*mq+zq*mw;
         2.25-zw*m8+z8*mw];
Kerdh = inv(Pin)*Const    

%%Generic feedback SAS variables%%

%%Controller
Kc = -1;
Ti = 0;
Td = 0;

if eq(Ti,0)
    Ncont = [Td 1];
    Dcont = [0 1];
    Gcont = Kc*tf(Ncont,Dcont);
else
    Ncont = [Ti*Td Ti 1];
    Dcont = [0 Ti 0];
    Gcont = Kc*tf(Ncont,Dcont);
end

%%Actuator
Kact = 1;
lamda = 4; 
Nact = [0 lamda];
Dact = [1 lamda];
Gact = Kact*tf(Nact,Dact);

%%Plant 
Gplant = Ghde;

%%Feedback
H = Kh;

%%Open Loop TF:
TF_OL = series(Gcont,Gact);
TF_OL = series(TF_OL,Gplant);
%%Closed Loop TF:
TF_CL = feedback(TF_OL,H);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%h feedback Root Locus
figure(1)
[poloi,mhden]=pzmap(Gplant);
pragmp = real(poloi);
fantp = imag(poloi);
pragmz = real(mhden);
fantz = imag(mhden);
area=70;
hold on
scatter(pragmp,fantp,area,'X','LineWidth',3)
scatter(pragmz,fantz,area,'O','LineWidth',3)
scatter(-4,0,area,'X','LineWidth',3)
%Desired pole positions cong -1.8+2.4i
hold on
scatter(-1.8,2.4,area,'r','O','filled')
hold on
scatter(-1.8,-2.4,area,'r','O','filled')
hold on
grid on
hold on
rlocus(TF_CL)
title('Фьрпт Сйжюн Клейуфпэ Всьчпх - h Feedback','FontSize',12)
axonasx=xlabel('Рсбгмбфйкьт Ьопнбт жщ_n');
set(axonasx,'FontSize',12)
axonasy=ylabel('Цбнфбуфйкьт Ьопнбт iщ_d');
set(axonasy,'FontSize',12)
grid on
legend('Рьлпй егкбфЬуфбузт','МзденйуфЭт егкбфЬуфбузт','Рьлпт еренесгзфЮ','ЕрйихмзфЮ ресйпчЮ рьлщн');
hold on
rlocus(TF_OL,':')
axis([-5 4 -4 4])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Actrl = A-B*Kerdh';

[Nhdot_de,Dhdot_de]=ss2tf(Actrl,B,C,D,1);

Ghdot_de = tf(Nhdot_de,Dhdot_de);
integrator = tf([0 1],[1 0]);
Ghde = series(integrator,Ghdot_de)
Gplant = Ghde;
%%Generic feedback SAS variables%%

%%Open Loop TF:
TF_OL = series(Gcont,Gact);
TF_OL = series(TF_OL,Gplant);
%%Closed Loop TF:
TF_CL = feedback(TF_OL,H);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Full state feedback Root Locus
figure(2)
[poloi,mhden]=pzmap(Gplant);
pragmp = real(poloi);
fantp = imag(poloi);
pragmz = real(mhden);
fantz = imag(mhden);
hold on
scatter(pragmp,fantp,area,'X','LineWidth',3)
scatter(pragmz,fantz,area,'O','LineWidth',3)
scatter(-4,0,area,'X','LineWidth',3)
%Desired pole positions cong -1.8+2.4i
hold on
scatter(-1.8,2.4,area,'r','O','filled')
hold on
scatter(-1.8,-2.4,area,'r','O','filled')
hold on
grid on
hold on
rlocus(TF_CL)
title('Фьрпт Сйжюн Клейуфпэ Всьчпх - Full State Feedback','FontSize',12)
axonasx=xlabel('Рсбгмбфйкьт Ьопнбт жщ_n');
set(axonasx,'FontSize',12)
axonasy=ylabel('Цбнфбуфйкьт Ьопнбт iщ_d');
set(axonasy,'FontSize',12)
grid on
legend('Рьлпй егкбфЬуфбузт','МзденйуфЭт егкбфЬуфбузт','Рьлпт еренесгзфЮ','ЕрйихмзфЮ ресйпчЮ рьлщн');
hold on
rlocus(TF_OL,':')
axis([-5 4 -4 4])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%