notch.m

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

              % t =0:1/100:20.47;d = 0.1*cos(pi/2*t);%(zhengchunyuande)

%t =0:1:299; d = 0.1*cos(pi*0.5*t);
t=0:1/3000:1;
d=0.1*cos(0+cos(2500*pi*t));
%t =0:1:10; d = 0.1*cos(pi*0.5*t);
subplot(331);
q=length(t);
plot(t,d);
axis([0,t(q),-0.3,0.3]);
title('zhengxuan信号源曲线图');
d=d';

rnd1 = 0.032*randn(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);

r0 = 0;
r00=0;
k1=1;
k0 = 0;
K0=0;
namda = 0.9965;
a = 0.95;

c0 = 0.000023;
d0 = 0.000082;
s0=0;
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
    
    A=s0+r0;
    s0 =s(i)-n(i);
    
    B=r00+s0;
    r00=r0;
    c0 = namda*c0+A*B;
    d0 = namda*d0+A*A;
    k0 = -c0/d0;
    K0=0.5*K0+0.5*k0;
    kk0(i) = K0;
    
    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信号曲线图');

%b=[1,1*(1+k1),k1];
%a1=[1,-1*(1+a*k1),a*k1];
%K0=-cos(pi*0.005);
b=[1,K0*(1+k1),k1];
a1=[1,K0*(1+a*k1),a*k1];
[H,w]=freqz(b,a1,200);

magh=abs(H);
subplot(337);
plot(w/pi,magh);
axis([0.8,1,-0.5,1.5]);
grid;
xlabel('频率单位pi');
ylabel('幅度');
title('幅度响应');


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