% Control system design for the lateral-Directional subsystem
% This code can be run after FlatEarthAnal.m has been run.
% FlatEarthAnal.m saves the lateral-directional subsystem in
% a file called modelLat

close all
disp(' '); disp('Start here'); disp(' ');
load modelLat aLD bLD cLD dLD Vt Hp aircraft % Load lateral directional subsystem linear model
%whos
disp(aircraft)
Vt
Hp
LDsys=ss(aLD,bLD,cLD,dLD);
set(LDsys,'statename',{'v(f/s)' 'p(r/s)' 'r(r/s)' 'phi(r)' 'psi(r)'})
set(LDsys,'inputname',{'deltaA(r)' 'deltaR(r)'})
set(LDsys,'outputname',{'v(f/s)' 'p(r/s)' 'r(r/s)' 'phi(r)' 'psi(r)' 'beta(r)'})
LDsys
[Wn,Z,Poles]=damp(LDsys)
LDtfsys=tf(LDsys)
LDzpksys=zpk(LDsys)


ifig=0;
% Get roll mode approximate system, p only
xselect=2; uselect=1; yselect=2;
[aRoll,bRoll,cRoll,dRoll]=selector(aLD,bLD,cLD,dLD,xselect,uselect,yselect)
Rollsys=ss(aRoll,bRoll,cRoll,dRoll);
ifig=ifig+1; figure(ifig);
rlocus(Rollsys) %
title('Root locus on p(s)/deltaA(s), roll mode approximation')
ifig=ifig+1; figure(ifig);
bode(Rollsys)
title('Bode Plot of p(s)/deltaA(s), roll mode approximation')

% Get roll mode approximate system, p and phi states but only phi output
xselect=[2,4]; uselect=1; yselect=4;
[aRoll2,bRoll2,cRoll2,dRoll2]=selector(aLD,bLD,cLD,dLD,xselect,uselect,yselect)
Rollsys2=ss(aRoll2,bRoll2,cRoll2,dRoll2);
ifig=ifig+1; figure(ifig);
rlocus(Rollsys2) %
title('Root locus on phi(s)/deltaA(s), roll mode approximation')
ifig=ifig+1; figure(ifig);
bode(Rollsys2)
title('Bode Plot of phi(s)/deltaA(s), roll mode approximation')

% Get phi/deltaA for full system
xselect=[1,2,3,4,5]; uselect=1; yselect=4;
[aRoll3,bRoll3,cRoll3,dRoll3]=selector(aLD,bLD,cLD,dLD,xselect,uselect,yselect)
Rollsys3=ss(aRoll3,bRoll3,cRoll3,dRoll3);
ifig=ifig+1; figure(ifig);
rlocus(Rollsys3) %
title('Root locus on phi(s)/deltaA(s), fifth order system')
ifig=ifig+1; figure(ifig);
bode(Rollsys3)
title('Bode Plot of phi(s)/deltaA(s), fifth order system')
disp('PhiDaOL');PhiDaOL=tf(Rollsys3)

Kphi=.7
closedLoop3=feedback(Kphi*Rollsys3,1,-1);
[phi3,t]=step(closedLoop3,25);
ifig=ifig+1; figure(ifig);
plot(t,phi3)
string2=['Step response of closed loop system, Kphi= ',num2str(Kphi)]; 
xlabel('time (sec)'); ylabel('phi(rad)'); title(string2)

ifig=ifig+1; figure(ifig);
bode(closedLoop3)
string1=['Bode Plot of phi(s)/phiC(s), Kphi= ',num2str(Kphi)];
title(string1)
disp('PhiDaCL'); PhiDaCL=tf(closedLoop3)

% Find the forward loop gain to get the right low frequency response.
[num,den]=tfdata(PhiDaCL)
nx=num{1,1}
dx=den{1,1}
Gforward=nx(5)/dx(5)
% We can correct the dc gain problem by multiplying by a forward gain of 1/Gforward.
Kphiforward=1/Gforward
[phi4,t]=step(closedLoop3*Kphiforward,25);
ifig=ifig+1; figure(ifig);
plot(t,phi4)
string2=['Step response of closed loop system, Kphi= ',num2str(Kphi),', Kphiforward= ',num2str(Kphiforward)]; 
xlabel('time (sec)'); ylabel('phi(rad)'); title(string2)


