clf;axis([1,2,3,4]);axis;
echo on
%---------------------------------------------------------
%
%          Dept. of Electrical & Computer Eng.
%       University of California, Santa Barbara.
%
%               ECE 147 B:  Digital Control Systems
%
%               module 7:    State feedback
%
%       -------------------------------------------------

% State feedback design is studied in this module.
% This material is covered in Chapter 8 of the text.

% Adjust the Command and Graphics windows to your preferred
% sizes.

pause	% press any key to continue.

% A simple second order system is studied as an example.  The 
% system is initially specified as a continuous system and
% then transformed, via zero-order hold equivalence, to 
% a discrete time system.

% Consider the problem of pointing a satellite receiving
% dish.  The angle of the dish must be maintained constant
% in the presence of disturbances.  Define theta as this angle.
% The equation of motion is,
%
%    J*theta" + B*theta' = Tc + Td,
%
% where theta' is the derivative of the angle, theta" is the second
% derivative, J is the moment of inertia, B is the damping,
% Tc is the motor drive torque and Td is the torque exerted
% by the wind disturbance.
%
% In Laplace form, this equation is,
%
%	(J*s^2 + B*s)theta = Td + Tc.
%

pause	% press any key to continue

% Tc is the control input and Td is the disturbance to be
% rejected.  Assume that the system is in equilibrium, pointed
% in the correct direction.  We can measure theta and theta' via
% sensors mounted on the dish.
%
% Define the state as x = [theta ; theta'].  The state equations are,
%
%	theta'  =  theta'  = [0 1] x,
% and
%       theta"  =  -(B/J)*theta' + (1/J)*Td + (1/J)*Tc.
%
% In the more usual form, these are,
%
%              | 0     1 |        |  0  |         |  0  | 
%	x'  =  |         | x   +  |     | Td  +   |     | Tc
%              | 0  -B/J |        | 1/J |         | 1/J |   
%

pause	% press any key to continue.

% OK, this gives us the A and B matrices of a state-space 
% description.  Choose some values and form these matrices.

J = 100;
B = 5;

A = [0, 1; 0, -B/J];
B = [0, 0; 1/J, 1/J];

% Note that we defined B with two inputs: Td and Tc.
% To form a complete state-space system, we need to 
% create C and D matrices.  Because we measure the
% state directly C is the identity and D is zero.

C = eye(2,2);
D = zeros(2,2);
sat_dish = ss(A,B,C,D);
set(sat_dish,'Notes','Satellite dish');

pause	% press any key to continue.

% We can look at a frequency response of this system.

omega = logspace(-3,1,100);
bode(sat_dish,omega);
title('Frequency response of Satellite Dish')

% The plot with the 1/s characteristic at low frequencies
% is that from Tc (and Td - they are identical) to theta.

pause	% press any key to continue.

% Let's also do an open-loop time response to see the
% effect of wind on the satellite dish.  Wind will be
% modeled as a random noise sequence going through a
% low pass filter.

t = [0:0.05:50]';
w = rand(length(t),1) - 0.5;
lpfilter = tf(20,[10,1]);
wind = lsim(lpfilter,w,t);
clear w lpfilter

% Let's look at the wind disturbance signal.

plot(t,wind,'-')
title('Wind disturbance signal')
xlabel('time: seconds')
ylabel('amplitude')

% It looks gusty to me.  It goes in both directions which may
% not be too realistic.  If it was predominantly in one direction,
% the integrator in the plant would integrate up.  I.e. it would
% push the dish around.

pause	% press any key to continue.

% Now lets look at the effect of this on the open-loop system.
% Because there is no control, that input must be set to
% zero.  

dist = [wind,0*wind];
wind_resp = lsim(sat_dish,dist,t);

% The first column of wind_resp is the angle and the second
% column is the derivative of the angle.  We can select each
% of these out for plotting.

theta = wind_resp(:,1);
thetadot = wind_resp(:,2);

plot(t,theta,'-',t,thetadot,'--',t,wind,'-.')
title('Wind response')
legend('angle','d(angle)/dt','wind')
xlabel('time: seconds')
ylabel('amplitude')

% The angle drifts around quite badly and the objective
% will to be improve this response.

pause	% press any key to continue.

% The actual system will be preceeded by a zero-order hold
% and both outputs will be sampled with period T.  We can
% calculate the discrete equivalent of this with sys2dsys.

T = 1;			% 1 second sample period.

sat_dish_Z = c2d(sat_dish,T,'zoh');
set(sat_dish_Z,'Notes','ZOH equivalent of the satellite dish');

% Examine the pole locations:

clf
zgrid
axis('square')
axis([0.25,1.25,-0.5,0.5])
pzmap(sat_dish_Z)
pole(sat_dish_Z)

% We can see the integrator in the discrete system.

pause	% press any key to continue.

% To get the discrete state-space matrices, the ssdata
% command is used.  

[Ad,Bd,Cd,Dd] = ssdata(sat_dish_Z)

% The C and D matrices are unchanged as expected.

pause	% press any key to continue.

% Now let's check whether the system is controllable.
% The controllability matrix is formed with a bit of
% manipulation.  The first thing to observe is that we
% wish to control the system using the second input (i.e. Tc).
% So the B matrix in question is just the first column of Bd.

BdTc = Bd(:,2);

Cmat = [BdTc, Ad*BdTc];

size(Cmat)

rank(Cmat)

% The controllability matrix is full rank.  Good;  we can place
% both poles where we would like.

pause	% press any key to continue.

% Now to choose some pole locations.  Lets try z = 0 and
% z = 0.5.  That should give a fast response.

% Ackermann's formula can be used to calculate the state
% feedback gain.  Firstly we need to figure out what is
% the polynomial that has roots at these locations.
%
% I.e.  z(z - 0.5) = 0
%
%	z^2 - 0.5z = 0.
% 
% So, using the notation in the book, 
%
%	 alpha_c(Ad) = Ad^2 - 0.5*Ad

alphac = Ad^2 - 0.5*Ad;

% and K can now be calculated:

K = [0, 1]*inv(Cmat)*alphac 

% There is a control toolbox command called "place" which also
% does this calculation.  Run help on that if you can't understand
% what I have done here.

pause	% press any key to continue

% The numbers look quite large.  We will do a simulation
% to see what happens.  To do this we must form the closed
% loop system.

% First, we will check where the poles of the system are.
% One way is to look at the eigenvalues of the closed loop
% system.   These are,

eig(Ad - BdTc*K)

% Good.  Exactly where we want them to be.

pause	% press any key to continue.

% This is quite a tricky simulation to do.  The input signal
% is in continuous time and the controller is in discrete time.
% OK, now do the simulation.  First we have to multiply K by
% -1 as negative feedback is assumed in the problem.

negK = -1*K;
[A,B,C,D] = ssdata(sat_dish);    % get the matrix data.
m7sd;

% We now do a sampled data simulation using simulink.  Note
% that we are interested in both the angle and the control
% actuator signal.

options = simset('InitialState',[0,0]);
options = simset(options,'OutputPoints','specified');
[tout,x,y] = sim('m7sd',[0,max(t)],options,[t,wind]);
thetaclp = y(:,1);
u = y(:,2);

clf
plot(tout,thetaclp,'-',t,theta,'--',tout,u,'-.')
title('Wind response')
legend('closed-loop angle','open-loop angle','control input: u')
xlabel('time: seconds')
ylabel('amplitude')

% Excellent closed-loop response!

% The input signal, u, actually looks a lot like the negative 
% of the wind.  The controller doesn't get to measure the
% wind directly - only its affect on the state.  However, the
% best control will more or less cancel out the effect of the
% wind.

pause	% press any key to continue.

% We can look at the control and wind signals in greater detail.
% Select only the first 10 seconds of each signal

t10idx = find(t <= 10);         % indices for t <= 10
to10idx = find(tout <= 10);     % indices for tout <= 10

clf
plot(tout(to10idx),u(to10idx),'+',t(t10idx),wind(t10idx),'-')
title('Control response')
legend('u','wind')
xlabel('time: seconds')
ylabel('amplitude')
grid

% Note that the control signal, u, seems to be a slightly
% delayed, and inverted version of wind.  The delay is about
% how long it takes for the effect of the wind to be seen at
% the output.  The sample period can also be seen in u.

%------------------------------------------------------------

echo off



