clear;
mode=input('請輸入模式（1為定常，2為自校正）：');
na=2;nb=2;
A=[1,zeros(1,na)];
B=zeros(1,nb);
A=fliplr(A);
B=fliplr(B);
yr=1;
arfa=0.2;
yK=0;
y0=0;y1=0;u0=0;u1=0;
y=[zeros(1,na),0]';u=zeros(1,nb)';
k=1;
epoch=50;



if mode==1
N=input('請輸入N:');
Nu=input('請輸入Nu:');
lamda=input('請輸入lamda:');

A=[0.7,-1.5,1];
B=[0.5,1];
ta=[-1,1];                    %求G
A1=conv(A,ta);
E=[1];
EE=[zeros(1,N-1),E];
F=-A1(1:end-1);
FF=F;
EB=conv(E,B);
G=[EB(end),zeros(1,Nu-1)];
H=EB(1:end-1);
for j=2:1:N
    zj=[1,zeros(1,j-1)];
    e=F(end);
    E=polyadd(E,e*zj);
    f=polyadd(F,-e*A1);
    F=f(1:end-1);
    EE(j,:)=[zeros(1,N-j),E];
    FF(j,:)=F;
    EB=conv(E,B);
    if j<=Nu
        G(j,:)=[EB(end-j+1:end),zeros(1,Nu-j)];  
    else
        G(j,:)=EB(end-j+1:end-j+Nu);
    end
    H(j,:)=EB(1:end-j);
end
G=inv(G'*G+lamda*eye(Nu,Nu))*G';%%%%%%%%%%%%%



for i=1:k:epoch                             %求U
    kesi=-0.05+0.1*rand(1);
    yk=(-A)*y+B*u+kesi;
    yy=[y(1:na)',yk]';
    for l=1:1:(na-1)
        y(l)=y(l+1);
    end
    y(na)=yk;
    yd=yk;
    yK(i)=yk;
    for j=1:1:N
        yd=arfa*yd+(1-arfa)*yr;
        ydk(j)=yd;
        y0=FF(j,:)*yy+H(j,:)*(u(2)-u(1));
        y0k(j)=y0;
    end
    drtu=G*(ydk-y0k)';
    for m=1:1:nb-1
        u(m)=u(m+1);
    end
    u(nb)=drtu(1)+u(nb);%%%%%%%%%%%%%%%%%%%%%%%%
    uu(i)=u(nb);
end
i=1:k:epoch;                     %畫圖
subplot(2,1,1);
plot(i,yr,'r',i,yK,'g');
xlabel('k');
ylabel('y(k)');
subplot(2,1,2);
plot(i,uu);
xlabel('k');
ylabel('u(k)');
%%%%%%%%%%%%%%%%%%%
 
 
elseif mode==2

N=input('請輸入N:');
Nu=input('請輸入Nu:');
lamda=input('請輸入lamda:');

A=[eye(1,na)/1000,1];
B=[eye(1,nb)/1000];
c0=[A(1:2),B]';
p0=10^6*eye(na+nb,na+nb);

for i=1:k:epoch
    kesi=-0.05+0.1*rand(1);
    yk=(-[0.7,-1.5,1])*y+[0.5,1]*u+kesi;%采樣
    yy=[y(1:na)',yk]';
    for l=1:1:(na-1)
        y(l)=y(l+1);
    end
    y(na)=yk;
    yd=yk;
    yK(i)=yk;
 
     h1=[-yy(1:2)',u']'; %辨識
     x=h1'*p0*h1+1; 
     x1=inv(x);
     k1=p0*h1*x1;
     d1=yk-h1'*c0; 
     c1=c0+k1*d1;
     c0=c1;
     p1=p0-k1*k1'*[h1'*p0*h1+1];
     p0=p1;
     A=[c1(1:2)',1];B=c1(3:4)';%%%%%%%%%%%%%%%%%%%%%%%%%%

    ta=[-1,1];                        %求G
    A1=conv(A,ta);
    E=[1];
    EE=[zeros(1,N-1),E];
    F=-A1(1:end-1);
    FF=F;
    EB=conv(E,B);
    G=[EB(end),zeros(1,Nu-1)];
    H=EB(1:end-1);
 for j=2:1:N
     zj=[1,zeros(1,j-1)];
     e=F(end);
     E=polyadd(E,e*zj);
     f=polyadd(F,-e*A1);
     F=f(1:end-1);
     EE(j,:)=[zeros(1,N-j),E];
     FF(j,:)=F;
     EB=conv(E,B);
     if j<=Nu
        G(j,:)=[EB(end-j+1:end),zeros(1,Nu-j)];  
     else
        G(j,:)=EB(end-j+1:end-j+Nu);
     end
     H(j,:)=EB(1:end-j);
 end
G=inv(G'*G+lamda*eye(Nu,Nu))*G';%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


    for j=1:1:N                              %求U
        yd=arfa*yd+(1-arfa)*yr;
        ydk(j)=yd;
        y0=FF(j,:)*yy+H(j,:)*(u(2)-u(1));
        y0k(j)=y0;
    end
    drtu=G*(ydk-y0k)';
    for m=1:1:nb-1
        u(m)=u(m+1);
    end
    u(nb)=drtu(1)+u(nb);
    uu(i)=u(nb);
end                                         %%%%%%%%%%%%%%%%%%%%%%


i=1:k:epoch;                     %畫圖
subplot(2,1,1);
plot(i,yr,'r',i,yK,'g');
xlabel('k');
ylabel('y(k)');
subplot(2,1,2);
plot(i,uu);
xlabel('k');
ylabel('u(k)');
%%%%%%%%%%%%%%%%%%%
end