notch1.m

自己编写的自适应notch fileter算法

t =0:1/500:4;
d = 0.1*sin(2*pi*t);
subplot(331)
q=length(t);
plot(t,d);
axis([0,t(q),-0.3,0.3]);

title('噪声源信号曲线图');
d=d';

rnd1 = normrnd(0,0.051,[q 1]); 
subplot(332)
plot(t,rnd1);
axis([0,t(q),-0.3,0.3]);
title('干扰信号曲线图');


s = d + rnd1;
subplot(333)
plot(t,s)
axis([0,t(q),-0.3,0.3]);
title('存在干扰的噪声源信号曲线图');

n = zeros(q,1);
kk0 = zeros(q,1);
kk1 = zeros(q,1);


r1 = 0;
r0 = 0;
k1 = 0;
k0 = 0;
namda = 0.999;
a = 0.9995;

c0 = 0.000071;
d0 = 0.000071;
c1 = 0.000071;
d1 = 0.000081;

for i=1:1:q

    if i == 1
        n(1) = s(1);
    end
    if i == 2
        n(i) = s(i)+k0*(1+k1)*s(i-1)+0-k0*(1+a*k1)*n(i-1)-0;
    end
    if i>2
        n(i) = s(i)+k0*(1+k1)*s(i-1)+k1*s(i-2)-k0*(1+a*k1)*n(i-1)-a*k1*n(i-2);
    end
    
    s0 = s(i)-n(i);
    
    c0 = namda*c0+s0*r0;
    d0 = namda*d0+0.5*s0*s0+0.5*r0*r0;
    k0 = -c0/d0;
    kk0(i) = k0;
    
    s1 = s0+k0*r0;
    c1 = namda*c1+s1*r1;
    d1 = namda*d1+0.5*s1*s1+0.5*r1*r1;
    k1 = -c1/d1;
    kk1(i) = k1;
 
    r1 = s0*k0+r0;

    r0 = s0;
    
end
out = s-n;
subplot(334)
plot(t,out);
axis([0,t(q),-0.3,0.3]);
title('输出信号S曲线图');

subplot(335)
plot(t,n);
axis([0,t(q),-0.3,0.3]);
title('陷波器输出信号曲线图');

subplot(336)
plot(t,kk0);
axis([0,t(q),-1,1]);
title('k0信号曲线图');

subplot(337)
plot(t,kk1);
axis([0,t(q),-1,1]);
title('k1信号曲线图');

b=[1,k0*(1+k1),k1];
a1=[1,k0*(1+a*k1),a*k1];
[H,w]=freqz(b,a1,100);
magh=abs(H);
subplot(338);
plot(w/pi,magh);
axis([-1,2,0,1.2]);
grid;
xlabel('频率单位pi');
ylabel('幅度');
title('幅度响应');


phah=angle(H);
subplot(339);
plot(w/pi,phah/pi);
xlabel('频率单位pi');
ylabel('相位单位pi');
title('相位相应');
grid;