gpc.m

generalized predictive control

function [ys,y,ym,u,du,NoiseVariance,G,A,B]=gpc(NU,NY,plant,delayp,model,delaym,par,alfa,gama,q,T);
set(plant,'iodelay',delayp);
set(model,'iodelay',delaym);

M=par(1); n1=par(2); n2=par(3);f=par(4); Ts=par(5); sw=par(6);
Noise=par(7);
Disturbance=par(8);
P=n2-n1+1;

plantd=c2d(plant,Ts,'zoh');
modeld=c2d(model,Ts,'zoh');

delp=plantd.iodelay;
delm=modeld.iodelay;

for i=1:NY;
    for j=1:NU,
        B=plantd.num{i,j};
        A=plantd.den{i,j};
        Bp(i,j,1:length(B)+delp(i,j))=[zeros(1,delp(i,j)) B];
        Ap(i,j,1:length(B)+delp(i,j))=[A zeros(1,delp(i,j))];
        nbp(i,j)=length(Bp);
        nap(i,j)=length(Ap);
    end,
end,

for i=1:NY;
    for j=1:NU,
        B=modeld.num{i,j};
        A=modeld.den{i,j};
        Bm(i,j,1:length(B)+delm(i,j))=[zeros(1,delm(i,j)) B];
        Am(i,j,1:length(B)+delm(i,j))=[A zeros(1,delm(i,j))];
        nbm(i,j)=length(Bm);
        nam(i,j)=length(Am);
    end,
end,

switch sw,  
    case 0,  
        for i=1:NY,
            ys(i,:)=zeros(T,1);
        end,


    case 1,
        for i=1:NY,
            ys(i,:)=[zeros(1,(i-1)*fix(0.5*T/NY)) ones(1,fix(0.5*T)) zeros(1,fix(0.5*T)-(i-1)*fix(0.5*T/NY))];
        end,
    case 2,
        for i=1:NY,
            ys(i,:)=sin(2*pi*0.001*f*(0:Ts:Ts*(T-1))/Ts+(i-1)*pi/NY);
        end,
    case 3,
        for i=1:NY,
            ys(i,:)=square(2*pi*0.001*f*(0:Ts:Ts*(T-1))/Ts+(i-1)*pi/NY);
        end,
end,

    %----------------------------------------------------------------------
switch Disturbance,
    case 0,  
        for i=1:NY,
            Dis(i,:)=zeros(1,T);
        end,

    case 1,
        for i=1:NY,
            Dis(i,:)=[zeros(1,(i-1)*fix(0.5*T/NY)+1) ones(1,fix(0.5*T)+1) zeros(1,fix(0.5*T)-(i-1)*fix(0.5*T/NY)+1)];
%             Dis(i,:)=[zeros(1,fix(0.5*T/NY)+1) ones(1,fix(0.5*T)+1) zeros(1,fix(0.5*T)-fix(0.5*T/NY)+1)];
        end,
    case 2,
        for i=1:NY,
            Dis(i,:)=sin(2*pi*0.007*(0:Ts:Ts*(T-1))/Ts+(i-1)*pi/NY);%.002 baraie f
        end,
    case 3,
        for i=1:NY,
            Dis(i,:)=square(2*pi*0.006*(0:Ts:Ts*(T))/Ts+(i-1)*pi/NY);%.002 baraie f
        end,
end,
switch Noise,
    case 0,      
        for i=1:NY,
            noise(i,:)=zeros(1,T+1);
        end, 
    case 1,
        for k=1:T+1,
            for i=1:NY,
                noise(i,k)=.1*rand(1);
            end,
        end,
        for i=1:NY,
            noise(i,:)=noise(i,:)-mean(noise(i,:));
        end,
    case 2,  
        for k=1:T+1,
            for i=1:NY,
                noise(i,k)=.3*rand(1);
            end,
        end,
        for i=1:NY,
            noise(i,:)=noise(i,:)-mean(noise(i,:));
        end,
        for k=1:T+1,
            for i=1:NY,
                if(k>1)
                    noise(i,k)=.5*noise(i,k-1)+.5*noise(i,k);
                else
                    noise(i,k)=.5*noise(i,k);
                end,
            end,
        end,
end,

    %----------------------------------------------------------------------

for i=1:NY,
    for j=1:NU,
        g(i,j,1:T)=filter(Bm(i,j,:),Am(i,j,:),ones(T,1));
    end,
end,

% Constructs dynamic matrix of the system
for i=1:NY,
    for j=1:NU,
        gr(1:M)=zeros(1,M);
        if n1 <= M,
            gr(1:n1)=g(i,j,n1+1:-1:2);
        else,
            gr(1:M)=g(i,j,n1+1:-1:n1+2-M);
        end,
        gc(1:n2-n1+1)=g(i,j,n1+1:n2+1);
        G((i-1)*P+1:i*P,(j-1)*M+1:j*M)=toeplitz(gc,gr);
        gdc(i,j)=g(i,j,size(g,3));
    end,
end,

% Calculate KGPC
for k=1:n2-n1+1,
    Q(1+NY*(k-1):NY*k,1+NY*(k-1):NY*k)=q;
end,

for k=1:M,
    rr(1+NY*(k-1):NY*k,1+NU*(k-1):NU*k)=gdc*gama;
end,
R=rr'*rr;
%R=eye(NU*M);
KGPC=inv(G'*Q'*Q*G+R)*G'*Q';
up=zeros(NU,1);
u(1:NU,1)=zeros(NU,1);
y(1:NY,1)=zeros(NY,1);
ym(1:NY,1)=zeros(NY,1);
ymt(1:NY,1:NU,1)=zeros(NY,NU,1);
dd(1:NY,1)=zeros(NY,1);

for k=1:T-1,
    if k > 1,
        up=u(:,k-1);
    end
    for i=1:NY,
        ydd(i,k)=y(i,k);
        for l=1:n2,
            ydd(i,k+l)=alfa(i,i)*ydd(i,k+l-1)+(1-alfa(i,i))*ys(i,k);
        end,
        yd((i-1)*(n2-n1+1)+1:i*(n2-n1+1),1)=ydd(i,k+n1:k+n2)';
    end,
  
    for i=1:NY,
        for j=1:NU,
            for m=1:n2,
                ymt(i,j,k+m)=0;
                for n=2:nam(i,j),
                    if k+m-n+1 > 0,
                        ymt(i,j,k+m)=ymt(i,j,k+m)-Am(i,j,n)*ymt(i,j,k+m-n+1);
                    end,
                end,
                for n=2:nbm(i,j),
                    if k+m-n+1 > 0,
                        if m-n+1 < 0,
                            ymt(i,j,k+m)=ymt(i,j,k+m)+Bm(i,j,n)*u(j,k+m-n+1);
                        else,
                            ymt(i,j,k+m)=ymt(i,j,k+m)+Bm(i,j,n)*up(j,1);
                        end,
                    end,
                end,
            end,
        end,
        for m=1:n2,
            ypastt(i,k+m)=0;
            for j=1:NU,
                ypastt(i,k+m)=ypastt(i,k+m)+ymt(i,j,k+m);
            end,
        end,
    end,
    
    for i=1:NY,
        ypast((i-1)*(n2-n1+1)+1:i*(n2-n1+1),1)=ypastt(i,k+n1:k+n2)';
    end,
    
    for i=1:NY,
        d((i-1)*(n2-n1+1)+1:i*(n2-n1+1),1)=dd(i,k)*ones(n2-n1+1,1);
    end,
   
    duu=KGPC*(yd-d-ypast);
    
    for j=1:NU,
        du(j,k)=duu((j-1)*M+1,1);
    end,

    u(1:NU,k)=up+du(:,k);
    
    for i=1:NY,
        for j=1:NU,
            yt(i,j,k+1)=0;
            for n=2:nap(i,j),
                if k+2-n > 0,
                    yt(i,j,k+1)=yt(i,j,k+1)-Ap(i,j,n)*yt(i,j,k+2-n);
                end,
            end,
            for n=2:nbp(i,j),
                if k+2-n > 0,
                    yt(i,j,k+1)=yt(i,j,k+1)+Bp(i,j,n)*u(j,k+2-n);
                end,
            end,
        end,
        y(i,k+1)=0;
        for j=1:NU,
            y(i,k+1)=y(i,k+1)+yt(i,j,k+1)+Dis(i,k+1)+noise(i,k+1);
        end,
    end,
          
    for i=1:NY,
        for j=1:NU,
            ymt(i,j,k+1)=0;
            for n=2:nam(i,j),
                if k+2-n > 0,
                    ymt(i,j,k+1)=ymt(i,j,k+1)-Am(i,j,n)*ymt(i,j,k+2-n);
                end,
            end,
            for n=2:nbm(i,j),
                if k+2-n > 0,
                    ymt(i,j,k+1)=ymt(i,j,k+1)+Bm(i,j,n)*u(j,k+2-n);
                end,
            end,
        end,
        ym(i,k+1)=0;
        for j=1:NU,
            ym(i,k+1)=ym(i,k+1)+ymt(i,j,k+1);
        end,
    end,
        
    for i=1:NY,
        dd(i,k+1)=y(i,k+1)-ym(i,k+1);
    end,
end,

for j=1:NU,
    u(j,T)=u(j,T-1);
    du(j,T)=du(j,T-1);
end,
if(par(7)==0)
    NoiseVariance=zeros(2,1);
else
    NoiseVariance=[std(noise(1,:)) std(noise(2,:))];
end
NN=max(NY,NU);
figure(1)

for i=1:NY,
    subplot(4,NN,i), plot((1:T)*Ts,y(i,:),'g',(1:T)*Ts,ys(i,:),'r'), title(['y',num2str(i),'     ys',num2str(i)]), grid
end,
for i=1:NY,
    subplot(4,NN,NN+i), plot((1:T)*Ts,ym(i,1:T),'g'), ylabel('ym'), grid
end,
for j=1:NU,
    subplot(4,NN,2*NN+j), plot((1:T)*Ts,u(j,:),'r'), title(['u',num2str(j)]), grid
end,
for j=1:NU,
    subplot(4,NN,3*NN+j), plot((1:T)*Ts,du(j,:),'g'), ylabel('du'), grid
end,
% for i=1:NY,
%      subplot(1,NN,i), plot((1:T)*Ts,y(i,:),'g',(1:T)*Ts,ys(i,:),'r'), title(['y',num2str(i),'     ys',num2str(i)]), grid
%  end,
%  figure(2)
%  for j=1:NU,
%      subplot(1,NN,j), plot((1:T)*Ts,u(j,:),'r'), title(['u',num2str(j)]), grid
%  end,
%