% smithreg2002.m
% PURPOSE
% Simulate a process with smith predictor.
%

clear all
% PROCESS AND MODEL PARAMETERS
T1=1; T2=0.5; k=0.5;
num=[0,0,k];                 % teller i h_p(s)
den=[T1*T2,T1+T2,1];           % nevner i h_p(s)
[A,B,D,E]=tf2ss([0,0,0.5],[T1*T2,T1+T2,1]); % Tilstandsrommodell.
dA=0; dB=0; C=[0;0];

n=2;
nt=20;                         % time delay equal to (nt) samples
%
% INITIALIZE SOME PARAMETERS
N=600;                        % number of samples to simulate
y0=[zeros(20,1);ones(580,1)]; % array of set-points (y0=r).
ey=zeros(N,4);                % array to hold results
Delay=zeros(nt); eed=Delay;   % arrays to make time delay, hold (nt) samples
%
y=0; z=0; x=zeros(n,1); v=0; Y=0;      % initial values, see simulation loop
ye=0; xe=x; Ye=0;             % initial values for estimator variables
%
Kp=0.56; Ti=1.25;             % PI-controller parameters 
dt=0.1; t=0;                  % sampling- and start-time

ireg=1;                       % ireg=0 to turn off the controller
%
% THE MAIN SIMULATION LOOP
for i=1:N;
%
e=y0(i)-ye-(Y-Ye);            % the deviation which is feed into the Pi-contr.
%
if i>200; v=0.5; end          % step in the disturbance at time i=201 sample 
if ireg==1                    % PI-controller
 u=Kp*e+Kp*z/Ti;
 z=z+dt*e;
else
 u=y0(i);
end

y=D*x+E*u;                    % process measurement
x=x+dt*(A*x+B*u+C*v);         % uppdate process state equation

% programming of time delay for process model, 
% hold (y) in array of length (nt) samples
for id=nt:-1:2;
   Delay(id)=Delay(id-1);
end
Delay(1)=y;
Y=Delay(nt);                  % the delayed process output Y=y(i-nt)

% estimator
ye=D*xe+E*u;
xe=xe+dt*(A*xe+B*u+C*v*0);
% time delay for estimator model
for id=nt:-1:2;
   eed(id)=eed(id-1);
end
eed(1)=ye;
Ye=eed(nt);                   % the delayed estimator output (estimate of y)
%
% STORE VARIABLES
ey(i,1)=Y;                    % process output 
ey(i,2)=e;                    % "control" deviation
ey(i,3)=v;                    % process deviation
ey(i,4)=u;
et(i,1)=t;                    % continuous time
%
t=t+dt;                       % uppdating continuous time
end

if ireg == 1
plot(et,[y0,ey(:,1),ey(:,3),ey(:,4)]) % plot y0, y and v
xlabel('[time]')
title('SMITH: process output after step in set-point and disturbance')
grid
else
plot(et,[y0,ey(:,1),ey(:,3)]) % plot y0, y and v
xlabel('[time]')
title('SMITH: process output after step in input and disturbance')
grid
end