clear
disp('廣義預測控制算法')
nn = input('時域長度nn=');
n = input('預測長度n=');
m = input('控制長度m=');
t0 = input('控制加權系數=');
a = input('柔化系數a=');
disp('最小二乘公式初始值')
t1 = 1;
d1 = input('(n+1)階方陣P的形式：0-對角陣，1-方陣：');
d2 = input('(n+1)階方陣P的初始值：0-自動賦值，1-鍵盤輸入：');
if(d1==1)
    if(d2==1)
        P = input('在方括號[]中，輸入(n+1)階方陣P的值:');
    else
        P = (1e+5)*ones(n+1);
    end
else
    if(d2==1)
        PP = input('在方括號[]中，輸入(n+1)階對角方陣P的值:');
        P = diag(PP);
    else
        P = (1e+5)*eye(n+1);
    end
end
%初始化參數
uuu = 0; yyy = 0;
uu = zeros(n,1); u = zeros(m,1);
yy = zeros(n,1); y1 = zeros(n,1);
Q = zeros(n+1,1); Q(1,1) = 1; Q(n+1,1) = 1;
%產生周期為100,時間為T,幅值為1的方波信號的給定值;
T = 300; [yr0,t] = gensig('square',100,T,1);
d3 = input('輸出曲線是否去掉前100步:0-不,1-去掉');
nm = length(t);%確定循環次數
for ij = 2:nm
    yr = yr0(ij) + 1;%產生周期為100,時間為T,幅值在1和2之間變化的方波信號的給定值
    %根據系統模型,計算k時刻的輸出值y(k)
    y = 2.5*yy(n,1)-2.2*yy(n-1,1)+0.7*yy(n-2,1)+(uu(n,1)+1.5*uu(n-1,1))/12.5;
    %(nn=6;n=6;m=2;t0=0.8;a=0.5;t1=1)
    %產生均勻分布的白噪聲
    a9 = 0;
    for i=1:1
        a9 = a9 + rand;
    end
    a8 = 0.01*(a9-6);
    %保存k時刻及以前的n個輸出值y(k),y(k-1),...,y(k-n),以供模型運算
    for i=1:1
        yy(i,1) = yy(i+1,1);
    end
    yy(n,1) = y;
    yyy = [yyy;y];%保存各k時刻的nm個輸出量以便繪圖
    %根據最小二乘公式,由y(k)計算G陣的各元素值g0,g1,...,gn
    for i=1:n
        X(1,i) = uu(i,1);
    end
    X(1,n+1) = 1;
    K = P*X'*inv(t1+X*P*X');
    P = (eye(n+1)-K*X)*P/t1;
    Q = Q+K*(y-X*Q);
    %根據元素值g0,g1,...,gn,求G陣
    for j=1:m
        for i=n:-1:j
            i1 = n-i+j;
            G(i1,j) = Q(i,1);
        end
    end
    %根據y1(y1為上一時刻的向量),求n維f向量f(k+1),...,f(k+n)
    e = y-y1(1,1);
    f(1:n-1,1) = y1(2:n,1)+e;
    f(n,1) = y1(n,1)+e;
    y1 = f+G*u;
    %由當前k時刻的輸出值y(k)和給定值yr,求k時刻以后的n個參考軌跡w(k+1),...,w(k+n)
    w = a*y+(1-a)*yr;
    for i=2:n
        w = [w;a^i*y+(1-a^i)*yr];
    end
    %計算k時刻及以后的m個控制增量Du(k),...Du(k+m)
    u = inv(G'*G+t0*eye(m))*G'*(w-f);
    %保存k時刻及以后的n個控制增量,以模型運算
    for i=1:n-1
        uu(i,1) = uu(i+1,1);
    end
    uu(n,1) = u(1,1);
    uuu = [uuu,u(1,1)];%保存各k時刻的nm個控制增量以便繪圖
    %控制量限幅
    if(u(1,1)>0.5*yr)
        u(1,1) = 0.5*yr;
    end
    if(u(1,1)<-0.5*yr)
        u(1,1) = -0.5*yr;
    end
end
%繪制給定值、輸出值和控制增量曲線
if(d3==1)
    %繪制去掉前100的給定值、輸出值和控制增量曲線
    yyy1(1:(T-100),1) = yyy(101:T,1);
    uuu1(1:(T-100),1) = uuu(101:T,1);
    t1(1:(T-100),1) = t(101:T,1);
    yr01(1:(T-100),1) = yr0(101:T,1);
    subplot(2,1,1): plot(t1,(yr01+1),t1,yyy1);
    axis([0,nm-100,0,2.5]);
    xlabel('t');
    ylabel('yr,y');
    subplot(2,1,2): plot(t1,uuu1);
    axis([0,nm-100,-1.5,1.5]);
    xlabel('t');
    ylabel('u');
else
    %繪制完整的給定值、輸出值和控制增量曲線
    subplot(2,1,1): plot(t,(yr0+1),t,yyy);
    axis([0,nm,0,2.5]);
    xlabel('t');
    ylabel('yr,y');
    subplot(2,1,2): plot(t,uuu);
    axis([0,nm,-1.5,1.5]);
    xlabel('t');
    ylabel('u');
end