echo off
% JETDEMO demonstrates MATLAB's facilities for classical control 
% system design by stepping through the design of a YAW DAMPER 
% for a Jet Transport aircraft.

%   Copyright (c) 1986-1999 The Mathworks, Inc. All Rights Reserved.
%   $Revision: 1.5.1.2 $  $Date: 1999/01/05 15:21:26 $
clc
help jetdemo
disp('% Press any key to continue ...')
pause
clc
disp('% Define Jet Transport model MACH=0.8, H=40,000ft')

fprintf('\n')
A=[-.0558 -.9968 .0802 .0415;
   .598 -.115 -.0318 0;
   -3.05 .388 -.4650 0;
   0 0.0805 1 0]
B=[.0729  .0001;
   -4.75   1.23;
   1.53   10.63;
   0      0]
C=[0 1 0 0;
   0 0 0 1]
D=[0 0;
   0 0]
echo on

states={'beta' 'yaw' 'roll' 'phi'};
inputs={'rudder' 'aileron'};
outputs={'yaw rate' 'bank angle'};

pause % Press any key to continue ...

sys = ss(A,B,C,D,'statename',states,'inputname',inputs,'outputname',outputs)

% These are the state space matrices for a Jet Transport during 
% cruise flight.  The model has two inputs and two outputs.  The
% units are radians for beta (sideslip angle) and phi (bank angle) 
% and radians/sec for yaw (yaw rate) and roll (roll rate).  
% The rudder and aileron deflections are in degrees.

pause % Press any key to continue ...
clc
% This model has a pair of lightly damped eigenvalues.  They 
% correspond to what is called the Dutch roll mode.  We need to design
% a compensator that increases the damping of these poles.  

% Open Loop Eigenvalues
damp(sys)

pzmap(sys)

pause % Press any key after plot ...

% Our design criteria is to provide damping ratio zeta > 0.35, with natural
% frequency Wn < 1.0 rad/sec.  We want to design the compensator using
% classical methods.  Let's do some open loop analysis to determine possible 
% control strategies.

% Time response (we could use STEP or IMPULSE here)
impulse(sys), 

pause % Press any key after plot ...

% The time responses show that the system is indeed lightly damped.  But 
% the time frame is much to long.  Let's look at the response over a 
% smaller time frame of 20 seconds:
impulse(sys,20)

% Look at the plot from aileron (input 2) to bank_angle (output 2).  The
% aircraft is oscillating around a non-zero bank angle.  Thus the aircraft
% turns in response to an aileron impulse.  This behavior will be important
% later.

pause % Press any key to continue ...
clc
% Typically yaw dampers are designed using yaw-rate as the sensed output
% and rudder as the input.  Let's look at the frequency response for this
% channel

bode(sys(1,1)), 

pause % Press any key after plot ...

% From this frequency responses we see that the rudder has much effect 
% around the lightly damped Dutch roll mode (at 1 rad/sec).  To make the 
% design easier, extract of the subsystem from rudder to yaw rate.

sys11 = sys(1,1); % Extract system with input 1 and output 1

% The simplest compensator is a gain.  We can determine adequate values 
% for this gain using the root locus
clf
rlocus(sys11), 

pause % Press any key after plot ...

% Oops, looks like we need positive feedback.
rlocus(-sys11), sgrid, 

pause % Press any key after plot ...

% That looks better.  So using just simple feedback we can achieve a 
% damping ratio of zeta=0.45.

% Now its your turn. Use RLOCFIND to select the point on the root locus
% with maximum damping.

pause % Press any key and then select point on the plot ...
[k,poles] = rlocfind(-sys11);

echo off
fprintf('\n');
disp(['You chose the gain: ',num2str(k)]), 
echo on
damp(esort(poles));

pause % Press any key to continue ...

% Let's form the closed loop system so that we can analyze the design.
% Note: feedback assumes negative feedback by default.
cl11 = feedback(sys11,-k);

% These eigenvalues should match the ones you chose.

% Closed loop eigenvalues
damp(cl11)

% Impulse response:
impulse(cl11,20);  

pause % Press any key after plot ...

% So the response looks pretty good.  Let's close the loop on the original
% model and see how the response from the aileron looks.  The feedback loop 
% involves input 1 and output 1 of plant:

cloop = feedback(sys,-k,1,1);

% Closed loop eigenvalues
damp(cloop);

% MIMO impulse response

impulse(cloop,20)
pause % Press any key to continue ...
clc
% Look at the plot from aileron (input 2) to bank_angle (output 2).  When
% we move the aileron the system no longer continues to bank like a normal
% aircraft.  We have over-stabilized the spiral mode.  The spiral mode is
% typically a very slow mode and allows the aircraft to bank and turn without
% constant aileron input.  Pilots are used to this behavior and will not like
% our design if it doesn't allow them to fly normally.  Our design has moved
% the spiral mode so that it has a faster frequency.

pause % Press any key to continue ...

% What we need to do is make sure the spiral mode doesn't move farther into
% the left half plane when we close the loop.  One way flight control 
% designers have fixed this problem is to use a washout filter, i.e.
%              k s
%    H(s) = --------
%            (s + a)

% Choosing a = 0.333 for a time constant of 3 seconds, form the washout
H = zpk(0,-0.333,1)

% Connect the washout in series with our design model (from input1 to output1)
oloop = H * sys11;

% Do another root locus to determine the filter gain k:
clf
rlocus(-oloop); sgrid, 

pause % Press any key after plot

% Now the maximum damping is zeta = 0.25.  Using RLOCFIND chose the gain for
% maximum damping:

pause % Press any key and then select point on plot ...

[k,poles] = rlocfind(-oloop);
echo off
fprintf('\n')
disp(['You chose gain: ',num2str(k)]), damp(poles);
echo on

% Look at the closed-loop response
cl11 = feedback(oloop,-k);
impulse(cl11,20);  

pause % Press any key to continue ...

clc
% Now form the controller (washout + gain) 
WOF = -k * H;

% Close the loop with the original model (using input 1 and output 1 
% for feedback)
cloop = feedback(sys,WOF,1,1);

% Final closed-loop impulse response
impulse(cloop,20)

% Although we didn't quite meet the criteria, our design increased the damping
% of the system substantially and the design does allow the pilots to fly the
% aircraft normally.

echo off


