// adaptive notch filter demo program    http://www.cepstrum.co.jp/

clear;
rand('normal');

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

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

in=0.9*in/max(abs(in));
out=zeros(1:DATLEN);
wbuf=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;
  w=w-MU*2*y*a/pwr;  
  pwr=KK*pwr+(1-KK)*x*x;
  zzx=zx;  zx=x;
  zzy=zy;  zy=y;
  zza=za;  za=a;
  out(i)=y;
  wbuf(i)=w;
end
scf(0);  clf;  plot(wbuf);
scf(1);  clf;  plot(acos(-wbuf/2)/2/%pi);
savewave('in.wav', in, 8000);
savewave('out.wav', out, 8000);