Adaptive MPC Design_模型预测、_模型预测控制_自适应MPC设计_模型预测

function [xk1, u, cost, useq, xseq, yseq, status, xest, Pk1, iAout] = mpc(... xk, old_u, ym, ref, md, umin, umax, ymin, ymax, E, F, G, S,... switch_in, ext_mv, MVtarget, isQP, nx, nxp, nup, ... nu, ny, degrees, Hinv, Kx, Ku1, Kut, Kr, Kv, Mlim, ... Mx, Mu1, Mv, z_degrees, utarget, p, maxiter, ... CustomSolver, CustomSolverCodeGen, UseWarmStart, UseSuboptimalSolution, nxQP, openloopflag, ... no_umin, no_umax, no_ymin, no_ymax, switch_inport, no_switch, enable_value, ... return_cost, H, return_mvseq, return_xseq, return_ovseq, Linv, Ac, ... ywt, uwt, duwt, rhoeps, isLTV, a, b, c, d, U, Y, X, DX, Pk, iA, ... no_ywt, no_uwt, no_duwt, no_rhoeps,... Wy, Wdu, Jm, SuJm, Su1, Sx, Hv, Wu, I1, ... A, C, B, D, Cid, Did, Mrows, nCC, Ecc, Fcc, Scc, Gcc, ... nv, no_md, no_ref, no_uref, no_mv, RYscale, RMDscale, myindex, ... xoff, CustomEstimation, mvindex, mdindex, unindex, nxid, Uscale, Yscale, uoff, voff, yoff, myoff, no_cc) %#codegen coder.extrinsic('mpcblock_optimizer_double_mex'); coder.extrinsic('mpcblock_optimizer_single_mex'); coder.extrinsic('mpcblock_refmd_double_mex'); coder.extrinsic('mpcblock_refmd_single_mex'); % Constants isSimulation = coder.target('Sfun') && ~coder.target('RtwForRapid') && ~coder.target('RtwForSim'); isAdaptive = ~isLTV; % isLTV=true forces isAdaptive=false isDouble = isa(ref,'double'); if isDouble DataType = 'double'; else DataType = 'single'; end ZERO = zeros('like',ref); ONE = ones('like',ref); hasMD = nv>1; % Pre-allocate all the MEX block outputs for the simulation mode %#ok<*NASGU> if isSimulation rseq = zeros(p*ny,1,'like',ref); %#ok<*NASGU> vseq = zeros((p+1)*nv,1,'like',ref); v = zeros(nv,1,'like',ref); xk1 = zeros(nx,1,'like',ref); u = zeros(nu,1,'like',ref); cost = ZERO; useq = zeros(p+1,nu,'like',ref); %#ok<*PREALL> status = ONE; xest = zeros(nx,1,'like',ref); iAout = iA; end % Model update nym = length(myindex); ai=zeros(nxp,nxp,'like',ref); bi=zeros(nxp,nup,'like',ref); ci=zeros(ny,nxp,'like',ref); di=zeros(ny,nup,'like',ref); ai(:,:)=a(:,:,1); bi(:,:)=b(:,:,1); ci(:,:)=c(:,:,1); di(:,:)=d(:,:,1); % Allocate matrices. Must allocate 3D matrix also in Adaptive case, % otherwise EML code does not compile. Bu=zeros(nx,nu,p+1,'like',ref); Bv=zeros(nx,nv,p+1,'like',ref); Dv=zeros(ny,nv,p+1,'like',ref); Dvm=zeros(nym,nv,p+1,'like',ref); Cm=zeros(nym,nx,p+1,'like',ref); if ~isLTV % Adaptive [A(:,:,1),C(:,:,1),Bu(:,:,1),Bv(:,:,1),Cm(:,:,1),Dv(:,:,1),Dvm(:,:,1),Qk,Rk,Nk] = mpc_plantupdate(... ai,bi,ci,di,A(:,:,1),B(:,:,1),C(:,:,1),D(:,:,1),mvindex,mdindex,unindex,nxp,nup,ny,nv,nxid, ... myindex,Uscale,Yscale,Cid,Did); else % Retrieve LTV model for i=1:p+1 ai(1:nxp,1:nxp)=a(:,:,i); bi(1:nxp,1:nup)=b(:,:,i); ci(1:ny,1:nxp)=c(:,:,i); di(1:ny,1:nup)=d(:,:,i); [A(:,:,i),C(:,:,i),Bu(:,:,i),Bv(:,:,i),Cm(:,:,i),Dv(:,:,i),Dvm(:,:,i),Qk,Rk,Nk] =... mpc_plantupdate(ai,bi,ci,di,A(:,:,i),B(:,:,i),C(:,:,i),D(:,:,i),mvindex,mdindex,unindex,nxp,nup,ny,nv,nxid, ... myindex,Uscale,Yscale,Cid,Did); end end % Offset update together with Mlim, utarget, Bv and Dv values [Mlim, utarget, uoff, voff, yoff, myoff, xoff, Bv, Dv] = ... mpc_updateFromNominal(isAdaptive,isQP,p,Mlim,Mrows,... U,Uscale,uoff,mvindex,voff,mdindex,utarget,nu,nv-1,... Y,Yscale,yoff,myoff,myindex,ny,... X,xoff,nxp,DX,A,Bu,Bv,C,Dv,nCC); % Remove last u offset old_u = old_u - uoff; % Get reference and MD signals -- accounting for previewing if isSimulation if isDouble [rseq, vseq, v] = mpcblock_refmd_double_mex(ref,md,nv,ny,p,yoff,voff,no_md,no_ref,openloopflag, RYscale, RMDscale); else [rseq, vseq, v] = mpcblock_refmd_single_mex(ref,md,nv,ny,p,yoff,voff,no_md,no_ref,openloopflag, RYscale, RMDscale); end else % When doing code generation, use M code directly [rseq, vseq, v] = mpcblock_refmd(ref,md,nv,ny,p,yoff,voff,no_md,no_ref,openloopflag, RYscale, RMDscale); end % External MV override. % NOTE: old_u and ext_mv input signals are dimensionless and offset-free. if no_mv==ONE % no external mv: old_u is the optimal u[k-1] from last_mv delmv = zeros(nu,1,'like',ref); else % has external mv: old_u is the external u[k-1] ext_mv = ext_mv - uoff; % bias correction delmv = ext_mv - old_u; old_u = ext_mv; end % Obtain x[k|k] xk = xk - xoff; % Remove offset if CustomEstimation==ONE % Input is x(k|k) xest = xk; else % Calculate gain matrices, and perform measurement update. Kinv = eye(nym,'like',ref)/(Cm(:,:,1)*Pk*Cm(:,:,1)' + Rk); L = (A(:,:,1)*Pk*Cm(:,:,1)' + Nk)*Kinv; M = Pk*Cm(:,:,1)'*Kinv; % LTVKF state estimation. % Scale measured output and remove offset. ym = ym.*RYscale(myindex) - myoff; % Correct x(k|k-1) for possible external mv override. % NOTE: Offset was removed from x[k|k-1] at k=0. xk = xk + Bu(:,:,1)*delmv; % Measurement upate to x(k|k) ym_est = Cm(:,:,1)*xk + Dvm(:,:,1)*v; y_innov = ym - ym_est; xest = xk + M*y_innov; end % Real-time MV target override % Note: utargetValue is a vector length p*nu. if no_uref==ONE % no external utarget utargetValue = utarget; else % Define constant targets for the entire horizon. MVtarget = MVtarget - uoff; % Bias correction utargetValue = reshape(MVtarget(:,ones(p,1)),nu*p,1); end % Real-time custom constraint override (scaled E/F/S) if no_cc~=ONE Ecc = E; Fcc = F; Gcc = G; if nv>1 Scc = S; end end return_sequence = (return_mvseq || return_xseq || return_ovseq)*ONE; if isSimulation if isDouble [u, cost, useq, status, iAout] = mpcblock_optimizer_double_mex(... rseq, vseq, umin, umax, ymin, ymax, switch_in, xest, old_u, iA, ... isQP, nu, ny, degrees, Hinv, Kx, Ku1, Kut, Kr, Kv, Mlim, ... Mx, Mu1, Mv, z_degrees, utargetValue, p, uoff, voff, yoff, maxiter, ... CustomSolver, false, UseWarmStart, UseSuboptimalSolution, nxQP, openloopflag, ... no_umin, no_umax, no_ymin, no_ymax, no_cc, switch_inport, ... no_switch, enable_value, return_cost, H, return_sequence, Linv, Ac, ... ywt, uwt, duwt, rhoeps, no_ywt, no_uwt, no_duwt, no_rhoeps,... Wy, Wdu, Jm, SuJm, Su1, Sx, Hv, Wu, I1, ... isAdaptive, isLTV, A, Bu, Bv, C, Dv, ... Mrows, nCC, Ecc, Fcc, Scc, Gcc); else [u, cost, useq, status, iAout] = mpcblock_optimizer_single_mex(... rseq, vseq, umin, umax, ymin, ymax, switch_in, xest, old_u, iA, ... isQP, nu, ny, degrees, Hinv, Kx, Ku1, Kut, Kr, Kv, Mlim, ... Mx, Mu1, Mv, z_degrees, utargetValue, p, uoff, voff, yoff, maxiter, ... CustomSolver, false, UseWarmStart, UseSuboptimalSolution, nxQP, openloopflag, ... no_umin, no_umax, no_ymin, no_ymax, no_cc, switch_inport, ... no_switch, enable_value, return_cost, H, return_sequence, Linv, Ac, ... ywt, uwt, duwt, rhoeps, no_ywt, no_uwt, no_duwt, no_rhoeps,... Wy, Wdu, Jm, SuJm, Su1, Sx, Hv, Wu, I1, ... isAdaptive, isLTV, A, Bu, Bv, C, Dv, ... Mrows, nCC, Ecc, Fcc, Scc, Gcc); end else % When doing code generation, use M code directly [u, cost, useq, status, iAout] = mpcblock_optimizer(... rseq, vseq, umin, umax, ymin, ymax, switch_in, xest, old_u, iA, ... isQP, nu, ny, degrees, Hinv, Kx, Ku1, Kut, Kr, Kv, Mlim, ... Mx, Mu1, Mv, z_degrees, utargetValue, p, uoff, voff,