%這是一個 使用誤差反饋更新的RLS格型－－－梯型濾波器 源于馬諾來克思 《統計與自適應信號處理》第554頁 表10.17
%r is very important factor to the performance of RLS based algrithms  (0.999<r<1) 
[n1,n2,n3,n4,n5]=dataread5('C:\MATLAB6p5\work\5ch8k1m4fu1cm2.dat',32000);%噪聲；
kk=1000;r=0.99999;
sample_n=32000;M=64;
e=zeros(M,sample_n);
n1=dataread0('C:\MATLAB6p5\work\DAT23NNR.DAT',sample_n,kk);
n2=dataread0('C:\MATLAB6p5\work\DAT23NNP.DAT',sample_n,kk);

for i=2:sample_n
    if max<abs(n2(i))
        max=abs(n2(i));
    end
end
x=n1/max;y=n2/max;
Kc=zeros(M,2);
sum1=0;
for i=1:2000
    sum1=x(i)^2/2000+sum1;
end
sum1=sum1;

% 時間初始化
for m=1:M
Ef(m,1)=sum1;Eb(m,1)=sum1; Kf(m,1)=0;Kb(m,1)=0;
eb(m,1)=0;Kc(m,1)=0;
end

for n=2:sample_n
% 階數初始化
  ef(1,2)=x(n);eb(1,2)=x(n);e(1,n)=y(n);a(1,2)=1; 
%格型部分 
  for m=1:M-1  
     ef(m+1,2)=ef(m,2)+Kf(m,1)*eb(m,1);
     eb(m+1,2)=eb(m,1)+Kb(m,1)*ef(m,2);
     Ef(m,2)=r*Ef(m,1)+a(m,1)*ef(m,2)^2;
     Eb(m,2)=r*Eb(m,1)+a(m,2)*eb(m,2)^2; 
     Kf(m,2)=Kf(m,1)-a(m,1)*eb(m,1)*ef(m+1,2)/Eb(m,1);
     Kb(m,2)=Kb(m,1)-a(m,1)*ef(m,2)*eb(m+1,2)/Ef(m,2);
     a(m+1,2)=a(m,2)-(a(m,2)*eb(m,2))^2/Eb(m,2);
    end
   %梯型部分：  
     for m=1:M-1
         e(m+1,n)=e(m,n)-Kc(m,1)*eb(m,2);
         Kc(m,2)=Kc(m,1)+a(m,2)*eb(m,2)*e(m+1,n)/Eb(m,2);
     end
    for m=1:M
        ef(m,1)=ef(m,2);eb(m,1)=eb(m,2);Ef(m,1)=Ef(m,2);Eb(m,1)=Eb(m,2);Kf(m,1)=Kf(m,2);Kb(m,1)=Kb(m,2);a(m,1)=a(m,2);Kc(m,1)=Kc(m,2);
    end
   n
end
plot(y(1:n))
hold on
plot(e(m,1:n),'g')