clear all
close all
%disp('GPC算法初值')
na=2;nb=2;nc=1
n=5; %預測長度;
nu=2; %控制長度;
t0=0.05; %控制加權系數;
p=0.3; %柔化系數;

y=zeros(n,1);u=zeros(n,1);
yy=zeros(3,1);uu=zeros(3,1);
yr=zeros(n,1);

%disp('系統參數初值')
a=zeros(1,na+1);a(1)=1;
b=zeros(1,nb);
%c=zeros(1,nc);c(1)=1
c=zeros(1,n);c(1)=1;

%產生周期為20s，幅值為1，采樣周期Ts=0.1s的四個周期的方波信號；
T=100;Ts=0.1;
[yr0,t]=gensig('square',20,T,1);
nm=length(t);   %總循環次數

%disp('RLS參數初值')
n1=na+nb;
thita0=ones(n1,1)*0.001;  
thita=zeros(n1,1);
P0=eye(n1,n1)*(1.0e6);
P=zeros(n1,n1);
K=zeros(n1,1);

for k=4:nm
    yr(1:n-1)=yr(2:n);
    yr(n)=p*yr(n-1)+(1-p)*(yr0(k)*1);  %柔化輸入曲線
     y1=1.9*y(n)-0.9*y(n-1)+1*u(n)+2*u(n-1);
    y(1:n-1)=y(2:n);
    y(n)=y1;
    yy=[yy;y1];
   X=[[-y(n-1:-1:n-na)]',[u(n:-1:n-nb+1)]'];  %用RLS辨識系統參數  
   K=P0*X'*inv(X*P0*X'+1);
   P=(eye(n1,n1)-K*X)*P0;
   thita=thita0+K*(y1-X*thita0);
   P0=P;
   thita0=thita;
    
    for i=1:na    %得到系統參數
        a(i+1)=thita(i);
    end
    for i=0:nb-1
        b(nb-i)=thita(n1-i);
    end
   
   f=zeros(1,n);f(1)=1;f1=zeros(1,n);  %遞推求解丟番方程
    g=zeros(1,na+1);g(1:na)=a(2:na+1);
    l=zeros(1,nb);l(1:nb-1)=b(2:nb);
    e=zeros(1,n);e(1)=b(1);e1=zeros(1,n);
    E=zeros(n,n);E(1,:)=e;
    G=zeros(n,na);G(1,:)=g(1:na);
    L=zeros(n,nb-1);L(1,:)=l(1:nb-1);
    rj=g(1);tj=l(1)+b(1)*rj;
    
    for ij=2:n
        f1(ij)=rj;
        if(ij<=nu) 
            e1(2:n)=e(1:n-1);
            e1(1)=tj;
        else
            e1(nu)=e(nu)+e(nu-1);
            e1(2:nu-1)=e(1:nu-2);
            e1(1)=tj;
        end
        g1=g-a*rj;
        g(1:na)=g1(2:na+1);
        G(ij,:)=g(1:na);
        l1=l+b*rj;l1(1)=l1(1)-tj;
        l(1:nb-1)=l1(2:nb);
        L(ij,:)=l(1:nb-1);
        f=f1;
        e=e1;
        E(ij,:)=e;
        rj=g(1);
        tj=l(1)+b(1)*rj;
    end
    y2=flipud(y(n-na+1:n));
    u2=flipud(u(n-nb+2:n));
    y0=G*y2+L*u2;
    d=c*inv(E'*E+t0.*eye(n,n))*E';
    u1=d*(yr-y0);
    u(1:n-1)=u(2:n);
    u(n)=u1;
    uu=[uu;u1];
    if u1>1
        u1=1;
    end
    if u1<-1
        u1=-1;
    end
end

subplot(2,1,1):plot(t,yr0,t,-yy,'r');
%axis([0,nm,0,2.5]);
%xlabel('t');ylabel('yr,y');
subplot(2,1,2):plot(t,uu);
%xlabel('t');ylabel('u');