% Step6_Longitudinal_Design.m  Version 8.0 1/23/03
% 
% OBJECTIVE: Set up linear models for use in linear control system design
%            for the longitudinal subsystem
% INPUTS: The file modelLong.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 (Lsys, Ltfsys, Lzpksys).
% 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 modelLong aL bL cL dL Vt Hp aircraft % Load lateral directional subsystem linear model
disp(aircraft)
Vt
Hp
Lsys=ss(aL,bL,cL,dL);
set(Lsys,'statename',{'u(f/s)' 'w(f/s)' 'q(r/s)' 'thet(r)' 'h(f)'})
set(Lsys,'inputname',{'deltaE(r)' 'Bhp(hp)'})
set(Lsys,'outputname',{'u(f/s)' 'w(f/s)' 'q(r/s)' 'thet(r)' 'h(f)' 'Vt(f/s)' 'alf(r)' 'gam(r)'})
Lsys
%Ltfsys=tf(Lsys)
Lzpksys=zpk(Lsys)
[Wn,Z,Poles]=damp(Lsys)
% u=[deltaE; Bhp]

t=0:0.01:25;% Generate step response for elevator input
deltaE=-0.1; 
Bhp=0;
u(1,100:length(t))=deltaE;
u(2,100:length(t))=Bhp;
[yL,t]=lsim(Lsys,u,t);
note=['Perturbation from Trim, deltaE = ',num2str(deltaE),' (rad), Bhp = ',num2str(Bhp),' (Hp)'];

subplot(3,1,1)
plot(t,yL(:,3)); title(note)
grid; xlabel('T (s)'); ylabel('q (rad/s)')
subplot(3,1,2)
plot(t,yL(:,4))
grid; xlabel('T (s)'); ylabel('Theta,\theta (rad)')
subplot(3,1,3)
plot(t,yL(:,7))
grid; xlabel('T (s)'); ylabel('Alpha,\alpha (rad)')

figure
subplot(3,2,1)
plot(t,yL(:,1)); grid; title('u(f/s)')
subplot(3,2,2)
plot(t,yL(:,2)); grid; title('w(f/s)')
subplot(3,2,3)
plot(t,yL(:,5)); grid; title('h(f)')
subplot(3,2,4)
plot(t,yL(:,6)); grid; title('Vt(f/s)')
subplot(3,2,5)
plot(t,yL(:,8)); grid; title('gam(rad)')


% To set up for q feedback to the elevator in order to 
% increase the damping ratio of the short period mode
QperDe=Lsys(3,1)
QperDeZPK=zpk(QperDe)
figure
rlocus(QperDe)
title('Input deltaE to output q(rad/sec)')
figure
rlocus(-QperDe) % Do this again with the opposite sign.
title('Input deltaE to output -q(rad/sec)')


sisotool 
% Import QperDeZPK 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 short period complex pair increases its damping ratio.
% Increased damping is noted in the root locus if the short period 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.

