% Step7_lateral_Design.m   Version 8.0 1/23/03
% 
% OBJECTIVE: Set up linear models for use in linear control system design
%            for the lateral-directional subsystem
% INPUTS: The file modelLat.mat created by Step5_Linearize_MATLABx.m
% OUTPUTS: Many transfer functions, poles, natural frequencies and 
%          damping ratios in the MATLAB command window. Also several linear
%          time invariant systems are created in the MATLAB workspace f
%          or subsequent analysis (LDsys, LDtfsys, LDzpksys).
% NOTES: 1. This code can be run after Step5_Linearize_MATLABx.m has been run.
%        2. This code clears memory at the start if execution


clear all
close all
disp(' '); disp('>>>>>>Start here<<<<<<<'); disp(' ');
disp(' Note: memory is cleared at the start of this script'); 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)'})
set(LDsys,'outputname',{'v(f/s)' 'p(r/s)' 'r(r/s)' 'phi(r)' 'psi(r)' 'beta(r)' 'Ya(f/s2)'})

LDsys
LDtfsys=tf(LDsys)
LDzpksys=zpk(LDsys)
[Wn,Z,Poles]=damp(LDsys)

t=0:0.01:25; % Generate step response for rudder input
deltaA=-0; 
deltaR=-0.1; 
u(1,100:length(t))=deltaA;
u(2,100:length(t))=deltaR;
[yLD,t]=lsim(LDsys,u,t);
note=['Perturbation from Trim, deltaA = ',num2str(deltaA),' (rad), deltaR = ',num2str(deltaR),' (rad)'];

subplot(3,1,1)
plot(t,yLD(:,3)); title(note)
grid; xlabel('T (s)'); ylabel('r (rad/s)')
subplot(3,1,2)
plot(t,yLD(:,5))
grid; xlabel('T (s)'); ylabel('psi,\psi (rad)')
subplot(3,1,3)
plot(t,yLD(:,6))
grid; xlabel('T (s)'); ylabel('beta,\beta (rad)')

figure
subplot(3,1,1)
plot(t,yLD(:,2))
grid; xlabel('T (s)'); ylabel('p (rad/s)')
subplot(3,1,2)
plot(t,yLD(:,4))
grid; xlabel('T (s)'); ylabel('phi,\phi (rad)')
subplot(3,1,3)
plot(t,yLD(:,1))
grid; xlabel('T (s)'); ylabel('Vt (f/s)')


% To set up for r feedback to the rudder in order to 
% increase the damping ratio of the dutch roll mode
RperDr=LDsys(3,2)    % Get R per Dr transfer function
RperDrZPK=zpk(RperDr)

PperDa=LDsys(2,1)    % Get P per Da transfer function
PperDaZPK=zpk(PperDa)


sisotool 
% Import RperDrZPK into G of sisotool
% Examine the root locus and make sure you have the sign for stabilizing
% feedback. If the locus goes the wrong way, click the +- sign in the
% lower left corner of the block diagram in sisotool. The locus goes the
% right way is the dutch roll complex pair increases its damping ratio.
% Increased damping is noted in the root locus if the dutch roll complex poles
% move towards the real axis. On the root locus, a line of constant damping is a radial line.
% On the root locus, a line of constant natural frequency is a circle.
% If you click on the root locus plot you can bring up the property editor.
% In the property editor, open the Options tab. Select Show grid.
% This will bring up lines of constant damping ratio on the root locus.
% In the root locus the purple squares are the closed loop pole locations
% for the nominal gain. Click on any square and move it until you get the
% damping ratio that you want. The nominal gain will automatically be
% adjusted to acheive that result. This should complete your design.
