echo on

% This file demonstrates MATLAB's ability for classical control system
% design by going through the design of a YAW DAMPER for a Jet Transport
% aircraft.

echo off
%	Copyright (c) 1986-93 by the MathWorks, Inc.
echo on

pause % Press any key to continue ...


% Define Jet Transport model MACH=0.8 H=40,000ft

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];
states='beta yaw roll phi';
inputs='rudder aileron';
outputs='yaw-rate bank-angle';
printsys(A,B,C,D,inputs,outputs,states)

% 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 ...

% This model has one set of eigenvalues that are lightly damped.  They 
% correspond to what is called the Dutch Roll Mode.  We need to design
% a compensator that increases the damping of these poles.  

disp('Open Loop Eigenvalues'), damp(A); 
subplot(111)
pzmap(A,[],[],[]); 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(A,B,C,D); 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.  Define the time vector T before invoking IMPULSE.

T = 0:0.2:20;	% Define time vector from 0 to 20 secs in steps of 0.2

pause % Press any key to continue ...
% Plot responses as separate graphs
%subplot(221), impulse(A,B,C(1,:),D(1,:),1,T); title('Input 1 Output 1')
%subplot(222), impulse(A,B,C(2,:),D(2,:),1,T); title('Input 1 Output 2')
%subplot(223), impulse(A,B,C(1,:),D(1,:),2,T); title('Input 2 Output 1')
%subplot(224), impulse(A,B,C(2,:),D(2,:),2,T); title('Input 2 Output 2')
%pause % Press any key after plot ...
echo off
subplot(221), impulse(A,B,C(1,:),D(1,:),1,T); title('Input 1 Output 1')
subplot(222), impulse(A,B,C(2,:),D(2,:),1,T); title('Input 1 Output 2')
subplot(223), impulse(A,B,C(1,:),D(1,:),2,T); title('Input 2 Output 1')
subplot(224), impulse(A,B,C(2,:),D(2,:),2,T); title('Input 2 Output 2')
pause
echo on

% 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 ...

% Typically yaw dampers are designed using yaw-rate as the sensed output
% and rudder as the input.  Let's look at that frequency response.
bode(A,B,C(1,:),D(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.
[a,b,c,d] = ssselect(A,B,C,D,1,1); % Extract system with input 1 and output 1

% Let's do some designs.  The simplest compensator is a gain.  We can determine
% values for this gain using the root locus
clg
rlocus(a,b,c,d); pause % Press any key after plot ...

% Oops, looks like we need positive feedback.
rlocus(a,b,-c,-d); 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(a,b,-c,-d);

disp(['You chose gain: ',num2str(k)]), damp(esort(poles));

pause % Press any key to continue ...

% Let's form the closed loop system so that we can analyze the design.
[ac,bc,cc,dc] = feedback(a,b,c,d,[],[],[],-k);

% These eigenvalues should match the ones you chose.
disp('Closed loop eigenvalues'), damp(ac);

% Time response using our time vector T
impulse(ac,bc,cc,dc,1,T);  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.  Feedback using
% input 1 and output 1 of plant.

% Feedback with selection vectors assumes positive feedback
[Ac,Bc,Cc,Dc] = feedback(A,B,C,D,[],[],[],k,[1],[1]);
disp('Closed loop eigenvalues'), damp(Ac);

% Time response
%subplot(221), impulse(Ac,Bc,Cc(1,:),Dc(1,:),1,T); title('Input 1 Output 1')
%subplot(222), impulse(Ac,Bc,Cc(2,:),Dc(2,:),1,T); title('Input 1 Output 2')
%subplot(223), impulse(Ac,Bc,Cc(1,:),Dc(1,:),2,T); title('Input 2 Output 1')
%subplot(224), impulse(Ac,Bc,Cc(2,:),Dc(2,:),2,T); title('Input 2 Output 2')
%pause % Press any key after plot ...
echo off
subplot(221), impulse(Ac,Bc,Cc(1,:),Dc(1,:),1,T); title('Input 1 Output 1')
subplot(222), impulse(Ac,Bc,Cc(2,:),Dc(2,:),1,T); title('Input 1 Output 2')
subplot(223), impulse(Ac,Bc,Cc(1,:),Dc(1,:),2,T); title('Input 2 Output 1')
subplot(224), impulse(Ac,Bc,Cc(2,:),Dc(2,:),2,T); title('Input 2 Output 2')
pause
echo on

% 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.
%              Ks
%    H(s) = --------
%            (s + a)

% Choosing a = 0.333 for a time constant of 3 seconds, form the washout
[aw,bw,cw,dw] = zp2ss([0],[-.333],1);

% Connect the washout in series with our design model
[a,b,c,d] = series(a,b,c,d,aw,bw,cw,dw);

% Do another root locus
clg
rlocus(a,b,-c,-d); 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(a,b,-c,-d);
disp(['You chose gain: ',num2str(k)]), damp(esort(poles));

% Look at the closed loop response
[ac,bc,cc,dc] = feedback(a,b,c,d,[],[],[],-k);
impulse(ac,bc,cc,dc,1,T);  pause % Press any key to continue ...

% Now form the controller (washout + gain) 
[aw,bw,cw,dw] = series(aw,bw,cw,dw,[],[],[],k);

% Close the loop with the original model
[Ac,Bc,Cc,Dc] = feedback(A,B,C,D,aw,bw,cw,dw,[1],[1]);

% Final closed-loop response
%subplot(221), impulse(Ac,Bc,Cc(1,:),Dc(1,:),1,T); title('Input 1 Output 1')
%subplot(222), impulse(Ac,Bc,Cc(2,:),Dc(2,:),1,T); title('Input 1 Output 2')
%subplot(223), impulse(Ac,Bc,Cc(1,:),Dc(1,:),2,T); title('Input 2 Output 1')
%subplot(224), impulse(Ac,Bc,Cc(2,:),Dc(2,:),2,T); title('Input 2 Output 2')
%pause % Press any key after plot
echo off
subplot(221), impulse(Ac,Bc,Cc(1,:),Dc(1,:),1,T); title('Input 1 Output 1')
subplot(222), impulse(Ac,Bc,Cc(2,:),Dc(2,:),1,T); title('Input 1 Output 2')
subplot(223), impulse(Ac,Bc,Cc(1,:),Dc(1,:),2,T); title('Input 2 Output 1')
subplot(224), impulse(Ac,Bc,Cc(2,:),Dc(2,:),2,T); title('Input 2 Output 2')
pause % Press any key to continue ...
echo on

% 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


