// adaptive iir notch filter 

clear;
rand('normal');

DATLEN=200000;
MU=0.0003;
RR=0.75;
KK=0.995;

in=rand(1:DATLEN)/10+sin(0.000016*(1:DATLEN)^2);  // sweep signal
//in=rand(1:DATLEN)/10+sin(2e-10*(1:DATLEN)^3);  // sweep signal
//in=rand(1:DATLEN)/10+sin(0.000016*(1:DATLEN)^2)+sin(2e-16*(DATLEN:-1:1)^4);  // sweep signal

in=0.99*in/max(abs(in));
out=zeros(1:DATLEN);
wout=zeros(1:DATLEN);

w=0;
zx=0;  zzx=0;
zy=0;  zzy=0;
za=0;  zza=0;
pwr=1;
for i=1:DATLEN
  x=in(i);
  y=x+w*zx+zzx-RR*w*zy-RR*RR*zzy;
  a=zx-RR*zy-RR*w*za-RR*RR*zza;
  pwr=KK*pwr+(1-KK)*x*x;  
  w=w-MU*2*y*a/pwr;                 // with normalize 
  //w=w-MU*2*y*a;                   // without normalize
 
  zzx=zx;  zx=x;
  zzy=zy;  zy=y;
  zza=za;  za=a;
  
  out(i)=y;
  wout(i)=w;
end

scf(0);  clf;  plot(wout);
scf(1);  clf;  plot(acos(-wout/2)/2/%pi);

savewave('in_iir_notch.wav', in, 8000);
savewave('out_iir_notch.wav', 0.99*out/max(abs(out)), 8000);

