% Copyright (c) 1994-98 by The MathWorks, Inc.
% $Revision: 1.4 $

echo on
%
%  Purpose: 	Demonstrate the use of controlier design and simulation
%            routines for step response models.
%            The plant considered is SISO with a SISO distrubance plant.
%
%	 Plant transfer function:  g = 5.72exp(-14s)/(60s+1)
%	 Disturbance transfer function:  gd = 1.52exp(-15s)/(25s+1)
%
%  Build the step response models for as sampling period of 7.

delt1=0;
delay1=14;
num1=5.72;
den1=[60 1];
g = poly2tfd(num1,den1,delt1,delay1);
tfinal = 245;
delt2=20;
nout1 = 1;
plant = tfd2step(tfinal,delt2,nout1,g);
pause
%
delay2=15;
num2=1.52;
den2=[25 1];
gd = poly2tfd(num2,den2,delt1,delay2);
delt2=20;
nout2 = 1;
dplant = tfd2step(tfinal,delt2,nout2,gd);

%
%  Calculate the MPC controller gain matrix for
%    No plant/model mismatch,
%    Output Weight = 1, Input Weight = 0
%    Input Horizon = 5, Output Horizon = 20
model = plant;
ywt = 1; uwt = 0;
M = 5; P = 20;
Kmpc1 = mpccon(model,ywt,uwt,M,P);
pause
%
%  Simulate and plot response for unmeasured and measured
%  step disturbance through dplant.
tend = 245;
r = []; usat = []; tfilter = [];
dmodel = [];
dstep = 1;
[y1,u1] = mpcsim(plant,model,Kmpc1,tend,r,usat,tfilter,dplant,dmodel,dstep);
dmodel = dplant; % measured disturbance
[y2,u2] = mpcsim(plant,model,Kmpc1,tend,r,usat,tfilter,dplant,dmodel,dstep);
plotall([y1,y2],[u1,u2],delt2);
pause;
%
%  Calculate the a new MPC controller gain matrix for
%    No plant/model mismatch,
%    Output Weight = 1, Input Weight = 10
%    Input Horizon = 5, Output Horizon = 20
model = plant;
ywt = 1; uwt = 10;
M = 5; P = 20;
Kmpc2 = mpccon(model,ywt,uwt,M,P);
%
%  Simulate and plot response for unmeasured and measured
%  step disturbance through dplant.
tend = 245;
r = []; usat = []; tfilter = [];
dmodel = [];
dstep = 1;
[y3,u3] = mpcsim(plant,model,Kmpc2,tend,r,usat,tfilter,dplant,dmodel,dstep);
dmodel = dplant; % measured disturbance
[y4,u4] = mpcsim(plant,model,Kmpc2,tend,r,usat,tfilter,dplant,dmodel,dstep);
plotall([y3,y4],[u3,u4],delt2);
pause;
%
%  Simulate and plot response for unmeasured
%  step disturbance through dplant with uwt = 0,
%  with and without noise filtering.
tend = 245;
r = []; usat = []; dmodel = [];
tfilter = [];
dstep = 1;
[y5,u5] = mpcsim(plant,model,Kmpc1,tend,r,usat,tfilter,dplant,dmodel,dstep);
tfilter = 20;  % noise filtering time constant = 20
[y6,u6] = mpcsim(plant,model,Kmpc1,tend,r,usat,tfilter,dplant,dmodel,dstep);
plotall([y5,y6],[u5,u6],delt2);
pause;
%
%  Simulate and plot response for unmeasured
%  step disturbance through dplant with uwt = 0,
%  with and without unmeasured disturbance time
%  being specified.
tend = 245;
r = []; usat = []; dmodel = [];
tfilter = [];
dstep = 1;
[y7,u7] = mpcsim(plant,model,Kmpc1,tend,r,usat,tfilter,dplant,dmodel,dstep);
tfilter = [0 ; 25]; % unmesured disturbance time constant = 25
[y8,u8] = mpcsim(plant,model,Kmpc1,tend,r,usat,tfilter,dplant,dmodel,dstep);
plotall([y7,y8],[u7,u8],delt2);
pause;
%
%  Construct a closed-loop system for no disturbances
%  and uwt = 0. Determine the poles of the system.
clmod = mpccl(plant,model,Kmpc1);
poles = smpcpole(clmod);
maxpole = max(poles)
pack;
%
%  Calculate and plot the frequency response of the
%  sensitivity and complementary sensitivity functions.
freq = [-3,0,30];
ny = 1;
out = [1:ny]; % output is yp for comp. sensitivity
in = [1:ny]; % input is r for comp. sensitivity
[frsp,eyefrsp] = mod2frsp(clmod,freq,out,in);
plotfrsp(eyefrsp); % sensitivity
pause;
plotfrsp(frsp); % complementary sensitivity
%
%  Simulate and plot response for unmeasured step
%  disturbance through dplant with and without
%  input constraints.
%    No plant/model mismatch,
%    Output Weight = 1, Input Weight = 0
%    Input Horizon = 5, Output Horizon = 20
%    Minimum Constraint on Input = -0.4
%    Maximum Constraint on Input = inf
%    Delta Constraint on Input = 0.1
model = plant;
ywt = 1; uwt = 0;
M = 5; P = 20;
tend = 245;
r = [];
ulim =[];
ylim = []; tfilter = []; dmodel = [];
dstep = 1;
[y9,u9] = cmpc(plant,model,ywt,uwt,M,P,tend,r,...
               ulim,ylim,tfilter,dplant,dmodel,dstep);
ulim = [-0.4, inf, 0.1]; % impose constraints
[y10,u10] = cmpc(plant,model,ywt,uwt,M,P,tend,r,...
               ulim,ylim,tfilter,dplant,dmodel,dstep);
plotall([y9,y10],[u9,u10],delt2);
pause;
echo off
