%NOISYMPC Basic Predictive Control with noise and distrubances.

(Script file)
%
% Implements unconstrained MPC for stable SISO discrete-time system
% defined as LTI object. Simulation of measurement noise, and of
% unmeasured input and output disturbances is included.
%
% The following are editable parameters (most have defaults):
% Tref: Time constant of exponential reference trajectory
% Ts: Sampling interval
% plant: Plant definition (discrete-time SISO LTI object)
% model: Model definition (discrete-time SISO LTI object)
% P: Vector of coincidence points
% M: Control horizon
% tend: Duration of simulation
% setpoint: Setpoint trajectory (column vector) - length must exceed
% no of steps in simulation by at least max(P).
% umpast, uppast, ympast, yppast: Initial conditions of plant & model.
% sd: Standard deviation of measurement noise.
% udist: Input disturbance trajectory (column vector)
% ydist: Output disturbance trajectory (column vector)
%
% Assumes Matlab 5.3. Uses Control Toolbox LTI object class.
% No other toolboxes required.

%% J.M.Maciejowski, 27 March 1999. Revised 15.12.99.

%% Copyright(C) 1999. All Rights Reserved.
%% Cambridge University Engineering Department.

%%%%%%%%%%%%%%%%% PROBLEM DEFINITION:

% Define time-constant of reference trajectory Tref:

Tref = 6;

% Define sampling interval Ts (default Tref/10):

if Tref == 0,
Ts = 1;
else
Ts = Tref/10;
end

% Define plant as SISO discrete-time 'lti' object 'plant'

%%%%% CHANGE FROM HERE TO DEFINE NEW PLANT %%%%%
nump=1;
denp=[1,-1.4,0.45];
plant = tf(nump,denp,Ts);
%%%%% CHANGE UP TO HERE TO DEFINE NEW PLANT %%%%%
plant = tf(plant); % Coerce to transfer function form
nump = get(plant,'num'); nump = nump{:}; % Get numerator polynomial
denp = get(plant,'den'); denp = denp{:}; % Get denominator polynomial
nnump = length(nump)-1; % Degree of plant numerator
ndenp = length(denp)-1; % Degree of plant denominator
if nump(1)~=0, error('Plant must be strictly proper'), end;
if any(abs(roots(denp))>1), disp('Warning: Unstable plant'), end

% Define model as SISO discrete-time 'lti' object 'model'

% (default model=plant):
%%%%% CHANGE FROM HERE TO DEFINE NEW MODEL %%%%%
model = plant;
%%%%% CHANGE UP TO HERE TO DEFINE NEW MODEL %%%%%
model = tf(model); % Coerce to transfer function form
numm = get(model,'num'); numm = numm{:}; % Get numerator polynomial
denm = get(model,'den'); denm = denm{:}; % Get denominator polynomial
nnumm = length(numm)-1; % Degree of model numerator
ndenm = length(denm)-1; % Degree of model denominator
if numm(1)~=0, error('Model must be strictly proper'), end;
if any(abs(roots(denm))>1), disp('Warning: Unstable model'), end

nump=[zeros(1,ndenp-nnump-1),nump]; % Pad numerator with leading zeros

numm=[zeros(1,ndenm-nnumm-1),numm]; % Pad numerator with leading zeros

% Define prediction horizon P (steps)(default corresponds to 0.8*Tref):

if Tref == 0,
P = 5;
else
P = round(0.8*Tref/Ts);
end

% Define control horizon (default 1):

M = 1;

% Compute model step response values over coincidence horizon:

stepresp = step(model,[0:Ts:max(P)*Ts]);
theta = zeros(length(P),M);
for j=1:length(P),
theta(j,:) = [stepresp(P(j):-1:max(P(j)-M+1,1))',zeros(1,M-P(j))];
end
S = stepresp(P);

% Compute reference error factor at coincidence points:

% (Exponential approach of reference trajectory to set-point)
if Tref == 0, % Immediate jump back to set-point trajectory
errfac = zeros(length(P),1);
else
errfac = exp(-P*Ts/Tref);
end

%%%%%%%%%%%%%%%% SIMULATION PARAMETERS:

if Tref == 0,
tend = 100*Ts;
else
tend = 10*Tref; % Duration of simulation (default 10*Tref)
end
nsteps = floor(tend/Ts); % (Number of steps in simulation).
tvec = (0:nsteps-1)'*Ts; % Column vector of time points (first one 0)

% Define set-point (column) vector (default constant 1):

setpoint = ones(nsteps+max(P),1);

% Define vectors to hold input and output signals, initialised to 0:

uu = zeros(nsteps,1); % Input
yp = zeros(nsteps,1); % Plant Output
ym = zeros(nsteps,1); % Model Output
ypd = zeros(nsteps,1);% Plant output with output disturbance
ypm = zeros(nsteps,1);% Measured plant output

% Initial conditions:
umpast = zeros(ndenm,1);
uppast = zeros(ndenp,1);
ympast = zeros(ndenm,1); % For model response
yppast = zeros(ndenp,1); % For plant response

% Noise and disturbances:

sd = 0.1; % Standard deviation of measurement noise
randn('state',0); % Resets state of random number generator. Change to
% get different random sequences generated.
noise = sd*randn(nsteps,1); % Measurement noise, normal distribution,
% mean=0, standard deviation = sd.
udist = 0.1*ones(nsteps,1); % Input disturbance (default constant 0.1)
ydist = 0.1*[ones(floor(nsteps/2),1);-ones(ceil(nsteps/2),1)];
% Output disturbance (default constant +/-0.1, changing sign halfway)

%%%%%%%%%%%%%%%% SIMULATION:

for k=1:nsteps,

% Add noise and disturbance to measurement of plant output:

ypd(k) = yp(k) + ydist(k); % Actual plant output
ypm(k) = ypd(k) + noise(k); % Measured plant output

% Define reference trajectory at coincidence points:

errornow = setpoint(k)-ypm(k);
reftraj = setpoint(k+P) - errornow*errfac;

% Free response of model over prediction horizon:

yfpast = ympast;
ufpast = umpast;
for kk=1:max(P), % Prediction horizon
ymfree(kk) = numm(2:nnumm+1)*ufpast-denm(2:ndenm+1)*yfpast;
yfpast=[ymfree(kk);yfpast(1:length(yfpast)-1)];
ufpast=[ufpast(1);ufpast(1:length(ufpast)-1)];
end

% Output disturbance estimate:

d = ypm(k) - ym(k);

% Compute input signal uu(k):

if k>1,
dutraj = theta\(reftraj-d-ymfree(P)');
uu(k) = dutraj(1) + uu(k-1);
else
dutraj = theta\(reftraj-d-ymfree(P)');
uu(k) = dutraj(1) + umpast(1);
end

% Simulate plant:
% Update past plant inputs
uppast = [uu(k)+udist(k);uppast(1:length(uppast)-1)];
yp(k+1) = -denp(2:ndenp+1)*yppast+nump(2:nnump+1)*uppast; % Simulation
% Update past plant outputs
yppast = [yp(k+1);yppast(1:length(yppast)-1)];

% Simulate model:
% Update past model inputs
umpast = [uu(k);umpast(1:length(umpast)-1)];
ym(k+1) = -denm(2:ndenm+1)*ympast+numm(2:nnumm+1)*umpast; % Simulation
 % Update past model outputs
ympast = [ym(k+1);ympast(1:length(ympast)-1)];

end % of simulation

%%%%%%%%%%%%%%%% PRESENTATION OF RESULTS:

disp('***** Results from script file NOISYMPC :')

disp(['Tref = ',num2str(Tref),', Ts = ',num2str(Ts),...
' P = ',int2str(P'),' (steps), M = ',int2str(M)])
diffpm = get(plant-model,'num');
if diffpm{:}==0,
disp('Model = Plant')
else
disp('Plant-Model mismatch')
end
disp(['Measurement noise SD = ',num2str(sd)]);
if udist ~= zeros(nsteps,1), disp('Input disturbance present'); end
if ydist ~= zeros(nsteps,1), disp('Output disturbance present'); end

figure
subplot(211)
% Plot output, solid line and set-point, dottedd line:
plot(tvec,ypd(1:nsteps),'-',tvec,ypm(1:nsteps),':',...
tvec,setpoint(1:nsteps),'--');
grid;
title(...
'Plant output (solid), Measured output (dotted) and set-point (dashed)')
xlabel('Time')

subplot(212)
% plot input signal as staircase graph:
stairs(tvec,uu,'-');
hold on;
stairs(tvec,uu+udist,':')
grid;
title('Controller output (solid), Plant input (dotted)')
xlabel('Time')