image thumbnail

MPC Tutorial IV - State Space MPC with input increment

by

 

functions and simulink block implement state space MPC using input increment

mpcsfunc(t,z,y,flag,A,B,C,D,P,M,Q,R,Ts)
function [sys,x0,str,ts] = mpcsfunc(t,z,y,flag,A,B,C,D,P,M,Q,R,Ts)

%sfunction for mpc.
%
%By Yi Cao, Cranfield University, UK, (c) 27 January 2009
%

    persistent ssmpc ny

    switch flag,
    
        %%%%%%%%%%%%%%%%%%
        % Initialization %
        %%%%%%%%%%%%%%%%%%
        case 0,
            error(nargchk(13,13,nargin));

            [nx,nu]=size(B);
            ny=size(C,1);
            
            if ~isequal(size(A), [nx nx]) || ~isequal(size(D),[ny nu])
                error('Model matrix dimension does not match.');
            end
            
            if ~isscalar(P) || ~isscalar(M) || P<=0 || M <= 0 || M>P
                error('P and M must be positive scalars and P>=M.')
            end
            
            if ~isequal(size(Q), [ny*P, ny*P]) || ~isequal(size(R), [nu*M, nu*M])
                error('Weight matrix dimension does not match.')
            end
            
            if ~isscalar(Ts) || Ts<=0
                error('dt must be a positive scalar.');
            end
            
            % MPC set-up
            ssmpc=mpcsetup(A,B,C,D,P,M,Q,R);
            
            sizes = simsizes;
            sizes.NumContStates  = 0;
            sizes.NumDiscStates  = 0;
            %      sizes.NumDiscStates  = nx+ny;
            sizes.NumOutputs     = nu;
            sizes.NumInputs      = 2*ny;  % output + setpoint
            sizes.DirFeedthrough = 1;
            sizes.NumSampleTimes = 1;
            sys = simsizes(sizes);
            str = [];
            x0 = [];
            %       offset=zeros(ny,1);
            %      x0 = zeros(nx+ny,1);
            %       du = zeros(nu,1);
            ts  = [Ts 0];
            
            
            %%%%%%%%%%
            % Update %
            %%%%%%%%%%
        case 2,
            sys = [];
            
            %%%%%%%%%%
            % Output %
            %%%%%%%%%%
        case 3,
            sys = ssmpc(y(1:ny),y(ny+1:2*ny));
            
            %%%%%%%%%%%%%
            % Terminate %
            %%%%%%%%%%%%%
        case 9,
            sys = []; % do nothing
            
            %%%%%%%%%%%%%%%%%%%%
            % Unexpected flags %
            %%%%%%%%%%%%%%%%%%%%
        otherwise
            error(['unhandled flag = ',num2str(flag)]);
    end
end

function f=mpcsetup(A,B,C,D,hP,hM,Q,R,x0,u0)

% Input and output check
error(nargchk(8,10,nargin));
error(nargoutchk(1,1,nargout));

% dimension of the system
nx=size(A,1);
nu=size(D,2);

% precalculate the gain matrices of the online controller
[K1,F,Am,Bm,Cm] = predmat(A,B,C,D,hP,hM,Q,R);

% default initial conditions
if nargin<9
    x0=zeros(nx,1);
end
if nargin<10
    u0=zeros(nu,1);
end
xm=[x0;u0];

% the online controller
f = @ssmpc;
    function u=ssmpc(y,r)
        % update state
        xm = Am*xm + Bm*u0;
        % get the reference
        nr=size(r,2);
        if nr>=hP
            ref=r(:,1:hP);
        else
            ref=[r r(:,end+zeros(hP-nr,1))];
        end
        % model-plant mismatch
        offset = (y(:) - Cm*xm);
        % update optimal control
%         u0 = -K1*x0 + Pr*reshape(bsxfun(@minus,ref,offset),[],1);
        u0 = K1*(reshape(ref-offset(:,ones(hP,1)),[],1) - F*xm);
        % controller output
        u = u0+xm(end-nu+1:end);
    end
end


function [K1,F,Am,Bm,Cm] = predmat(A,B,C,D,hP,hM,Q,R)
 
%%%% Initialise
[nx,nu]=size(B);
ny = size(C,1);
Am = [A B; zeros(nu,nx) eye(nu,nu)];
Bm = [B; eye(nu,nu)];
Cm = [C D];

F = zeros(ny*hP,nx+nu);
F0 = Cm;
for i = 1:hP
    F0 = F0*Am;
    F((i-1)*ny+(1:ny),:) = F0;
end
H = zeros(ny*hP,nu*hM);
for i = 1:hP
    Hj = zeros(ny,nu*hM);
    Hi = Cm;
    for j = min(hM,i):-1:1
        Hj(:,(j-1)*nu+(1:nu)) = Hi*Bm;
        Hi = Hi*Am;
    end
    H((i-1)*ny+(1:ny),:) = Hj;
end
K=(H'*Q*H+R)\(H'*Q);
K1=K(1:nu,:);
end

Contact us