Contents

function MPC = mpc_matrices(S, P, x, D, k, ops)

Construct the MPC matrices for a DWN System

The function mpc_matrices prepares the MPC matrices necessary for the formulation of an MPC problem as a constrained conved QP problem.

Syntax: MPC = mpc_matrices(S,P,x,D,k);

MPC_MATRICES admits the following input arguments, all of which are mandatory:

S - A structure that describes the discrete-time dynamical system

$$x_{k+i+1\mid l} = Ax_{k+i\mid k}+Bu_{k+i\mid k}+G_d d_{k+i\mid k}$$

where x is the nx-dimensional state of the system, u is the nu-dimensional input vector and d is the nd-dimensional vector of disturbances.

Additionally, we consider that the input variable u and the distrurbance d are coupled through the equality constraint

$$Eu_{k+i\mid k}+E_d d_{k+i\mid k}=0$$

where E and Ed and matrices of appropriate dimensions. For more information about the structure of S, type help DWN_INFO.

P - A structure with information about the MPC problem. Among other, P has the following entries:

$$U_k = \left[ \begin{array}{c} u_k\\ u_{k+1\mid k}\\ \vdots\\ u_{k+H_u\mid k}\\ u_{k+H_u+1\mid k}\\ \vdots\\ u_{k+H_p-1\mid k}\end{array}\right]$$

where

$$u_{k+Hu+i}=u_{k+Hu},\ \forall i$$

That is, after the time instant k+Hu we assume that the same constant input value is applied to the system up to the time instant k+Hp-1.

For simplicity, we consider only the part of U_k up to u_{k+Hu}, knowning that all trailing input values are equal to u_{k+Hu}.

x - is the current state (measured).

D - is a vector of demands, vertically packed, i.e.,

$$D_k = \left[ \begin{array}{c} d_k\\ d_{k+1\mid k}\\ \vdots\\ d_{k+H_p-1\mid k}\end{array}\right]$$

where d_k is the current measured disturbance while d_{k+i} for i=1,...,Hp-1 are forecast disturbances using some model. The length of the vector D should be S.nd*P.Hp.

k - is the time instant when the measurement x is taken. This is important to be specified as the overall MPC problem is time-varying.

ops - Options for this function.

mpc_matrices will return a structure with the following fields: Sx, Su, Sd - The matrices of the batch MPC formulation. Let

$$ X_k=\left[ \begin{array}{c} x_{k+1\mid k}\\ x_{k+2\mid k}\\ \vdots\\ x_{k+H_p\mid k}\end{array}\right] $$

and U_k and D_k be as above, then:

$$ X_k = S^x x_k + S^u U_k + S^d D_k $$

H, f - The matrices of the cost function:

$$J(y)=y'Hy + f'y$$

where

$$y=\left[\begin{array}{c}U_k\\ \Xi_k\end{array}\right]$$

F, phi - The set of constraints of the MPC problem can be written compactly in the form

$$Fy \leq \phi$$

where y is as above.

G, gamma - The set of equality constraints of the form

$$Eu_{k+i}+E_dd_{k+i}=0$$

can be compactly written in the form G*y=gamma, where y is as above.

yL, yH - Box-type constraints for the vector y, i.e.,

$$y^L \leq y \leq y^H$$

Handling of soft constraints: A softened version of the constraint x>=xs is used, where xs is a field specified in P.

If xs is not provided, but P.beta is available, then P.xs is calculated as

P.xs = P.beta * S.xmax
defaultops = struct('use_soft_constraints',true,'use_beta',false);
if (nargin==6)
    options=ops;
elseif (nargin==5)
    options=defaultops;
end
if (~isfield(S,'nx'))
    S.nx=size(S.A,1);
end
if (~isfield(S,'nu'))
    S.nu=size(S.B,2);
end
if (~isfield(S,'nd'))
    S.nd=size(S.Gd,2);
end
if (isempty(S)||isempty(P)||isempty(D)||isempty(x)||isempty(k))
    error('Empty input arguments are not allowed.');
end
[checkS, errorMessage] = check_system(S);
if (~checkS)
    error('System structure error: %s',errorMessage);
end
[checkP, errorMessage] = check_problem(P);
if (~checkP)
    error('Problem structure error: %s',errorMessage);
end

Calculate Delta

$$ \tilde{\Delta}=\left[ \begin{array}{ccccc} -I_{n_u}&I_{n_u}\\ &-I_{n_u}&I_{n_u}\\ &&\ddots&\ddots\\ &&&-I_{n_u}&I_{n_u}\\ \end{array}\right] $$

Delta=zeros(S.nu*P.Hu, S.nu*(P.Hu+1));
Delta(sub2ind([S.nu*P.Hu, S.nu*(P.Hu+1)],1:S.nu*P.Hu,1:S.nu*P.Hu))=-1;
for i=1:P.Hu*S.nu,
    Delta(i,i+S.nu)=1;
end
MPC.Delta=Delta;

Caclulate Sx

$$ S^x=\left[ \begin{array}{c}A\\A^2\\ \vdots\\ A^{H_p-1}\end{array}\right] $$

MPC.Sx=zeros(S.nx*P.Hp,S.nx);
MPC.Sx(1:S.nx,:)=S.A;
for i=2:P.Hp
    MPC.Sx(1+(i-1)*S.nx:i*S.nx,:)=MPC.Sx(1+(i-2)*S.nx:(i-1)*S.nx,:)*S.A;
end

Calculate Su

$$ S^u=\left[ \begin{array}{ccccc} B\\ AB & B\\ \vdots&&\ddots\\ A^{H_u}B& A^{H_u-1}B & \cdots & B\\ \\ A^{H_u+1}B& A^{H_u}B & \cdots & B+AB\\ \vdots&\vdots&&\vdots\\ A^{H_p-1}B& A^{H_p-2}B & \cdots & B+AB+\ldots+A^{H_p-H_u-1}B \end{array} \right] $$

MPC.Su=zeros(S.nx*P.Hu,S.nu*(P.Hu+1));
MPC.Su(1:S.nx,1:S.nu)=S.B;
for i=1:P.Hp-1,
    MPC.Su(i*S.nx+1:(i+1)*S.nx, 1:S.nu) = S.A * MPC.Su((i-1)*S.nx+1:i*S.nx, 1:S.nu);
end
for di=1:P.Hu,
    block = MPC.Su((di-1)*S.nx+1:di*S.nx, 1:S.nu);
    for i=1:P.Hu-di+2
        MPC.Su((i+di-2)*S.nx+1:(i+di-1)*S.nx,(i-1)*S.nu+1:i*S.nu)=block;
    end
end
if (P.Hu<P.Hp-1)
    for i=1:P.Hp-P.Hu-1,
        for s=1:P.Hu,
            MPC.Su((P.Hu+i)*S.nx+1:(P.Hu+i+1)*S.nx,s*S.nu+1:(1+s)*S.nu)=...
                MPC.Su((P.Hu+i-1)*S.nx+1:(P.Hu+i)*S.nx,(s-1)*S.nu+1:s*S.nu);
        end
    end
    Mtemp=S.B;
    for i=1:P.Hp-P.Hu-1
        MPC.Su((P.Hu+i)*S.nx+1:(P.Hu+i+1)*S.nx, end-S.nu+1:end)=...
            MPC.Su((P.Hu+i)*S.nx+1:(P.Hu+i+1)*S.nx, end-S.nu+1:end)+Mtemp;
        Mtemp = Mtemp + MPC.Su(i*S.nx+1: (i+1)*S.nx, 1:S.nu);
    end
end

Calculate Sd

$$ S^d=\left[ \begin{array}{ccccc} G_d\\ AG_d & G_d\\ \vdots&&\ddots\\ A^{H_p-1}G_d& A^{H_p-2}G_d & \cdots & G_d \end{array} \right] $$

MPC.Sd=zeros(S.nx*P.Hp, S.nd*P.Hp);
MPC.Sd(1:S.nx,1:S.nd)=S.Gd;
for i=1:P.Hp-1,
    MPC.Sd(i*S.nx+1:(i+1)*S.nx, 1:S.nd) = S.A * MPC.Sd((i-1)*S.nx+1:i*S.nx, 1:S.nd);
end
for di=1:P.Hp,
    block = MPC.Sd((di-1)*S.nx+1:di*S.nx, 1:S.nd);
    for i=1:P.Hp-di+1
        MPC.Sd((i+di-2)*S.nx+1:(i+di-1)*S.nx,(i-1)*S.nd+1:i*S.nd)=block;
    end
end

Calculate Wat, Wxt and Wut

Here, we calculate the following matrices:

$$ \tilde{W}_\alpha=W_\alpha\left(a_1\otimes \mathbf{1}_{H_u+1}'+ \left[ \begin{array}{c} \mathbf{a}_{2,k}\\ \mathbf{a}_{2,k+1}\\ \vdots\\ \mathbf{a}_{2,k+H_u} \end{array}\right]\right) $$

$$\tilde{W}_x=I_{H_p}\otimes W_x$$

and

$$\tilde{W}_u=\tilde{\Delta}'(I_{H_u}\otimes W_u)\tilde{\Delta}$$

MPC.Wat = kron(ones(P.Hu+1,1),P.alpha1) + vec(P.alpha2(k:k+P.Hu,:)');
MPC.Wxt = kron(eye(P.Hp),P.Wx);
MPC.Wut = Delta'*kron(eye(P.Hu), P.Wu)*Delta;

Cost Function Matrices

We calculate the matrices:

$$ H=2\mathrm{blockdiag}\ \{ \tilde{W}_u,\tilde{W}_x\} $$

and

$$ f=\left[ \begin{array}{cc} \tilde{W}_\alpha' & 0_{n_x H_p}'\end{array}\right]' $$

MPC.H = 2*blkdiag(MPC.Wut, MPC.Wxt);
MPC.f = [MPC.Wat; zeros(S.nx*P.Hp,1)];

Calculate the matrices of the inequality constraints

MPC.Xmin=kron(ones(P.Hp,1), S.xmin);
MPC.Xmax=kron(ones(P.Hp,1), S.xmax);
MPC.Umin=kron(ones(P.Hu+1,1), S.umin);
MPC.Umax=kron(ones(P.Hu+1,1), S.umax);
if (isfield(P,'xs') && ~options.use_beta)
    MPC.Xs=kron(ones(P.Hp,1), P.xs);
elseif (isfield(P,'beta'))
    MPC.Xs=P.beta*MPC.Xmax;
end

Upper and Lower Bounds

MPC.ymin = [MPC.Umin; zeros(S.nx*P.Hp,1)];
MPC.ymax = [MPC.Umax; Inf(S.nx*P.Hp,1)];

Equality Constraints

ne = size(S.E,1);
MPC.G = zeros(ne*(P.Hu+1),S.nu*(P.Hu+1)+S.nx*P.Hp);
MPC.G(1:ne*(P.Hu+1), 1:S.nu*(P.Hu+1))=kron(eye(P.Hu+1), S.E);
MPC.gamma = kron(-eye(P.Hu+1), S.Ed)*D(1:S.nd*(P.Hu+1));

% construct the matrix F:
MPC.F = zeros(3*S.nx*P.Hp, S.nx*P.Hp+S.nu*(P.Hu+1));
MPC.F(1:S.nx*P.Hp,1:S.nu*(P.Hu+1))=MPC.Su;
MPC.F(S.nx*P.Hp+1:2*S.nx*P.Hp,1:S.nu*(P.Hu+1))=-MPC.Su;
if (options.use_soft_constraints), % soft constraints here
    MPC.F(2*S.nx*P.Hp+1:3*S.nx*P.Hp,1:S.nu*(P.Hu+1))=-MPC.Su;
    MPC.F(2*S.nx*P.Hp+1:3*S.nx*P.Hp,S.nu*(P.Hu+1)+1:end)=-eye(S.nx*P.Hp);
end

% construct the vector phi:
MPC.phi = [ +MPC.Xmax - MPC.Sx*x - MPC.Sd*D
    -MPC.Xmin + MPC.Sx*x + MPC.Sd*D];
if (options.use_soft_constraints),
    MPC.phi=[ MPC.phi;
        -MPC.Xs + MPC.Sx*x + MPC.Sd*D];
else
    MPC.phi=[ MPC.phi;
        zeros(size(MPC.Xs))];
end

Auxiliary Functions

    function [checkS, errorMessage] = check_system(S)
        %CHECK_SYSTEM checks whether S is a valid system structure.
        checkS=true;
        errorMessage = [];
        fields = { 'A','B','Gd', 'xmin', 'xmax', 'umin', 'umax', 'E', 'Ed'};
        for j=1:length(fields)
            field_i = fields{j};
            if (~isfield(S,field_i))
                checkS=false;
                errorMessage = ['Element ' field_i ...
                    ' is missing from the system struct.'];
                return;
            end
        end
        if (size(S.A,1)~=size(S.B,1))
            checkS=false;
            errorMessage = 'Sizes of A and B are inconsistent.';
            return;
        end
        if (size(S.A,1)~=size(S.Gd,1))
            checkS=false;
            errorMessage = 'Sizes of A and Gd are inconsistent.';
            return;
        end
        if (size(S.E,1)~=size(S.Ed,1))
            checkS=false;
            errorMessage = 'Sizes of E and Ed are inconsistent.';
            return;
        end
        if (sum(size(S.xmin)==[S.nx 1])~=2)
            checkS=false;
            errorMessage = 'xmin has wrong size - should be a nx-by-1 vector.';
            return;
        end
        if (sum(size(S.xmax)==[size(S.A,1) 1])~=2)
            checkS=false;
            errorMessage = 'xmin has wrong size - should be a nx-by-1 vector.';
            return;
        end
        if (sum(size(S.umin)==[size(S.B,2) 1])~=2)
            checkS=false;
            errorMessage = 'xmin has wrong size - should be a nx-by-1 vector.';
            return;
        end
        if (sum(size(S.umax)==[size(S.B,2) 1])~=2)
            checkS=false;
            errorMessage = 'xmin has wrong size - should be a nx-by-1 vector.';
            return;
        end
    end

    function [checkP, errorMessage] = check_problem(P)
        %CHECK_PROBLEM checks whether P is a valid problem structure.
        checkP=true;
        errorMessage = [];
        fields = { 'Hu','Hp', 'beta', 'alpha1', 'alpha2', 'Wx', 'Wu'};
        for j=1:length(fields)
            field_i = fields{j};
            if (~isfield(P,field_i))
                checkP=false;
                errorMessage = ['Element ' field_i ...
                    ' is missing from the system struct.'];
                return;
            end
        end
        if (P.Hu>=P.Hp)
            checkP=false;
            errorMessage='Control Horizon must be less than the prediction horizon.';
            return;
        end
        if (P.Hu==0)
            checkP=false;
            errorMessage='The control horizon should be at least equal to 1.';
        end
    end


    function x = vec(M)
        %VEC packs the elements of a matrix into a vector.
        N = numel(M);
        x = reshape(M,N,1);
    end

end


Published with MATLAB® R2012b