Decentralized Linear Model Predictive Control
The class Decentralized LINear CONstrained (dlincon) provide the used with an extension of the object lincon, which is available with the Hybrid toolbox (by A. Bemporad) toward decentralized control.
Contents
Class description
Starting with an user defined decentralization, the toolbox automatically creates the corresponding set of decentralized lincon controllers of appropriate dimension. The user is requested to provide a starting configuration for the centralized controller (using the same parameters as in in lincon) and that will be readapted, in proper dimension, to each subcontroller. Moreover each subcontroller can be customized according to needing as it is a lincon class instance.
Methods are available for three kinds of control action computation:
- global : centralized controller;
- Dglobal: run all DMPCs and assemble the components to return the complete set of inputs;
- i : run the i-th controller and return its results only;
Both regulator and tracking modes are supported, however the latter is realized by means of coordinates shift, accordingly to "Barcelli and Bemporad 'Decentralized Model Predictive Control of Dynamically-Coupled Linear Systems: Tracking under Packet Loss'". Update covering the normal tracking mode is matter of actual development.
classdef dlincon
Properties
decent : Decentralization structure: is supposed to an array of structure containing fileds:
- x (states of the subsystem);
- y (outputs of the subsystem);
- u (inputs of the subsystem);
- applied (inputs effectively applied, no overlap is allowed). each containing the array of indices to be included in the subsystem.
M : number of subsystem
W : selection matrix for states
Z : selection matrix for inputs
G : selection matrix for outputs
APP : selection matrix for applied inputs
Zr : state coordinate shift
Vr : input coordinate shift
sub_sys : cell array of subcontrollers models
ccon : centralized controller
dcon : cell array of lincon objects
var_bounds : 1 if bounds are specified online by the user, 0 else
type : 'reg' or 'track'
properties (Access = public) decent; M; W; Z; G; APP; Zr; Vr; sub_sys; ccon; dcon; var_bounds; type; end methods (Access = public) function dl = dlincon(varargin)
Class constructor
dl = dlincon(model,type,cost,interval,limits,yzerocon,... decent,varbounds);
- model : centralized LTI model of the plant
- type : 'reg' for regulator or 'track' for tracking
- cost : structure with state and input matrices weights (see lincon for more details);
- interval : structure with fields N and Nu, prediction and simulation horizons respectively (see lincon for more details);
- limits : structure with bounds on states and inputs (see lincon for more details);
- yzerocon : if 1 constraints are also enforced at the starting instant (see lincon for more details);
- decent : decentralization structure, see Properties for a more detailed help;
- varbounds : if 1 state are added to handle variant bounds specified by the user online to be enforced.
if (nargin<1) || (~strcmp(class(varargin{1}),'ss')) error('Missing model!'); else model=varargin{1}; dl.type='reg'; cost.Q=eye(length(model.a)); cost.R=eye(size(model.b,2)); interval.N=10; interval.Nu=5; limits=[]; dl.decent=[]; yzerocon=0; dl.var_bounds=0; if nargin>=2 switch varargin{2} case {'reg',''} dl.type='reg'; cost=varargin{3}; if isfield(cost,'S') cost=rmfield(cost,'S'); end if isfield(cost,'T') cost=rmfield(cost,'T'); end case 'track' dl.type='track'; cost.Q=varargin{3}.S; cost.R=varargin{3}.T; otherwise dl=[]; error(['type string unrecognized, must be '... 'either ''reg'' or ''track''!']); end if ~isempty(cost) if ~isfield(cost,'rho') cost.rho=+inf; end end if nargin>=4 if ~isfield(varargin{4},'N')||~isfield(varargin{4},'Nu') dl=[]; error('interval must have fields N and Nu'); else interval=varargin{4}; end if nargin>=5 limits=varargin{5}; if nargin>=6 if (varargin{6}==1) || (varargin{6}==0) yzerocon=varargin{6}; else dl=[]; disp('yzerocon must be either 0 or 1'); end if nargin>=7 dl.M=length(varargin{7}); dl.decent=varargin{7}; if nargin>=8 if (varargin{8}==1)||(varargin{8}==0) dl.var_bounds=varargin{8}; end end end end end end end if dl.var_bounds [dl,model,cost,interval,limits,yzerocon] = ... add_var_bounds(dl,model,cost,interval); end dl = create_matrices(dl,model); if decent_verify(dl,model) dl=create_subsys(dl,model); else dl=[]; error('Wrong decentralization'); end dl.ccon = lincon(model,'reg',cost,interval,limits,[],yzerocon); dl = create_controllers(dl,cost,interval,limits); end
end function [u dl] = Deval(varargin)
Control computation method
u = dl.Deval(type,xk,r) or u = Deval(dl,type,xk,r) where dl is a dlincon obj
- type: global : centralized controller; Dglobal: run all DMPCs and assemble the components to output the complete set of inputs; i : run the i-th controller and outputs its results only;
- xk : state measurements of estimations (must be coherent with state dimension
- r : vector of references for the outputs, ignored if type='reg'.
if nargin>=3 dl=varargin{1}; type=varargin{2}; xk=varargin{3}; if (nargin<4) if strcmp(dl.type,'track') warning(['reference should be specified. Null '... 'value will be used']); r=zeros(dl.ccon.ny,1); end else r=varargin{4}; if isempty(r) if strcmp(dl.type,'track') warning('Empty reference, null reference is used'); end r=zeros(dl.ccon.ny,1); end end else error('type end measurements must be specified') end u2=[]; if strcmp(dl.type,'reg') dl.Vr=zeros(dl.ccon.nu,1); dl.Zr=zeros(dl.ccon.nx,1); else [A B C D]=ssdata(dl.ccon.model); Nu=dl.ccon.nu; n=dl.ccon.nx; if ~dl.var_bounds dl.Vr = ( C*inv(eye(n)-A)*B ) \ r; dl.Zr = inv(eye(n)-A)*(B*dl.Vr); else n=n-2*dl.ccon.nu; dl.Vr = ( C(1:n,1:n) * inv(eye(n)-... A(1:n,1:n))*B(1:n,:) ) \ r(1:n,:); dl.Zr = [inv(eye(n)-A(1:n,1:n))*(B(1:n,:)*dl.Vr);... zeros(2*dl.ccon.nu,1)]; end end zk = xk(1:length(dl.W{1})) - dl.Zr; switch type case 'global' u = eval( dl.ccon , zk); u = u + dl.Vr; case 'Dglobal' M=length(dl.dcon); u=zeros(dl.ccon.nu,1); for i=1:M
Obtain i-th subsystem state
zk_i=getIthSubState(dl,zk,xk,i);
Compute i-th sbsystem's inputs
u_tot=eval(dl.dcon{i},zk_i);
Apply only the ones which deserve to be
u_app=zeros(length(u_tot),1); for j = 1:length(dl.APP{i}) s=size(dl.Z{i}); for k=1:s(2) if find(dl.Z{i}(:,k))==dl.APP{i}(j) u_app(k)=u_tot(k); end end end u = u + dl.Z{i}*u_app;
end u = u + dl.Vr; otherwise
Controller to be evaluated
i = type; try zk = xk(1:length(dl.W{1})) - dl.Zr; zk_i=dl.W{i}'*zk; catch e zk_i = xk - dl.W{i}'*dl.Zr; end
Obtain i-th subsystem state
zk_i=getIthSubState(dl,zk,xk,i);
Compute the inputs of the i-th subsystem
u_tot=eval(dl.dcon{i},zk_i);
Apply only the ones which deserve to be
u_app=zeros(length(u_tot),1); for j = 1:length(dl.APP{i}) s=size(dl.Z{i}); for k=1:s(2) if find(dl.Z{i}(:,k))==dl.APP{i}(j) u_app(k)=u_tot(k); end end end vr_i=dl.Z{i}'*dl.Vr; u = u_app + vr_i;
end
end function res=stability_test(dl,range,cost)
Stability test around the origin
stability_test(dl,range,cost)
Test described in Barcelli and Bemporad, NECSYS09, returning true or false.
W = dl.W; Z = dl.Z; [Nx Nu] = size(dl.ccon.model.b); [A B C D]=ssdata(dl.ccon.model); if dl.var_bounds Nx=Nx-2*Nu; A = A(1:Nx,1:Nx); B = B(1:Nx,:); C = C(1:Nx,1:Nx); D = D(1:Nx,:); end M=dl.M;
Create explicit controllers ranges
for i = 1:M Drange(i).xmin = W{i}' * range.xmin; Drange(i).xmax = W{i}' * range.xmax; end
Create explicit controllers
for i = 1:M DExpCont{i} = expcon(dl.dcon{i},Drange(i)); end % % Get raw data from expcon obj for i=1:M i1{i}=DExpCont{i}.i1; i2{i}=DExpCont{i}.i2; H{i}=DExpCont{i}.H; K{i}=DExpCont{i}.K; F{i,1}=DExpCont{i}.F; G{i,1}=DExpCont{i}.G; nu{i}=DExpCont{i}.nu; nr{i}=DExpCont{i}.nr; end
Computational optimization
for i=1:M u_opt{i}=zeros(nu{i},1); end x=zeros(Nx,1); for j=1:M flag=1; i=1; if (nr{j}>1) while i<=nr{j} & flag,
i1i=i1{j}(i); i2i=i2{j}(i); Hi=H{j}(i1i:i2i,:);
Check the region
if all(Hi*W{j}'*x<=K{j}(i1i:i2i,:)), or_F{j}=F{j}(1+(i-1)*nu{j}:i*nu{j},:); or_G{j}=G{j}(1+(i-1)*nu{j}:i*nu{j},:); flag=0; end i=i+1;
end else or_F{j}=F{j}; or_G{j}=G{j}; end end app_F=zeros(Nu,Nx); app_G=zeros(Nu,1); for i=1:M app_F(i,:)=or_F{i}(dl.decent(i).applied,:)*W{i}'; app_G(i)=or_G{i}(dl.decent(i).applied); end
We consider the stability test in an area around the origin for which the controllers are linear (not affine -> Gs=0)
ClosedLoop=(A+B*app_F); Ceig=eig(ClosedLoop); res=all(abs(Ceig) < 1); if res fprintf(['Closed Loop is stable with this decentralization\n '... 'at least around the origin.(feedback method)\n']); else fprintf(['Closed loop is not stable either around the origin\n'... 'with this decentralization.(feedback method)\n']); end
Our method stability test
Q=cost.Q; R=cost.R;
Lyapunov equation solution computation
P=[]; for i=1:length(W) Ai{i}=W{i}'*A*W{i}; Bi{i}=W{i}'*B*Z{i}; P{i}=dlyap(Ai{i}',W{i}'*Q*W{i}); end
Global optimal input
u=app_F*x;
Local optimal inputs
for i=1:length(W) u_opt{i}=or_F{i}*W{i}'*x+or_G{i}; end
Maximum length of missing packet sample times
maxL=1;
Compute sum(WWQWW)
part1=[]; for j=1:maxL part1{j}=W{1}*W{1}'*Q*W{1}*W{1}'; for i=2:length(W) part1{j}=part1{j}+W{i}*W{i}'*Q*W{i}*W{i}'; end end part2=[];for i=1:maxL, part2{i}=zeros(Nx);end; for j=1:maxL for i=1:length(W) Dx=(eye(Nx)-W{i}*W{i}'); Du=(app_F-Z{i}*or_F{i}*W{i}'); Dy=W{i}*W{i}'*(A*Dx + B*Z{i}*Z{i}'*Du )... +dl.DeltaA(i)+dl.DeltaB(i)*app_F; TildeX=(Ai{i}+Bi{i}*or_F{i})*W{i}'; Aj_1=A^(j-1); F_{i}=(2*TildeX'*W{i}'+Dy')*Aj_1'*W{i}*P{i}*W{i}'*Aj_1*Dy; part2{j}=part2{j}+F_{i}; %new part1{j}=part1{j}+... TildeX'*(P{i}-W{i}'*Aj_1'*W{i}*P{i}*W{i}'*Aj_1*W{i})*TildeX; end end ver=0; flag=1; i=1; while( (i<=maxL) && flag )
Positive definitiveness of result is to be checked
try chol(part1{i}-part2{i}); ver=ver+1; catch flag=0; i=i-1; end i=i+1;
end if ver>0 fprintf(['The closed loop with this decentralization is stable at\n ' ... 'least around the origin even with a maximum of' ... ' %d losses (stability test)!!\n'],ver-1); else fprintf(['The test is not respected so we can say nothing\n'... ' about stability\n']); fprintf('The critical loss is number %d\n',i-1); end ris=ver;
end end methods (Access = private) function ris=shiftlimits(dl,Z,lim) s=size(Z); for i=1:s(2) ris(i)=lim(find(Z(:,i))); end end function dl = create_controllers(dl,cost,interval,limits)
Since the reference may be unknown at this stage, the offset cannot be computed
Vr = zeros(dl.ccon.nu,1); Zr = zeros(dl.ccon.nx,1); type = dl.ccon.type; W=dl.W; Z=dl.Z; APP=dl.APP; if strcmp(type,'reg') fields={'umin','umax'}; threshold=1; else fields={'umin','dumin','umax','dumax'}; threshold=2; end clear Dcon; Dcon=[]; M = dl.M; for i=1:M
Cost
Dcost{i}=[]; Dcost{i}.Q=W{i}'*cost.Q*W{i}; Dcost{i}.R=Z{i}'*cost.R*Z{i}; try Dcost{i}.P=W{i}'*cost.P*W{i}; Dcost{i}.K=Z{i}'*cost.K*W{i}; catch end if ~dl.var_bounds
Limits
Dlimits{i}=[];
for k=1:length(fields)
Only those input that will really be applied need to be constrained
eval(['applied.' fields{k} ... '=shiftlimits(dl,APP{i},limits.' fields{k} ... '-Vr);']); if k<=threshold eval(['Dlimits{i}.' fields{k} ... '=-inf*ones(length(applied.' fields{k} ... '),1);']); else eval(['Dlimits{i}.' fields{k} ... '=inf*ones(length(applied.' fields{k}... '),1);']); end for j=1:length(dl.decent(i).applied) for v=1:length(dl.decent(i).u) if dl.decent(i).u(v)==dl.decent(i).applied(j) eval(['Dlimits{i}.' fields{k} ... '(v)=applied.' fields{k} '(j);']); if isnan(eval(['Dlimits{i}.' fields{k}... '(v);'])) if k<=threshold eval(['Dlimits{i}.' fields{k}... '(v)=-inf;']); else eval(['Dlimits{i}.' fields{k}... '(v)=inf;']); end end else if k<=threshold eval(['Dlimits{i}.' fields{k}... '(v)=-inf;']); else eval(['Dlimits{i}.' fields{k}... '(v)=inf;']); end end end end clear applied
end dcon{i}=lincon(dl.sub_sys{i},dl.ccon.type,Dcost{i},... interval,Dlimits{i});
else
[A B C D]=ssdata(dl.sub_sys{i}); [Nstates Ninputs]=size(B); [sup Noutputs]=size(C);
Add constant states umin,umax
[Nstates Ninputs]=size(B); Aa=[ A zeros( Nstates , 2*Ninputs ); zeros( 2*Ninputs , Nstates ) eye(2*Ninputs) ]; Ba=[ B ; zeros( 2*Ninputs , Ninputs ) ];
Set y2=umin(t)-u(t), y3=umax(t)-u(t)
[Noutputs sup]=size(C); clear sup Ca=[C zeros( Noutputs , 2*Ninputs ); zeros(2*Ninputs , Nstates ) eye( 2*Ninputs ) ]; sup=''; for j=1:Ninputs sup=[sup ' -ones(2,1),']; end sup(end)=' '; eval(['dd=blkdiag(' sup ');']); Da=[D;dd]; Ts=dl.ccon.model.ts; model=ss(Aa,Ba,Ca,Da,Ts);
Limit
Dlimits{i}=[];
for k=1:length(fields)
Check existence of related fields
try eval(['limits.' fields{k} ';']); catch if k<=3, eval(['limits.' fields{k} ... '=-inf*ones(length(Z{1}),1);']); else eval(['limits.' fields{k} ... '=inf*ones(length(Z{1}),1);']); end end eval(['applied.' fields{k} ... '=shiftlimits(dl,APP{i},limits.' fields{k} '-Vr);']); eval(['Dlimits{i}.' fields{k} ... '=shiftlimits(dl,Z{i},limits.' fields{k} ');']);
end Nin=length(dl.decent(i).u); Nout=length(dl.decent(i).y); sup=[]; sup2=[]; for ii =1:Nin if sum(find(dl.decent(i).applied==dl.decent(i).u(ii))) sup=[sup -Inf 0]; sup2=[sup2 0 Inf]; else sup=[sup -Inf -inf]; sup2=[sup2 inf Inf]; end end
Change ymin and ymax if required
Dlimits{i}.ymin=[-Inf*ones(1,Nout) sup ]; Dlimits{i}.ymax=[Inf*ones(1,Nout) sup2 ]; if strcmp(dl.ccon.type,'reg')
Output weight
Dcost{i}.Q=blkdiag(Dcost{i}.Q,zeros(2*Ninputs));
Input increment weight
Dcost{i}.R=1*ones(Ninputs); Dcost{i}.P=zeros(length(Dcost{i}.Q));
else
Output weight
Dcost{i}.S=blkdiag(Dcost{i}.S,zeros(2*Ninputs));
Input increment weight
Dcost{i}.T=.1*ones(Ninputs);
end
Hard constraints
Dcost{i}.rho=cost.rho; moves=interval.Nu;
Input constraints horizon k=0,...,Ncu
interval.Ncu=moves-1;
Output constraints horizon k=1,...,Ncy
interval.Ncy=moves-1;
Check also output constraints for k=0
yzerocon=1;
dcon{i}=lincon(model,dl.ccon.type,Dcost{i},interval,...
Dlimits{i},[],yzerocon);
Qn=blkdiag(1*eye(Nstates),1e-5*eye(2*Ninputs));
Rn=blkdiag(eye(Noutputs),zeros(2*Ninputs));
ymeasured=1:(Noutputs+2*Ninputs);
dcon{i}=kalmdesign(dcon{i},Qn,Rn,ymeasured);
end
end dl.Zr = Zr; dl.Vr = Vr; for i=1:M dl.APP{i}=dl.decent(i).applied; end dl.dcon = dcon;
end function [dl,model,cost,interval,limits,yzerocon] = ... add_var_bounds(dl,model,cost,interval)
[A B C D]=ssdata(model);
Add constant states umin,umax
[Nstates Ninputs]=size(B); Aa=[ A zeros( Nstates , 2*Ninputs ); zeros( 2*Ninputs , Nstates ) eye(2*Ninputs) ]; Ba=[ B ; zeros( 2*Ninputs , Ninputs ) ];
Set y2=umin(t)-u(t), y3=umax(t)-u(t)
Noutputs=size(C,1); Ca=[C zeros( Noutputs , 2*Ninputs ); zeros(2*Ninputs , Nstates ) eye( 2*Ninputs ) ]; str=''; for i=1:Ninputs str=[str ' -ones(2,1),']; end str(end)=' '; eval(['dd=blkdiag(' str ');']); Da=[D;dd]; model=ss(Aa,Ba,Ca,Da,model.Ts);
Since the limits will be given online, the fixed limits should be removed
clear limits limits.umin=-Inf*ones(Ninputs,1); limits.umax=Inf*ones(Ninputs,1); newymin=[]; newymax=[]; for i =1:Ninputs newymin=[newymin -Inf 0]; newymax=[newymax 0 Inf]; end
Change ymin and ymax if required
limits.ymin=[-Inf*ones(1,Noutputs) newymin ]; limits.ymax=[Inf*ones(1,Noutputs) newymax ]; clear newymin newymax
Output weight
cost.Q=blkdiag(cost.Q,zeros(2*Ninputs));
Input increment weight
cost.R=.1*ones(Ninputs); cost.P=zeros(length(cost.Q)); moves=interval.Nu;
Input constraints horizon k=0,...,Ncu
interval.Ncu=moves-1;
Output constraints horizon k=1,...,Ncy
interval.Ncy=moves-1;
Check also output constraints for k=0
yzerocon=1;
end function dl = create_matrices(dl,sys) decent=dl.decent; M=dl.M; [n m]=size(sys.B); g=size(sys.C,1); for i=1:M ni(i)=length(decent(i).x); W{i}=zeros(n,ni(i)); for j=1:ni(i) W{i}(decent(i).x(j),j)=1; end mi(i)=length(decent(i).u); Z{i}=zeros(m,mi(i)); for j=1:mi(i) Z{i}(decent(i).u(j),j)=1; end gi(i)=length(decent(i).y); G{i}=zeros(g,gi(i)); for j=1:gi(i) G{i}(decent(i).y(j),j)=1; end ai(i)=length(decent(i).applied); APP{i}=zeros(m,ai(i)); for j=1:ai(i) APP{i}(decent(i).applied(j),j)=1; end end dl.APP=APP; dl.W=W; dl.Z=Z; dl.G=G; end function res = decent_verify(dl,sys)
decent=dl.decent; M = dl.M;
All elements in decent have to be >0 and < number of corresponding elements in system (because they are indices)
[n m]=size(sys.B); ny=size(sys.C,2); field_lim={m,n,ny,m}; fields={'u','x','y','applied'};
Flag to determine if decentralization is wrong
flg=0; er_smg=''; for i = 1 : M for ff=1:length(fields)
Indices have to array columns
if eval(['size(decent(i).' fields{ff} ')<2']) eval(['decent(i).' fields{ff} '=decent(i). ' ... fields{ff} ''';']); end
Indices have to >0 and <= #corresponding element
if ~all(eval(['decent(i).' fields{ff}])>0) er_smg=[er_smg 'Subsystem = ' num2str(i) '; filed='... fields{ff} '\n']; flg=1; er_smg=[er_smg 'Indices <=0 are :\n']; er_smg=[er_smg sprintf(num2str(find(eval(... ['decent(i).' fields{ff}])<=0))) '\n\n']; end if ~all(eval(['decent(i).' fields{ff}])<=field_lim{ff}) er_smg=[er_smg 'Subsystem = ' num2str(i) '; filed='... fields{ff} '\n']; flg=1; er_smg=[er_smg 'Indices grater than corresponding'... ' dimension are :\n']; er_smg=[er_smg sprintf(num2str(find(eval([... 'decent(i).' fields{ff}])>field_lim{ff}))) '\n\n']; end
end end assigned=zeros(m,1); for i = 1 : M mat = zeros(m,1); for j=1:length(decent(i).applied) mat(decent(i).applied(j))=1; end assigned = assigned +mat; end if find(assigned==0) er_smg=[er_smg 'Indices of missing applied inputs are :\n']; er_smg=[er_smg sprintf(num2str(find(assigned==0))) '\n\n']; flg=1; end if find(assigned>1) er_smg=[er_smg 'Indices of multiple assigned inputs are :\n']; er_smg=[er_smg sprintf(num2str(find(assigned>1))) '\n\n']; flg=1; end for i=1:M for j=1:length(decent(i).applied) if length(find(decent(i).u==decent(i).applied(j)))==0 flg=1; er_smg=[er_smg 'In subsystem ' num2str(i) ... ' applied input of index ' num2str(j) ' is not'... ' in .u\n\n']; end end end W = dl.W; Z = dl.Z; APP = dl.APP; sup=W{1}; for i=2:M for j=1:size(W{i}) sup(j)=sup(j)+sum(W{i}(j,:)); end end if min(find(sup)) ind=find(sup); er_smg=[er_smg 'States number ' num2str(ind') ' are not'... 'assigned to any subsystem\n\n']; end if flg disp('Wrong DECENTRALIZATION !!!') fprintf(['\n' er_smg]); end res=~flg;
end function dl = create_subsys(dl,sys) M = dl.M; W=dl.W; Z=dl.Z; G=dl.G; for i = 1 : M dl.sub_sys{i}=ss(W{i}'*sys.a*W{i},W{i}'*sys.b*Z{i},... G{i}'*sys.c*W{i},G{i}'*sys.d*Z{i},sys.Ts); end end function zk_i = getIthSubState(dl,zk,xk,i) zk_i=dl.W{i}'*zk; if dl.var_bounds
Bounds references
b_vr=[]; for ii=1:dl.ccon.nu b_vr(end+1,1)=dl.Vr(ii); b_vr(end+1,1)=dl.Vr(ii); end sup=xk(dl.ccon.nx-2*dl.ccon.nu+1:end)-b_vr; bounds = []; for j=1:size(dl.Z{i}') if find(dl.APP{i}==find(dl.Z{i}(:,j))) ind=2*find(dl.Z{i}(:,j))-1; bounds=[bounds; sup(ind:ind+1)]; else bounds=[bounds; -1e6;1e6]; end end zk_i=[zk_i;bounds];
end end function out=DeltaA(dl,i) A=dl.ccon.model.a; Wi=dl.W{i}; out=(eye(length(A))-Wi*Wi')*A; end function out=DeltaB(dl,i) B=dl.ccon.model.b; Wi=dl.W{i}; Zi=dl.Z{i}; out=B-Wi*Wi'*B*Zi*Zi'; end function out=DeltaS(dl,i,x,u,u_opt,Pi) A=dl.ccon.model.a; B=dl.ccon.model.n; Wi=dl.W{i}; Zi=dl.Z{i}; sup=DeltaY(Wi,Zi,A,B,x,u,u_opt); Ai=Wi'*A*Wi; Bi=Wi'*B*Zi; out = 2*(Ai*Wi'*x+Bi*u_opt)'*Pi*Wi'*sup + sup'*Wi*Pi*Wi'*sup; end function out=DeltaU(dl,i,u,u_opt) Zi=dl.Z{i}; out = u-Zi*u_opt; end function out=DeltaX(dl,i,x) Wi=dl.W{i}; out=(eye(length(Wi))-Wi*Wi')*x; end function out=DeltaY(dl,i,x,u,u_opt) Wi=dl.W{i}; Zi=dl.Z{i}; A=dl.ccon.model.a; B=dl.ccon.model.b; out=Wi*Wi'*( A*DeltaX(Wi,x)+B*Zi*Zi'*DeltaU(u,Zi,u_opt) )... +DeltaA(Wi,A)*x+DeltaB(Wi,Zi,B)*u; end end
end