clc
echo on
hold off
% **************************************************************************
% Example 9.2.1 sompc  control of a SISO system
% copyright 1992 by Babu Joseph
% Washington Universiy St.Louis
%****************************************************************************
%
%  In this demo program we will implement single objective model predictive
% control of a siso system
%
% A model of the process is entered in the form of a transfer function
% We then generate a step response model of the process with a model
% error in the form a change in process gain expressed as a percentage
% A control vector is generated by the least square method
% Provision is made for incorporating a move suppression factor
% Next a test is conducted by simulating the process with this controller
% implemented
% A set point change is made and the resulting control moves and process
% output are printed on the screen and then plotted
%
% You can change the model to study other processes
%
pause % Press any key
clc
%
% Enter  process model
kp=1.0   ;
tp= 1.0 ;
gpnum=[-54.68 4.05];
gpden=[ 675 63.5 1 ];
%gpnum=[1];gpden=[600 30 1];
%
%
printsys(gpnum,gpden);
% Now let us generate a step response to see the process characteristics
% we can use this to judge the sample size and horizon length needed
pause % press any key
step(gpnum,gpden);pause
clc
%
% Enter sample time
% From the step response a value of 5 is suggested
% 
ts=5
% ts=input(' Enter sample time=');ts
clc
% Next a state space model is obtained for simulation purposes
%
[a1,b1,c1,d1]=tf2ss(gpnum,gpden);
[ad,bd,cd,dd]=c2dm(a1,b1,c1,d1,ts);
% Enter the output horizon length, p
%  From the step response a value of 30
% is suggested
p=30
p=input('enter output horizon length=') ;p
clc
% Enter the no of steps needed for simulation
% A value of 40 should allow  the process to reach a steady state
%
nt=60
nt=input('enter the no of time steps for simulation results');nt
clc
%
% Enter the no of future control moves included in the calculation
% a value between 1 should be satisfactory. Increasing n increases
% the instability
n=2
n=input('enter control move horizon length=') ;n
clc
% Next enter the model error in the gain of the process
% This is entered as a percentage of the steady state gain of the process
% A value between -50% to +50 is suggested. A value of 0 means no error
% in the model
kerr=0
kerr=input(' Enter % error in process gain=');kerr
%
clc
t=0:ts:(p+1)*ts; % this generates a time vector for simulation
pause % press any key
clc;
% The next task is to generate a step response model of  the process
% This is done using the parameters you just entered
%
% generate step response
[y,x,t]=step(gpnum,gpden,t);
a=y(2:p+1,:) .* (1+kerr/100);
% Generate dynamic matrix
echo off;
A=zeros(p,n);
for i=1:p
   for j=1:i
     if( j <= i  & j <= n)
         A(i,j)=a(i-j+1);
     end;
   end;
end;
% the dynamic matrix is given by
A
pause % press any key
clc
echo on;
% the next task is to generate the control vector
% Enter move suppression factor
% a value between 0-1 is suggested
beta=1
beta=input('enter move suppression factor=');beta
B=eye(n,n) .*(beta*beta);
%
% the control matrix is computed using the least squares solution
%
C=(A'*A +B)\A';
% retrieve the first row of C
C1=C(1,:) ;
% the control row vector is given by
C1'
pause; % press any key
clc
% the next task is to initilize various quantities so we can
% implement and test the sompc algorithm
%
% initialize yp,yr,ym,d
yp=zeros(p,1);   % this is vector of predicted values of y
yr=ones(p,1) ;   % this is the set point vector
u=[0 0]';        % this is two element vector of input at current time and
%                  the next sample time . It is used for the simulation of the
%                  process to   generate the measurement at next sample time
%
ym=0;            % this is the measurement of output
d=zeros(p,1);    % this is the disturbance estimates for the horizon
[na,ma]=size(ad);% this retrieves the size of the state vector
x0=zeros(na,1);  % this initilizes the state vector for simulation purposes
%                  the next few lines initialize arrays for storing results
ypl=zeros(nt,1); % This vector stores the output values
upl=zeros(nt,1); % This vector stores the manipulated variables
tpl=zeros(nt,1); % This vector stores the time values
%
pause; % Press any key
clc
echo off

% Start mpc algorithm
for ii=1:nt-1
d(1)=ym-yp(1);  % Estimate the current disturbance
d1=ym-yp(1);
d=ones(p,1) .*d1;
b=yr-d-yp;
du1=C1*b;
if ii == 1 clc; disp( '       time         ym       yp        dist      du'); end
fprintf(' %10.1f  %10.2f  %10.2f',tpl(ii),ym,yp(1));
fprintf('%10.2f  %10.2f \n',d1,du1);
t=[0. ts]; % This sets up simulation from now until next sample time
u=u + [du1 du1 ]';  % This is the control input to be applied two values
                    % are necessary value at beginning and value at end
[y,x]=dlsim(ad,bd,cd,dd,u,x0);
% save results for plotting later
ypl(ii+1)=y(2);
upl(ii+1)=u(1);
tpl(ii+1)=tpl(ii)+ts;
% retrieve new x0 and ym  ; % this is required for the simulation of the
%                             time interval
ym=y(2);  % This is the new value of the output
x0=x(2,:);
%update yp and yr
dyp=[0 a'] .*du1; % This computes the effect of du1 on output y as predicted
%                   by the model. The zero is needed to incorporate change in y
%                    at present time
dyp=dyp(:,1:p)';   % We retain only p values
yp=yp+dyp+d ;
%now we must discard yp(1) and add y(p)
yp=yp(2:p);
yp(p)=yp(p-1);
yr=yr(2:p);
yr(p)=yr(p-1);
end ;
echo on
plot(tpl,ypl,'-',tpl,upl,'.') ; pause;
% The key points to note are how fast and how smoothly does the output
% reach the steady state? How large a move size is required to achieve this
% this result? Look at the graph again to see the results
pause % press any key
shg; pause
% this is the end of the demo
%