% 功能：自適應(yīng)濾波器(并聯(lián))
% 注:這里取f1=40Hz,f2=50Hz,T1=T2=5s,snr=0dB,30-60Hz的64階FIR前置濾波器
% 作者：liukai
% 日期:2007年6月13日
% 狀態(tài):調(diào)試完成
%-------------------------------------------------------------------------%
function ad1
clear all,close all
fs=200;%采樣頻率(Hz)
snr=0;%信噪比（dB），可調(diào)節(jié)
R=10^(snr/20);
u=1/200;%步長(zhǎng),可調(diào)節(jié)
%---------------------制造信號(hào)---------------------------------------------%
t0=0:1/fs:25;
dn0=randn(size(t0));
t1=0:1/fs:5;
dn1_t=R*sin(2*pi*40*t1);
t2=0:1/fs:5;
dn2_t=R*sin(2*pi*50*t2);
dn1=zeros(size(t0));
dn2=zeros(size(t0));
s0=1000;%xn1的起始時(shí)刻
s1=length(t1)+s0-1;
dn1(s0:s1)=dn1_t;
s2=s1+1000;%xn2的起始時(shí)刻，可調(diào)節(jié)
s3=length(t2)+s2-1;
dn2(s2:s3)=dn2_t;
dn=dn0+dn1+dn2;%其中dn為濾波前期望信號(hào)
t=t0;
figure(1)
subplot(3,1,1)
plot(t,dn);
title('濾波前的期望信號(hào)');
% D=fft(dn,2048);
% wd=(0:1023)*fs/2048;
% figure
% plot(wd,abs(D(1:1024)));
% axis([0 8000 -100 1000]);
%----------------------------對(duì)前置信號(hào)濾波--------------------------------%
n=64;
ws=([30 60])*2/fs;
h=fir1(n,ws,hamming(n+1));                                                 
len=length(h);
N=256;
[h1 w1]=freqz(h,1,N,fs);
subplot(3,1,2)
plot(w1,abs(h1),'y*--');
title('濾波器的頻響');
block=length(dn);
rn=zeros(1,block);
L=ceil(len/2);
for i=L:(block+L-1)
    if i<=len
        for j=1:i
        rn(i-L+1)=rn(i-L+1)+h(j)*dn(i-j+1);
        end
    elseif len<i&&i<=block
        for j=1:len
            rn(i-L+1)=rn(i-L+1)+h(j)*dn(i-j+1);
        end
    else
        for j=1:(len-(i-block))
           rn(i-L+1)=rn(i-L+1)+h(j)*dn(block-j);
        end
    end
end
subplot(3,1,3)
plot(t,rn,'k');%濾波后的期望信號(hào)
title('濾波后的期望信號(hào)');
% dn=rn;
dn=dn;%選擇是否要用前置濾波器，可調(diào)節(jié)
%-----------------------對(duì)信號(hào)進(jìn)行自適應(yīng)濾波--------------------------------%
xn1=sin(2*pi*40*t);
xn2=cos(2*pi*40*t);
wn1=zeros(size(t));
wn2=zeros(size(t));
yn=zeros(size(t));%xn1的實(shí)際輸出
en=zeros(size(t));%誤差的輸出
xn3=sin(2*pi*50*t);
xn4=cos(2*pi*50*t);
wn3=zeros(size(t));
wn4=zeros(size(t));
ynn=zeros(size(t));%xn2的實(shí)際輸出
en(1)=dn(1)-yn(1)-ynn(1);
wn1(1)=2*u*en(1)*xn1(1);
wn2(1)=2*u*en(1)*xn2(1);
wn3(1)=2*u*en(1)*xn3(1);
wn4(1)=2*u*en(1)*xn4(1);
for i=1:length(t)-1
    yn(i)=xn1(i)*wn1(i)+xn2(i)*wn2(i);
    ynn(i)=xn3(i)*wn3(i)+xn4(i)*wn4(i);
    en(i)=dn(i)-yn(i)-ynn(i);
    wn1(i+1)=wn1(i)+2*u*en(i)*xn1(i);
    wn2(i+1)=wn2(i)+2*u*en(i)*xn2(i);
    wn3(i+1)=wn3(i)+2*u*en(i)*xn3(i);
    wn4(i+1)=wn4(i)+2*u*en(i)*xn4(i);
end 
figure(2)
subplot(3,1,1)
plot(t,yn,'r');
title('xn1的時(shí)域圖');
subplot(3,1,2)
plot(t,ynn);
title('xn2的時(shí)域圖');
subplot(3,1,3)
plot(t,en,'c')
title('誤差的時(shí)域圖');
Y=fft(yn,4096);
wn=(0:2047)*fs/4096;
YN=fft(ynn,4096);
wnn=(0:2047)*fs/4096;
figure(3)
subplot(2,1,1)
plot(wn,abs(Y(1:2048)));
title('xn1的頻域圖');
subplot(2,1,2)
plot(wnn,abs(YN(1:2048)),'g');
title('xn2的頻域圖');