gusucode.com > 一个简单的协同自适应巡航控制的MPC控制器设计源码程序 > genQP.m

    %此函数用于将有约束预测控制问题转化为标准QP问题
%输入：A-系统阵，B-控制阵，C-输出阵，YW-输出误差加权阵，UW-输入加权阵，
%p-预测时域，m-控制时域
%输出：H-二次项矩阵，f-计算标准型中F的系数阵，c-输出不等式约束的系数阵
function [H,f,YC]=genQP(A,B,C,YW,UW,m,p)
%状态加权阵
Q=C'*YW*C;
szb=size(B);
szp=size(B,1);
ba=B;
base=zeros(m*szb);
% %无穷预测时域时求解终端状态加权阵P
if p==Inf
    P=dlyap(A',C'*YW'*YW*C);
else
     P=zeros(szp,szp);
end
%生成下三角阵[B;AB B;...;A^(m-1)B ... B]
for i=1:m
    base(((i-1)*szb(1)+1):i*szb(1),1:i*szb(2))=ba;
    ba=[A*ba B];
end
UWI=UW;
C1=C;
CI=C;
CII=C;
Q1=Q;
QW=Q1;
Y1=YW;
YWI=Y1;
for i=2:m
    %生成对角阵diag(UW,...,UW);
    UWI=[UWI zeros([size(UWI,1) size(UW,2)]);zeros([size(UW,1) size(UWI,2)]) UW];
    if i==m
        Q1=P;
        Y1=P;
        C1=eye(size(A));
    end
    %生成对角阵diag(Q,...,Q,P);
    QW=[QW zeros([size(QW,1) size(Q1,2)]);zeros([size(Q1,1) size(QW,2)]) Q1];
    %生成对角阵diag(YW,...,YW,P);
    YWI=[YWI zeros([size(YWI,1) size(Y1,2)]);zeros([size(Y1,1) size(YWI,2)]) Y1];
    %生成对角阵diag(C,...,C);
    CI=[CI zeros([size(CI,1) size(C,2)]);zeros([size(C,1) size(CI,2)]) C];
    %生成对角阵diag(C,...,C,I);
    CII=[CII zeros([size(CII,1) size(C1,2)]);zeros([size(C1,1) size(CII,2)]) C1];
end
%生成二次项矩阵H
H=base'*QW*base+UWI;
%生成用于计算F的系数阵
f=YWI*(CII*base);
%生成用于计算输出不等式约束的系数阵
YC=CI*base;