clear;%清理工作間變量
load ZS1.txt;
load X1.txt;
M=length(ZS1);%數據長度
%u=ZS1+randint(200,1,[-5,5]);
u=ZS1;%輸入數據
z=X1;%輸出數據
figure(1);%第1個圖形
plot(ZS1);%輸入數據顯示
figure(2);
plot(X1);%輸出數據顯示
%RLS遞推最小二乘辨識
L1=30;L2=12;L3=7;%記憶長度
a1=0;%一階核個數
a2=0;%二階核個數
a3=0;%三階核個數
for i=1:1:L1
      a1=a1+1;
      for j=i:1:L2
          a2=a2+1;
          for k=j:1:L3
              a3=a3+1;
          end
      end
end
c_length=a1+a2+a3;%計算總共核項數
c00=ones(c_length,1);          
c0=c00*0.0001;%直接給出被辨識參數的初始值,即一個充分小的實向量
p0=10^6*eye(c_length,c_length);%直接給出初始狀態P0，即一個充分大的實數單位矩陣,其項數為nxn，n為未知參數個數
E=0.000000005;%相對誤差E=0.000000005
c=[c0,zeros(c_length,M-L1)];%被辨識參數矩陣的初始值及大小,行數為辨識核的個數，列數為數據長度
e=zeros(c_length,M-L1+1);%相對誤差的初始值及大小，大小根據c矩陣變化
HL=zeros(M-L1+1,c_length);
for k=L1:M %開始求K 
    h1=zeros(c_length,1); %分三塊寫出h(k)
    i=1:1:a1;
        h1(i,1)=u(k-i+1);
  counter1=0;
  %for i=(a1+1):1:(a1+a2);
    for m=1:L2
        for n=m:L2
            if m==n 
               h1(a1+1+counter1,1)=u(k-m+1)*u(k-n+1);
            else h1(a1+1+counter1,1)=u(k-m+1)*u(k-n+1)*2;
            end
            counter1=counter1+1;
        end
    end
  %end
 counter2=0;
 %for   i=(a2+a1+1):1:c_length;
    for g1=1:L3
        for g2=g1:L3
            for g3=g2:L3
                if (g1==g2)&(g2==g3)&(g3==g1)
                   h1(a1+a2+1+counter2,1)=u(k-g1+1)*u(k-g2+1)*u(k-g3+1);
                else if (g1~=g2)&(g2~=g3)&(g3~=g1)
                        h1(a1+a2+1+counter2,1)=6*u(k-g1+1)*u(k-g2+1)*u(k-g3+1);
                    else h1(a1+a2+1+counter2,1)=3*u(k-g1+1)*u(k-g2+1)*u(k-g3+1);
                    end
                end 
            counter2=counter2+1;
            end
        end 
    end
 %end
  
    HL(k-L1+1,:)=h1';
    x=h1'*p0*h1+1; x1=inv(x); %開始求K(k)
    k1=p0*h1*x1;%求出K的值
    d1=z(k)-h1'*c0; c1=c0+k1*d1;%求被辨識參數c
    e1=c1-c0;%求參數當前值與上一次的值的差值
    e2=e1./c0;%求參數的相對變化
    e(:,k-L1+1)=e2; %把當前相對變化的列向量加入誤差矩陣的最后一列       
    c0=c1;%新獲得的參數作為下一次遞推的舊參數
    c(:,k-L1+1)=c1;%把辨識參數c 列向量加入辨識參數矩陣的最后一列 
    p1=p0-k1*k1'*[h1'*p0*h1+1];%求出 p(k)的值
    p0=p1;%給下次用
    %if e2<=E break;%若參數收斂滿足要求，終止計算
    if all(e2(:))<=E break;
    end%小循環結束
end%大循環結束
c%顯示被辨識參數
e%顯示辨識結果的收斂情況
%分離各不同階參數
 i=1:a1;
    c1_1(i)=c1(i,1);

 i=1:a2;
    c1_2(i)=c1(a1+i,1);

 i=1:a3;
    c1_3(i)=c1(a1+a2+i,1);
 
   % ea1=e(1,:); ea2=e(2,:); eb1=e(3,:); eb2=e(4,:);%分離各階參數誤差 
c1_1;
c1_2;
c1_3;
figure(3);%第2個圖形
plot(c1_1);
figure(4);
plot(c1_2);
figure(5);
plot(c1_3);


%二階三維圖繪制
c1_2rec=zeros(m,n);
counter3=0;
for m=1:L2
    for n=m:L2
        c1_2rec(m,n)=c1_2(1+counter3);
        counter3=counter3+1;
    end
end

for m=1:L2
    for n=m:L2
        c1_2rec(n,m)=c1_2rec(m,n)
    end
end
figure(6);
surf(c1_2rec);

%二階圖對角線繪制
for m=1:L2
    cross1(m)=c1_2rec(m,m);
end
for m=1:L2
    cross2(m)=c1_2rec(m,L2-m+1);
end
figure(7);
subplot(2,1,1);
plot(cross1);
subplot(2,1,2);
plot(cross2);

%利用計算結果重構輸出波形
figure(8);
plot(X1);
hold;
Z_result=HL*c1;
plot(30:200,Z_result(1:171));