notchfilter.m

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

%陷波器，将原始输入信号(cos+宽带噪声)中的cos由参考输入信号对消

%参考输入
t =0:1/500:10;
s1 = 0.1*cos(pi/4*t);
subplot(331)
q=length(t);
plot(t,s1);
axis([0,t(q),-0.3,0.3]);
title('噪声源信号1曲线图');


t =0:1/500:10;
s2 = 0.21*sin(pi/4*t);
subplot(332)
plot(t,s2);
axis([0,t(q),-0.3,0.3]);
title('噪声源信号1曲线图');
hold on;

%(cos+宽带噪声)
d1 = normrnd(0,0.051,[q 1]); 
subplot(333)
plot(t,d1);
d=zeros(1,2001);
d=s2'+d1;
subplot(334)
plot(t,d);
axis([0,t(q),-0.6,0.6]);
title('信号源曲线图');

w1=0;
w2=0;
y=zeros(1,length(d));
e=zeros(1,length(d));
for n=1:length(d)/2
    y(n)=w1*s1(n)+w2*s2(n);
    e(n)=d(n)-y(n);
    w1=w1+2*0.02185*e(n)*s1(n);
    w2=w2+2*0.02185*e(n)*s2(n);
end
for n=length(d)/2:length(d)
    y(n)=w2*s1(n)+w1*s2(n);
    e(n)=d(n)-y(n);
    w1=w1+2*0.02185*e(n)*s1(n);
    w2=w2+2*0.02185*e(n)*s2(n);
end
    
subplot(335)
n=1:q;
plot(n,y,'r');
axis([1,q,-0.3,0.3]);
title('notch估计输出');

%输出只剩宽带噪声
subplot(336)
n=1:q;
plot(n,e);
axis([1,q,-0.5,0.5]);
title('噪声对消器输出');

hold off;