www.gusucode.com > mpcobsolete 工具箱 matlab 源码程序 > mpcobsolete/mpcsim.m
function [y,u,ym,KF] = mpcsim(plant, model, controller,...
tfinal, setpoint, uconstraint, tfilter,...
distplant, distmodel, diststep)
%MPCSIM Simulation of the unconstrained Model Predictive Controller.
% [y,u,ym] = mpcsim(plant, model, Kmpc, tend, r,usat, tfilter,
% dplant, dmodel, dstep)
% REQUIRED INPUTS:
% plant(model): the step response coefficient matrix of the plant (model)
% generated by the function tfd2step.
% Kmpc: the constant control law matrix computed by the function mpccon
% (closed-loop simulations).For open-loop simulation, controller=[].
% tend: final time of simulation.
% r: for the closed-loop simulation, it is a constant or time-varying
% reference trajectory. For the open-loop simulation, it is the
% trajectory of the manipulated variable u.
% OPTIONAL INPUTS:
% usat: the matrix of manipulated variable constraints.It is a constant
% or time-varying trajectory of the lower limits (Ulow), upper limits
% (Uhigh) and rate of change limits (DelU) on the manipulated
% variables. Default=[].
% tfilter: time constants for noise filter and unmeasured disturbance lags.
% Default is no filtering and step disturbance.
% dplant: step response coefficient matrix for the disturbance effect on the
% plant output generated by the function tfd2step. If distplant is
% provided, dstep is also required. Default = [].
% dmodel: step response coefficient matrix for the measured disturbance
% effect on the model output generated by the function tfd2step.
% If distmodel is provided, dstep is also required. Default=[].
% dstep: matrix of disturbances to the plant. For output step disturbances
% it is a constant or time-varying trajectory of disturbance values
% For disturbances through step response models,it is a constant or
% time-varying trajectory of disturbance model inputs.Default=[].
% OUTPUT ARGUMENTS: y (system response), u (manipulated variable) and
% ym (model response)
% See also PLOTALL, PLOTEACH, CMPC, MPCCL, MPCCON.
% Copyright 1994-2003 The MathWorks, Inc.
if (nargin==0)
disp('usage: [y,u,ym] = mpcsim(plant,model,Kmpc,tend,r,usat,tfilter,...');
disp(' dplant, dmodel, dstep)');
return;
end;
casenum = 1;
[npny,nu] = size(plant);
[nmny,num] = size(model);
ny = plant(npny-1,1); ny1 = model(nmny-1,1);
delt = plant(npny,1); deltm = model(nmny,1);
if isempty(setpoint)
setpoint=zeros(1,ny);
end;
[ls,ws] = size(setpoint);
nfinal = fix(tfinal/delt);
if (ny1~=ny)
error('Check the dimensions of PLANT & MODEL.');
end;
if (num~=nu)
error('Check the dimensions of PLANT & MODEL.');
end;
if (deltm~=delt)
error('The sampling time must be the same in PLANT & MODEL.');
end;
if (nargin<5)
error('At least five input arguments are required');
end;
if (nargin>10)
disp('Too many input arguments ... accepting only the first ten of them.');
end;
npny = npny-2-ny; nmny = nmny-2-ny;
np = npny/ny; nm = nmny/ny;
noutp = plant(npny+1:npny+ny,1); noutm = model(nmny+1:nmny+ny,1);
intp = ones(ny,1)-noutp; intm = ones(ny,1)-noutm;
noutdp = ones(ny,1); noutdm = ones(ny,1);
if (any(intp)==1 | any(intm)==1)
casenum = 2;
end;
%
% check the setpoint argument
%
if (isempty(controller)) % Loop controller status
% for open-loop simulation
if (ws~=nu)
error('Check the dimension of SETPOINT.');
end;
else
% for closed-loop simulation
[nuc,pny] = size(controller);
p = (pny/ny);
if (nuc~=nu)
error('Check the dimension of CONTROLLER.');
end;
if (ws~=ny)
error('Check the dimension of SETPOINT.');
end;
if (ls<p)
for is = ls+1:p
setpoint(is,:) = setpoint(ls,:);
end;
end;
%
% if [p(horizon)+1] > nm (the truncation order of the model),
% make nm = p+1, and add the last step response coefficients
% (p+1-nm) times to the MODEL
%
if ((p+1)>nm)
k = nmny; kx = nmny-ny+1;
int = intm*ones(1,nu);
for ib = nm+1:p+1
model(k+1:k+ny,:) = model(kx:k,:)+int.*(model(kx:k,:)...
-model(kx-ny:kx-1,:));
k = k+ny; kx = kx+ny;
end;
nmny = k; nm = p+1;
end;
end; % Loop controller status
if (nargin>5)
if (nargin==6)
tfilter = [];
end;
if (nargin==8 | nargin==9)
error('A term multiplying the disturbance plant must be specified');
end;
if (nargin>9) % Loop nargin>9
%
% check dimensions of DISTPLANT and D
%
if (isempty(distplant))
distmodel=[];
end;
if (isempty(distplant)==1 & isempty(diststep)==0) % Loop dist status
[ld,wd] = size(diststep);
if (wd~=ny)
error('For the chosen configuration the diststep is the wrong dimension');
end;
%
% difference the disturbance
%
diststep(1:ld+1,:)=...
[diststep;diststep(ld,:)]-[zeros(1,wd);diststep];
elseif (isempty(distplant)==1 & isempty(diststep)==1)
disp('No modeled disturbances');
elseif (isempty(distplant)==0 & isempty(diststep)==0)
%
% for DISTSTEP into disturbance model, make the truncation order of Su
% and distplant equal.
%
% for the plant
%
% difference disturbance
[ld,wd] = size(diststep);
diststep(1:ld+1,:)=...
[diststep;diststep(ld,:)]-[zeros(1,wd);diststep];
[ndp1,ndp2] = size(distplant);
ny1 = distplant(ndp1-1,1);
deltdp = distplant(ndp1,1);
if (ny1~=ny)
disp('The number of disturbance plant (DISTPLANT) outputs');
disp('and system (PLANT) outputs must be the same.');
return;
end;
if (ndp2~=wd)
disp('The number of disturbance plant (DISTPLANT) inputs');
disp('and disturbance (DISTSTEP) columns must be the same.');
return;
end;
if (deltdp~=delt)
error('The sampling time must be the same in PLANT & DISTPLANT.');
end;
ndp1 = ndp1-2-ny;
ndp = ndp1/ny;
noutdp = distplant(ndp1+1:ndp1+ny,1);
intdp = ones(ny,1)-noutdp;
if (any(intdp)==1)
casenum = 2;
end;
if (ndp>np)
k = npny; kx = npny-ny+1;
int = intp*ones(1,nu);
for ib = np+1:ndp
plant(k+1:k+ny,:) = plant(kx:k,:)+int.*(plant(kx:k,:)...
-plant(kx-ny:kx-1,:));
kx=kx+ny;
k = k+ny;
end;
npny = k; np = ndp;
elseif (ndp<np)
k = ndp1; kx = ndp1-ny+1;
int = intdp*ones(1,ndp2);
for ib = ndp+1:np
distplant(k+1:k+ny,:) = distplant(kx:k,:)+int.*(distplant...
(kx:k,:)-distplant(kx-ny:kx-1,:));
k = k + ny;
kx = kx + ny;
end;
end;
% for the model
if (isempty(distmodel)==0)
[ndm1,ndm2] = size(distmodel);
ny1 = distmodel(ndm1-1,1);
deltdm = distmodel(ndm1,1);
if (ny1~=ny)
disp(' The number of disturbance model (DISTMODEL) outputs');
disp(' and system (PLANT) outputs must be the same.');
return;
end;
if (ndm2~=wd)
disp(' The number of disturbance model (DISTMODEL) inputs');
disp(' and disturbance (DISTSTEP) columns must be the same.');
return;
end;
if (deltdm~=delt)
disp(' The sampling time must be the same in MODEL & DISTMODEL.');
return;
end;
ndm1 = ndm1-2-ny;
ndm = ndm1/ny;
noutdm = distmodel(ndm1+1:ndm1+ny,1);
intdm = ones(ny,1)-noutdm;
if (any(intdm)==1)
casenum = 2;
end;
if (ndm>nm)
k = nmny; kx = nmny-ny+1;
int = intm*ones(1,nu);
for ib=nm+1:ndm
model(k+1:k+ny,:) = model(kx:k,:)+int.*(model(kx:k,:)...
-model(kx-ny:kx-1,:));
k = k+ny;
kx = kx+ny;
end;
nmny = k; nm = ndm;
elseif (ndm<nm)
k = ndm1; kx = ndm1-ny+1;
int = intdm*ones(1,ndm2);
for ib=ndm+1:nm,
distmodel(k+1:k+ny,:) = distmodel(kx:k,:)+int.*...
(distmodel(kx:k,:)-distmodel(kx-ny:kx-1,:));
k = k+ny;
kx = kx+ny;
end;
end;
end;
end; % Loop dist status
else % Loop nargin>9
distplant = [];
distmodel = [];
diststep = [];
end; % Loop nargin>9
else % Loop nargin>5
uconstraint = [];
tfilter = [];
distplant = [];
distmodel = [];
diststep = [];
end; % Loop nargin>5
% interpret uconstraints
if (isempty(uconstraint)==0)
[nuconstraint,ncol]=size(uconstraint);
if ncol ~= 3*nu | nuconstraint <= 0
error('UCONSTRAINT matrix is wrong size.');
end;
if any(any(uconstraint(:,2*nu+1:3*nu) < 0))
error('A constraint on delta u was < 0');
elseif any(any(uconstraint(:,nu+1:2*nu)-uconstraint(:,1:nu) < 0))
error('A lower bound on u was greater than corresponding upper bound.');
end;
for ir = 1:nuconstraint
for iu = 1:nu
if (uconstraint(ir,iu)==-inf)
uconstraint(ir,iu) = -1.0E10;
end;
end;
for iu = nu+1:3*nu
if (uconstraint(ir,iu)==inf)
uconstraint(ir,iu) = 1.0E10;
end;
end;
end;
end;
% interpret filter and disturbance time constants
[nrtfil,nctfil]=size(tfilter);
if (isempty(tfilter))
tdist=[];
elseif (nctfil ~= ny)
error(' No. of columns of tfilter should be equal to no. outputs.');
elseif nrtfil==1
tdist=[];
else
tdist=tfilter(2,:);
tfilter=tfilter(1,:);
casenum=2;
end;
KF = zeros(nmny,ny);
for i = 1:ny
if (isempty(tfilter)==1)
fa(i)=1;
for j = 1:nm
KF((j-1)*ny+i,i)=fa(i);
end;
else
if tfilter(i)==0
fa(i) = 1;
elseif tfilter(i)==inf
fa(i) = 0;
else
fa(i)=1-exp(-delt/tfilter(i));
if fa(i) < 0 | fa(i) > 1
error('Filter time constants must be positive')
end;
end;
for j = 1:nm
KF((j-1)*ny+i,i)=fa(i);
end;
end;
end;
%
% Simulation begins
%
y = zeros(nfinal+2,ny);
u = zeros(nfinal+2,nu);
ym = zeros(nfinal+2,ny);
if (casenum~=2) % Loop basic systems
t1 = clock;
if (isempty(controller)==1) % Loop simulation - basic
% open-loop simulation
if (isempty(distplant)==1 & isempty(diststep)==0)
d = diag(diststep(1,:))*ones(ny,np);
YP = d(:);
YM = KF*d(:,1);
y(1,:) = YP(1:ny)';
ym(1,:) = YM(1:ny)'; % time=0;
else
YP = zeros(npny,1);
YM = zeros(nmny,1);
end;
if tfinal ~= 0,
fprintf (' Time remaining %g/%g\n',tfinal,tfinal);
end
u(1,:) = setpoint(1,:);
Du = u(1,:)';
if (isempty(uconstraint)==0)
Dusign = sign(Du);
Du = min(abs(Du),uconstraint(1,2*nu+1:3*nu)');
Du = Dusign.*Du;
u(1,:) = Du';
u(1,:) = max(u(1,:),uconstraint(1,1:nu));
u(1,:) = min(u(1,:),uconstraint(1,nu+1:2*nu));
Du = u(1,:)';
end;
YPA = plant(1:npny,1:nu)*Du;
YMA = model(1:nmny,1:nu)*Du;
if (isempty(distplant)==1 & isempty(diststep)==0)
d = diag(diststep(2,:))*ones(ny,np);
YPA(1:npny) = YPA(1:npny) + d(:);
elseif (isempty(distplant)==0 & isempty(diststep)==0)
d = diststep(1,:)';
YPA = YPA + distplant(1:npny,:)*d;
if (isempty(distmodel)==0)
YMA = YMA+distmodel(1:nmny,:)*d;
end;
end;
for i = 1:(npny-ny)
YPA(i) = YP(i+ny) + YPA(i);
end;
for i = (npny-ny+1):npny
YPA(i) = YP(i)+YPA(i);
end;
for i = 1:(nmny-ny)
YMA(i) = YM(i+ny)+YMA(i);
end;
for i = (nmny-ny+1):nmny
YMA(i) = YM(i)+YMA(i);
end;
DY = YPA(1:ny)-YMA(1:ny);
YMA = YMA + KF*DY;
YP = YPA; YM = YMA;
y(2,:) = YP(1:ny)';
ym(2,:) = YM(1:ny)'; %time=1;
for k = 2:nfinal+1 % Loop k open-loop
if (fix(k/10)-k/10)==0 & tfinal ~= 0,
fprintf(' Time remaining %g/%g\n',tfinal-k*delt,tfinal);
end;
u(k,:) = setpoint(min(k,ls),:);
Du = [u(k,:)-u(k-1,:)]';
if (isempty(uconstraint)==0)
rusat = min(k,nuconstraint);
Dusign = sign(Du);
Du = min(abs(Du),uconstraint(rusat,2*nu+1:3*nu)');
Du = Dusign.*Du;
u(k,:) = Du'+u(k-1,:);
u(k,:) = max(u(k,:),uconstraint(rusat,1:nu));
u(k,:) = min(u(k,:),uconstraint(rusat,nu+1:2*nu));
Du = (u(k,:)-u(k-1,:))';
end;
YPA = plant(1:npny,1:nu)*Du;
YMA = model(1:nmny,1:nu)*Du;
if (isempty(distplant)==1 & isempty(diststep)==0)
d = diag(diststep(min(k+1,ld+1),:))*ones(ny,np);
YPA(1:npny) = YPA(1:npny)+ d(:);
elseif (isempty(distplant)==0 & isempty(diststep)==0)
d = diststep(min(k,ld+1),:)';
YPA = YPA + distplant(1:npny,:)*d;
if (isempty(distmodel)==0)
YMA = YMA + distmodel(1:nmny,:)*d;
end;
end;
for i = 1:(npny-ny)
YPA(i) = YP(i+ny) + YPA(i);
end;
for i = (npny-ny+1):npny
YPA(i) = YP(i)+YPA(i);
end;
for i = 1:(nmny-ny)
YMA(i) = YM(i+ny)+YMA(i);
end;
for i = (nmny-ny+1):nmny
YMA(i) = YM(i)+YMA(i);
end;
DY = YPA(1:ny)-YMA(1:ny);
YMA = YMA +KF*DY;
y(k+1,1:ny) = YPA(1:ny)';
ym(k+1,1:ny) = YMA(1:ny)';
YP = YPA; YM = YMA;
end; % Loop k open-loop
y = y(1:nfinal+1,:);
ym = ym(1:nfinal+1,:);
u = u(1:nfinal+1,:);
else % Loop simulation - basic
% Closed-loop simulation
% Build a utility matrix used in the closed-loop simulation
E = zeros(pny,1);
if (isempty(distplant)==1 & isempty(diststep)==0)
d = diag(diststep(1,:))*ones(ny,np);
YP = d(:);
YM = KF*d(:,1);
E = YM(ny+1:ny+pny);
y(1,:) = YP(1:ny)';
ym(1,:) = YM(1:ny)'; % time=0;
else
YP = zeros(npny,1);
YM = zeros(nmny,1);
end;
if tfinal ~= 0,
fprintf (' Time remaining %g/%g\n',tfinal,tfinal);
end
setpointblock = setpoint(1:p,:)';
E = setpointblock(:) - E;
if (isempty(distmodel)==0 & isempty(diststep)==0)
d=diststep(1,:)';
E = E - distmodel(1:pny,:)*d;
end;
Du = controller*E;
u(1,:) = Du';
if (isempty(uconstraint)==0)
Dusign = sign(Du);
Du = min(abs(Du),uconstraint(1,2*nu+1:3*nu)');
Du = Dusign.*Du;
u(1,:) = Du';
u(1,:) = max(u(1,:),uconstraint(1,1:nu));
u(1,:) = min(u(1,:),uconstraint(1,nu+1:2*nu));
Du = u(1,:)';
end;
YPA = plant(1:npny,1:nu)*Du;
YMA = model(1:nmny,1:nu)*Du;
if (isempty(distplant)==1 & isempty(diststep)==0)
d = diag(diststep(2,:))*ones(ny,np);
YPA = YPA + d(:);
elseif(isempty(distplant)==0 & isempty(diststep)==0)
d = diststep(1,:)';
YPA= YPA + distplant(1:npny,:)*d;
if (isempty(distmodel)==0)
YMA = YMA + distmodel(1:nmny,:)*d;
end;
end;
for i = 1:(npny-ny)
YPA(i) = YP(i+ny) + YPA(i);
end;
for i = (npny-ny+1):npny
YPA(i) = YP(i)+YPA(i);
end;
for i = 1:(nmny-ny)
YMA(i) = YM(i+ny)+YMA(i);
end;
for i = (nmny-ny+1):nmny
YMA(i) = YM(i)+YMA(i);
end;
DY = YPA(1:ny)-YMA(1:ny);
YMA = YMA + KF*DY;
YP = YPA; YM = YMA;
y(2,:) = YP(1:ny)';
ym(2,:) = YM(1:ny)';
for k=2:nfinal+1
% Loop k closed-loop
if ((fix(k/10)-k/10 )==0) & tfinal ~= 0,
fprintf(' Time remaining %g/%g\n',tfinal-k*delt,tfinal);
end;
E = YM(ny+1:ny+pny);
setpointblock(:,1:p) = [setpointblock(:,2:p), ...
setpoint(min(ls,p+k-1),:)'];
E = setpointblock(:) - E;
if (isempty(distmodel)==0 & isempty(diststep)==0)
d(:)=diststep(min(k,ld+1),:)';
E = E - distmodel(1:pny,:)*d;
end;
Du = controller*E;
u(k,:) = u(k-1,:)+Du';
Du = u(k,:)' - u(k-1,:)';
if (isempty(uconstraint)==0)
rusat = min(k,nuconstraint);
Dusign = sign(Du);
Du = min(abs(Du),uconstraint(rusat,2*nu+1:3*nu)');
Du = Dusign.*Du;
u(k,:) = u(k-1,:)+Du';
u(k,:) = max(u(k,:),uconstraint(rusat,1:nu));
u(k,:) = min(u(k,:),uconstraint(rusat,nu+1:2*nu));
Du = (u(k,:)-u(k-1,:))';
end;
YPA = plant(1:npny,1:nu)*Du;
YMA = model(1:nmny,1:nu)*Du;
if (isempty(distplant)==1 & isempty(diststep)==0)
d = diag(diststep(min(k+1,ld+1),:))*ones(ny,np);
YPA(1:npny) = YPA(1:npny) + d(:);
elseif (isempty(distplant)==0 & isempty(diststep)==0)
d = diststep(min(k,ld+1),:)';
YPA = YPA + distplant(1:npny,:)*d;
if (isempty(distmodel)==0)
YMA = YMA + distmodel(1:nmny,:)*d;
end;
end;
for i = 1:(npny-ny)
YPA(i) = YP(i+ny) + YPA(i);
end;
for i = (npny-ny+1):npny
YPA(i) = YP(i)+YPA(i);
end;
for i = 1:(nmny-ny)
YMA(i) = YM(i+ny)+YMA(i);
end;
for i = (nmny-ny+1):nmny
YMA(i) = YM(i)+YMA(i);
end;
DY = YPA(1:ny) - YMA(1:ny);
YMA = YMA + KF*DY;
y(k+1,1:ny) = YPA(1:ny)';
ym(k+1,1:ny) = YMA(1:ny)';
YP = YPA; YM = YMA;
end; % Loop k closed-loop
y = y(1:nfinal+1,:);
ym= ym(1:nfinal+1,:);
u = u(1:nfinal+1,:);
end; % Loop simulation - basic
else % Loop basic systems
for i=1:ny
if (isempty(tdist))
if (noutm(i)==0|noutdm(i)==0)
alpha(i)=1;
else
alpha(i)=0;
end;
else
if tdist(i)==0
alpha(i)=0;
elseif tdist(i)==inf
alpha(i)=1;
elseif ((noutm(i)==0|noutdm(i)==0) & tdist~=inf)
disp('WARNING: tdist(i) must equal inf');
disp(' for integrating outputs.');
disp(' tdist(i) will be set to inf.');
alpha(i)=1;
else
alpha(i)= exp(-delt/tdist(i));
if alpha(i) < 0 | alpha(i) > 1
error('Disturbance time constants must be positive');
end;
end;
end;
fb(i) = alpha(i)*fa(i)^2/(1+alpha(i)-alpha(i)*fa(i));
end;
KFA = zeros(nmny,ny);
Ad=diag(alpha);
for i=2:nm
KFA((i-1)*ny+1:i*ny,1:ny)=KFA((i-2)*ny+1:(i-1)*ny,1:ny) + Ad^(i-2);
end;
KFA(nmny+1:nmny+ny,1:ny)=Ad^(nm-1);
KF(nmny+1:nmny+ny,1:ny)=zeros(ny,ny);
KFB=KFA*diag(fb);
KF = KF + KFB;
hp = noutp.*noutdp-ones(ny,1);
HP = diag(hp);
cm = noutm.*noutdm;
CM = diag(cm);
hm = noutm.*noutdm-ones(ny,1);
HM = diag(hm);
gp = 2*ones(ny,1)-noutp.*noutdp;
GP = diag(gp);
gm = 2*ones(ny,1)-noutm.*noutdm;
GM = diag(gm);
t1 = clock;
if (isempty(controller)==1) % Loop simulation - integrating
% open-loop simulation
if (isempty(distplant)==1 & isempty(diststep)==0)
d = diag(diststep(1,:))*ones(ny,np);
YP = d(:);
YM = KF(1:nmny,1:ny)*d(:,1);
y(1,:) = YP(1:ny)';
ym(1,:) = YM(1:ny)'; % time=0;
else
YP = zeros(npny,1);
YM = zeros(nmny,1);
end;
Xdist=zeros(ny,1);
if tfinal ~= 0,
fprintf(' Time remaining %g/%g\n',tfinal,tfinal);
end
u(1,:) = setpoint(1,:);
Du = u(1,:)';
if (isempty(uconstraint)==0)
Dusign = sign(Du);
Du = min(abs(Du),uconstraint(1,2*nu+1:3*nu)');
Du = Dusign.*Du;
u(1,:) = Du';
u(1,:) = max(u(1,:),uconstraint(1,1:nu));
u(1,:) = min(u(1,:),uconstraint(1,nu+1:2*nu));
Du = u(1,:)';
end;
YPA = plant(1:npny,1:nu)*Du;
YMA = model(1:nmny,1:nu)*Du;
if (isempty(distplant)==1 & isempty(diststep)==0)
d = diag(diststep(2,:))*ones(ny,np);
YPA(1:npny) = YPA(1:npny) + d(:);
elseif (isempty(distplant)==0 & isempty(diststep)==0)
d = diststep(1,:)';
YPA = YPA + distplant(1:npny,:)*d;
if (isempty(distmodel)==0)
YMA = YMA+distmodel(1:nmny,:)*d;
end;
end;
for i = 1:(npny-ny)
YPA(i) = YP(i+ny) + YPA(i);
end;
YPA(npny-ny+1:npny) = HP*YP(npny-2*ny+1:npny-ny)+GP*YP(npny-ny+1:npny)...
+YPA(npny-ny+1:npny);
for i = 1:(nmny-ny)
YMA(i) = YM(i+ny)+YMA(i);
end;
YMA(nmny-ny+1:nmny) = HM*YM(nmny-2*ny+1:nmny-ny)+GM*YM(nmny-ny+1:nmny)...
+YMA(nmny-ny+1:nmny)+CM*Xdist;
XdistA=Ad*Xdist;
DY = YPA(1:ny)-YMA(1:ny);
YMA = YMA + KF(1:nmny,1:ny)*DY;
XdistA = XdistA + KF(nmny+1:nmny+ny,1:ny)*DY;
YP = YPA; YM = YMA; Xdist = XdistA;
y(2,:) = YP(1:ny)';
ym(2,:) = YM(1:ny)'; %time=1;
for k = 2:nfinal+1 % Loop k open-loop
if ((fix(k/10)-k/10)==0) & tfinal ~= 0,
fprintf(' Time remaining %g/%g\n',tfinal-k*delt,tfinal);
end;
u(k,:) = setpoint(min(k,ls),:);
Du = [u(k,:)-u(k-1,:)]';
if (isempty(uconstraint)==0)
rusat = min(k,nuconstraint);
Dusign = sign(Du);
Du = min(abs(Du),uconstraint(rusat,2*nu+1:3*nu)');
Du = Dusign.*Du;
u(k,:) = Du'+u(k-1,:);
u(k,:) = max(u(k,:),uconstraint(rusat,1:nu));
u(k,:) = min(u(k,:),uconstraint(rusat,nu+1:2*nu));
Du = (u(k,:)-u(k-1,:))';
end;
YPA = plant(1:npny,1:nu)*Du;
YMA = model(1:nmny,1:nu)*Du;
if (isempty(distplant)==1 & isempty(diststep)==0)
d = diag(diststep(min(k+1,ld+1),:))*ones(ny,np);
YPA(1:npny) = YPA(1:npny)+ d(:);
elseif (isempty(distplant)==0 & isempty(diststep)==0)
d = diststep(min(k,ld+1),:)';
YPA = YPA + distplant(1:npny,:)*d;
if (isempty(distmodel)==0)
YMA = YMA + distmodel(1:nmny,:)*d;
end;
end;
for i = 1:(npny-ny)
YPA(i) = YP(i+ny) + YPA(i);
end;
YPA(npny-ny+1:npny) = HP*YP(npny-2*ny+1:npny-ny)+GP*YP(npny-ny+1:npny)...
+YPA(npny-ny+1:npny);
for i = 1:(nmny-ny)
YMA(i) = YM(i+ny)+YMA(i);
end;
YMA(nmny-ny+1:nmny) = HM*YM(nmny-2*ny+1:nmny-ny)+GM*YM(nmny-ny+1:nmny)...
+YMA(nmny-ny+1:nmny)+CM*Xdist;
XdistA=Ad*Xdist;
DY = YPA(1:ny)-YMA(1:ny);
YMA = YMA + KF(1:nmny,1:ny)*DY;
XdistA = XdistA + KF(nmny+1:nmny+ny,1:ny)*DY;
y(k+1,1:ny) = YPA(1:ny)';
ym(k+1,1:ny) = YMA(1:ny)';
YP = YPA; YM = YMA; Xdist = XdistA;
end; % Loop k open-loop
y = y(1:nfinal+1,:);
ym = ym(1:nfinal+1,:);
u = u(1:nfinal+1,:);
else % Loop simulation - integrating
% Closed-loop simulation
% Build a utility matrix used in the closed-loop simulation
E = zeros(pny,1);
if (isempty(distplant)==1 & isempty(diststep)==0)
d = diag(diststep(1,:))*ones(ny,np);
YP = d(:);
YM = KF(1:nmny,1:ny)*d(:,1);
E = YM(ny+1:ny+pny);
y(1,:) = YP(1:ny)';
ym(1,:) = YM(1:ny)'; % time=0;
else
YP = zeros(npny,1);
YM = zeros(nmny,1);
end;
Xdist=zeros(ny,1);
fprintf(' Time remaining %g/%g\n',tfinal,tfinal);
setpointblock = setpoint(1:p,:)';
E = setpointblock(:) - E;
if (isempty(distmodel)==0 & isempty(diststep)==0)
d=diststep(1,:)';
E = E - distmodel(1:pny,:)*d;
end;
Du = controller*E;
u(1,:) = Du';
Du = u(1,:)';
if (isempty(uconstraint)==0)
Dusign = sign(Du);
Du = min(abs(Du),uconstraint(1,2*nu+1:3*nu)');
Du = Dusign.*Du;
u(1,:) = Du';
u(1,:) = max(u(1,:),uconstraint(1,1:nu));
u(1,:) = min(u(1,:),uconstraint(1,nu+1:2*nu));
Du = u(1,:)';
end;
YPA = plant(1:npny,1:nu)*Du;
YMA = model(1:nmny,1:nu)*Du;
if (isempty(distplant)==1 & isempty(diststep)==0)
d = diag(diststep(2,:))*ones(ny,np)
YPA = YPA + d(:);
elseif(isempty(distplant)==0 & isempty(diststep)==0)
d = diststep(1,:)';
YPA= YPA + distplant(1:npny,:)*d;
if (isempty(distmodel)==0)
YMA = YMA + distmodel(1:nmny,:)*d;
end;
end;
for i = 1:(npny-ny)
YPA(i) = YP(i+ny) + YPA(i);
end;
YPA(npny-ny+1:npny) = HP*YP(npny-2*ny+1:npny-ny)+GP*YP(npny-ny+1:npny)...
+YPA(npny-ny+1:npny);
for i = 1:(nmny-ny)
YMA(i) = YM(i+ny)+YMA(i);
end;
YMA(nmny-ny+1:nmny) = HM*YM(nmny-2*ny+1:nmny-ny)+GM*YM(nmny-ny+1:nmny)...
+YMA(nmny-ny+1:nmny)+CM*Xdist;
XdistA=Ad*Xdist;
DY = YPA(1:ny)-YMA(1:ny);
YMA = YMA + KF(1:nmny,1:ny)*DY;
XdistA = XdistA + KF(nmny+1:nmny+ny,1:ny)*DY;
YP = YPA; YM = YMA;
Xdist = XdistA;
y(2,:) = YP(1:ny)';
ym(2,:) = YM(1:ny)';
for k=2:nfinal+1 % Loop k closed-loop
if ((fix(k/10)-k/10)==0)
fprintf(' Time remaining %g/%g\n',tfinal-k*delt,tfinal);
end;
E = YM(ny+1:ny+pny);
setpointblock(:,1:p) = [setpointblock(:,2:p), ...
setpoint(min(ls,p+k-1),:)'];
E = setpointblock(:) - E;
if (isempty(distmodel)==0 & isempty(diststep)==0)
d=diststep(min(k,ld+1),:)';
E = E - distmodel(1:pny,:)*d;
end;
Du = controller*E;
u(k,:) = u(k-1,:)+Du';
Du = u(k,:)' - u(k-1,:)';
if (isempty(uconstraint)==0)
rusat = min(k,nuconstraint);
Dusign = sign(Du);
Du = min(abs(Du),uconstraint(rusat,2*nu+1:3*nu)');
Du = Dusign.*Du;
u(k,:) = u(k-1,:)+Du';
u(k,:) = max(u(k,:),uconstraint(rusat,1:nu));
u(k,:) = min(u(k,:),uconstraint(rusat,nu+1:2*nu));
Du = (u(k,:)-u(k-1,:))';
end;
YPA = plant(1:npny,1:nu)*Du;
YMA = model(1:nmny,1:nu)*Du;
if (isempty(distplant)==1 & isempty(diststep)==0)
d = diag(diststep(min(k+1,ld+1),:))*ones(ny,np);
YPA(1:npny) = YPA(1:npny) + d(:);
elseif (isempty(distplant)==0 & isempty(diststep)==0)
d = diststep(min(k,ld+1),:)';
YPA = YPA + distplant(1:npny,:)*d;
if (isempty(distmodel)==0)
YMA = YMA + distmodel(1:nmny,:)*d;
end;
end;
for i = 1:(npny-ny)
YPA(i) = YP(i+ny) + YPA(i);
end;
YPA(npny-ny+1:npny) = HP*YP(npny-2*ny+1:npny-ny)+GP*YP(npny-ny+1:npny)...
+YPA(npny-ny+1:npny);
for i = 1:(nmny-ny)
YMA(i) = YM(i+ny)+YMA(i);
end;
YMA(nmny-ny+1:nmny) = HM*YM(nmny-2*ny+1:nmny-ny)+GM*YM(nmny-ny+1:nmny)...
+YMA(nmny-ny+1:nmny)+CM*Xdist;
XdistA=Ad*Xdist;
DY = YPA(1:ny)-YMA(1:ny);
YMA = YMA + KF(1:nmny,1:ny)*DY;
XdistA = XdistA + KF(nmny+1:nmny+ny,1:ny)*DY;
y(k+1,1:ny) = YPA(1:ny)';
ym(k+1,1:ny) = YMA(1:ny)';
YP = YPA; YM = YMA;
Xdist = XdistA;
end; % Loop k closed-loop
y = y(1:nfinal+1,:);
ym= ym(1:nfinal+1,:);
u = u(1:nfinal+1,:);
end; % Loop simulation - integrating
end;
if tfinal ~= 0,
fprintf('Simulation time is %g seconds.\n', etime(clock,t1));
end
% end of function MPCSIM.M