echo on
% This file demonstrates MATLAB's ability for LQG control system design by 
% going through the design of a controller for a boiler system.

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

pause % Press any key to continue ...

% Define boiler system
A = [ -3.93 -3.15e-3 0 0 0 4.03e-5 0 0 0
      3.68e2 -3.05 3.03 0 0 -3.77e-3 0 0 0
      2.74e1  7.87e-2 -5.96e-2 0 0 -2.81e-4 0 0 0
      -6.47e-2 -5.2e-5 0 -2.55e-1 -3.35e-6 3.6e-7 6.33e-5 1.94e-4 0
      3.85e3 1.73e1 -1.28e1 -1.26e4 -2.91 -1.05e-1 1.27e1 4.31e1 0
      2.24e4 1.8e1 0 -3.56e1 -1.04e-4 -4.14e-1 9e1 5.69e1 0
      0 0 2.34e-3 0 0 2.22e-4 -2.03e-1 0 0
      0 0 0 -1.27 1e-3 7.86e-5 0 -7.17e-2 0
      -2.2 -1.77e-3 0 -8.44 -1.11e-4 1.38e-5 1.49e-3 6.02e-3 -1e-10];
B = [ 0 0 0 -1.0e-2
      0 0 0 0
      1.56 0 0 0
      0 -5.13e-6 0 0
      8.28 -1.5 3.95e-2 5.2e1
      0 1.78 0 0
      2.33 0 0 0
      0 -2.45e-2 2.84e-3 0
      0 2.94e-5 0 0];
C = [0 0 0 0 0 1 0 0 0 
     0 0 0 0 0 0 0 0 1];
D = [0 0 0 0
     0 0 0 0];

states='x1 x2 x3 x4 x5 x6 x7 x8 x9';
inputs='fuel-flow water-flow water-temp steam-flow';
outputs='water-level pressure';
printsys(A,B,C,D,inputs,outputs,states);

pause % Press any key to continue ...

% This is the state space model for a boiler system consisting of a superheater
% and a riser in cascade.  It is desired to regulate the water-level and
% pressure to desired set points.  The control inputs are fuel-flow,water-flow
% and water-temp.  The steam-flow is a disturbance.  This system has one very
% slow pole at -1.e-10 and is non-minimum phase.

% Let's look at some step responses to get a feel for how the system behaves.
t = 0:5:500;		% Define time vector (automatic vector too long here)
%subplot(221), step(A,B,C(1,:),D(1,:),1,t); title('Input 1 Output 1')
%subplot(222), step(A,B,C(2,:),D(2,:),1,t); title('Input 1 Output 2')
%subplot(223), step(A,B,C(1,:),D(1,:),2,t); title('Input 2 Output 1')
%subplot(224), step(A,B,C(2,:),D(2,:),2,t); title('Input 2 Output 2'), pause
%clg
%subplot(221), step(A,B,C(1,:),D(1,:),3,t); title('Input 3 Output 1')
%subplot(222), step(A,B,C(2,:),D(2,:),3,t); title('Input 3 Output 2')
%subplot(223), step(A,B,C(1,:),D(1,:),4,t); title('Input 4 Output 1')
%subplot(224), step(A,B,C(2,:),D(2,:),4,t); title('Input 4 Output 2')
%pause
echo off
subplot(221), step(A,B,C(1,:),D(1,:),1,t); title('Input 1 Output 1')
subplot(222), step(A,B,C(2,:),D(2,:),1,t); title('Input 1 Output 2')
subplot(223), step(A,B,C(1,:),D(1,:),2,t); title('Input 2 Output 1')
subplot(224), step(A,B,C(2,:),D(2,:),2,t); title('Input 2 Output 2'), pause
clg
subplot(221), step(A,B,C(1,:),D(1,:),3,t); title('Input 3 Output 1')
subplot(222), step(A,B,C(2,:),D(2,:),3,t); title('Input 3 Output 2')
subplot(223), step(A,B,C(1,:),D(1,:),4,t); title('Input 4 Output 1')
subplot(224), step(A,B,C(2,:),D(2,:),4,t); title('Input 4 Output 2')
pause
echo on

% The time responses show the effect of the slow pole.  The water-level 
% (output 1) responds in 500 seconds but the pressure shows ramp-like behavior
% over this time frame.

pause % Press any key to continue ...

% Since we plan to use LQG control methods, let's look at the singular value
% responses for this system to determine the dynamic controllability.
subplot(111)
sigma(A,B,C,D), pause % Press any key after plot

% The frequency responses show a lot of gain at low frequencies so that
% achieving acceptable tracking of input commands should be easy. 

% To start lets design a full state feedback controller.  MATLAB contains two
% commands to do full state feedback controller design, LQR and LQRY.
% They differ only in the the specific cost function used.  LQR uses a cost
% function that weights the states and LQRY uses a cost function that weights
% the outputs.  We will use LQRY below.  This means that we must form a set
% of criteria outputs to be weighted in the cost function.  We will use 
% the plant outputs as criteria outputs.

pause % Press any key to continue ...

% First extract a plant containing the control inputs and plant outputs.
[a,b,c,d] = ssselect(A,B,C,D,[1:3],[1:2]);

% Perform the LQR gain matrix calculation.  Use an "optimal root locus" to 
% chose the "best" relative weighting between the Q and R matrices.  Note
% the weights below were chosen after a few design iterations.
Q = [.001 0
     0   10];	% Output weighting matrix
R = [1 0 0
     0 .2 0
     0 0  1];	% Input weighting matrix

[nx,na] = size(a);
E = zeros(nx,11);

for i = 1:10;
  rho = i*.1;
  [K,S,E(:,i)] = lqry(a,b,c,d,rho*Q,R);
end
plot(real(E),imag(E),'x'), title('Optimal Root Locus'), pause % Press any key

% Using the last design, the closed loop eigenvalues are
damp(esort(E(:,10)));

pause % Press any key to continue ...

% Now analyze the design.  To do this we must form the closed loop system.
%      --->O---[Plant]--+--->
%          |            | x
%          +-----[K]----+
% 
% So append the states as outputs, and do a feedback connection. 
[ac,bc,cc,dc] = augstate(a,b,c,d);
inputs = [1:3]; outputs=[3:11];	% Define selection vectors
[ac,bc,cc,dc] = feedback(ac,bc,cc,dc,[],[],[],K,-inputs,outputs);
[ac,bc,cc,dc] = ssdelete(ac,bc,cc,dc,[],outputs); % Remove state outputs

% Look at the closed-loop time response.
%subplot(221), step(ac,bc,cc(1,:),dc(1,:),1,t); title('Input 1 Output 1')
%subplot(222), step(ac,bc,cc(2,:),dc(2,:),1,t); title('Input 1 Output 2')
%subplot(223), step(ac,bc,cc(1,:),dc(1,:),2,t); title('Input 2 Output 1')
%subplot(224), step(ac,bc,cc(2,:),dc(2,:),2,t); title('Input 2 Output 2'), pause
echo off
subplot(221), step(ac,bc,cc(1,:),dc(1,:),1,t); title('Input 1 Output 1')
subplot(222), step(ac,bc,cc(2,:),dc(2,:),1,t); title('Input 1 Output 2')
subplot(223), step(ac,bc,cc(1,:),dc(1,:),2,t); title('Input 2 Output 1')
subplot(224), step(ac,bc,cc(2,:),dc(2,:),2,t); title('Input 2 Output 2'), pause
echo on

% The time responses show that the water-level is controlled quickly, while
% the pressure takes about 11 min (660 secs) to achieve steady state.

pause % Press any key to continue ...

% Let's plot the step response for the first 10 secs to see how the 
% water-level behaves.
tq = 0:.1:10;
clg
%subplot(221), step(ac,bc,cc(1,:),dc(1,:),1,tq); title('Input 1 Output 1')
%subplot(222), step(ac,bc,cc(2,:),dc(2,:),1,tq); title('Input 1 Output 2')
%subplot(223), step(ac,bc,cc(1,:),dc(1,:),2,tq); title('Input 2 Output 1')
%subplot(224), step(ac,bc,cc(2,:),dc(2,:),2,tq); title('Input 2 Output 2')
%pause % Press any key after plot ...
echo off
subplot(221), step(ac,bc,cc(1,:),dc(1,:),1,tq); title('Input 1 Output 1')
subplot(222), step(ac,bc,cc(2,:),dc(2,:),1,tq); title('Input 1 Output 2')
subplot(223), step(ac,bc,cc(1,:),dc(1,:),2,tq); title('Input 2 Output 1')
subplot(224), step(ac,bc,cc(2,:),dc(2,:),2,tq); title('Input 2 Output 2')
pause % Press any key to continue ...
echo on

% The behavior looks good.  

% To recap, we've designed a state feedback gain matrix that requires all 
% the states to be fedback.  However, we only have the two outputs so it will
% be necessary to design a state estimator for our controller.  MATLAB
% contains two commands to do this design, LQE and LQEW.  They only differ in
% that LQEW allows direct feedthrough of process noise into the sensors.
% Since our D matrix is zero we will use LQE.

% For estimator design we need to identify the sensor outputs and process 
% noise inputs.  The sensors will be our two outputs, and we will use the
% control inputs as well as the steam flow (output 4) as process noise inputs.
% The control inputs are included as process noise inputs since to do so helps
% to make the system robust to parameter changes at the control inputs.

pause % Press any key to continue ...

% Estimator design model
a = A; b = B; c = C; d = D;

% Perform the LQE gain matrix calculation.  Use an "optimal root locus" to 
% chose the "best" relative weighting between the Q and R matrices.

% Define process noise and sensor noise covariance.
Q = [.1 0 0 0
     0 100 0 0
     0 0 1 0
     0 0 0 100];	% Process noise covariance
R = [1000 0
     0 1];	% Sensor noise covariance

[nx,na] = size(a);
E = zeros(nx,11);

for i = 1:10;
  rho = i*.1;
  [L,P,E(:,i)] = lqe(a,b,c,rho*Q,R);
end
subplot(111)
plot(real(E),imag(E),'x'), title('Optimal Root Locus'), pause % Press any key

% Using the last design, the closed loop eigenvalues are
damp(esort(E(:,10)));

pause % Press any key to continue ...

% Now form the controller so that we can form the closed-loop system.
sensors= [1:2];  controls=[1:3];
[ak,bk,ck,dk] = reg(A,B,C,D,K,L,sensors,[],controls);

% Form the closed loop system so that the inputs are output commands, yc:
%  yc --->O--[Controller]---[Plant]--+---> y
%         |                          |
%         +--------------------------+
% Use a combination of SERIES and CLOOP.
[ac,bc,cc,dc] = series(ak,bk,ck,dk,A,B,C,D,[1:3],controls);
[ac,bc,cc,dc] = cloop(ac,bc,cc,dc);

pause % Press any key to continue ...

% Closed loop eigenvalues.  Should be a combination of the closed-loop feedback
% and estimator eigenvalues as guaranteed by the separation theorem.
damp(ac)

pause % Press any key to continue ...

% Now plot the step response to water-level-command and pressure-command
% (inputs 1 and 2 of the closed-loop system)
step(ac,bc,cc,dc,1); title('Water-Level-Command'), pause % press any key ...

% The plot shows that the water is controlled quickly (in about 5 seconds)
% and that after about 20 seconds the steady state error is small.  In addition
% the pressure response is very small and is thus decoupled from this input.

pause % Press any key to continue ...

% Plot the response to pressure command
step(ac,bc,cc,dc,2); title('Pressure-Command'), pause % Press any key ...

% This plot shows that the pressure achieves its steady state value in
% about 50 minutes (3000 seconds) and that the water level is adversely
% affected during this transient.  One way to remove this transient is to
% use integral control on the water-level.  The integral control would 
% help to decouple the pressure response from the water-level response.

pause % Press any key to continue ...

% Lets look at how a constant steam-flow disturbance can affect our steady 
% state tracking.  Form a new closed loop system with the plant inputs as
% inputs.   --->O----[Plant]---+--->
%               |              |
%               +-[Controller]-+
[ac,bc,cc,dc] = feedback(A,B,C,D,ak,bk,ck,dk,-[1:3],[1:2]);

% Do step response from steam-flow (input 4)
step(ac,bc,cc,dc,4); title('Steam-flow Disturbance'), pause % Press any key

% So the steam-flow affects the water-level substantially.  Again an integrator
% on the water-level would provide better disturbance rejection.  

pause % Press any key to continue ...

% Although we will not redo the design, to include the integrator in the 
% design we would:

% 1) Add integrator to the plant model by doing a series connection with the
%    plant and the integrator:
%    [ai,bi,ci,di] = zp2ss([],[0],[1]); % Integrator model

% 2) form error output by adding command input:
%    B = [B,zeros(9,1)]; D = [D,[-1;0]];

% 3) Connect integrator in series using APPEND and CLOOP since SERIES removes
% the outputs of system1 and the inputs of system2 from the resulting model.
% The integral output will now be output 3.
%   [a,b,c,d] = append(A,B,C,D,ai,bi,ci,di);
%   [a,b,c,d] = cloop(a,b,c,d,[1],[5]);

% 4) Design the feedback gain matrix as above but also weight the integrator.

pause % Press any key to continue ...

% 5) Design the estimator exactly as above without the integrator.  The
% estimator formed will be a reduced order estimator since the integrator 
% state will not be estimated.  

% 6) So before forming the controller add an extra row of zeros to the Kalman 
% gain matrix for the integral state.  

% 7) Finally when forming the controller use the plant containing the integral
% and specify the integral command (input 4) as a known input.

echo off

