多通道notch滤波器

clc
clear all
close all
%-------------信号产生------------------------------------------------------
%-------------基本参数------------------------------------------------------
% CW信号中心频率10kHz，脉宽10ms，采样率80kHz前后加5ms噪声  
A=1;
f0=10.e3; 
f1=11.e3;
f2=9.e3; 
fs=80.e3;
SNR=10;
signal_time=10.e-3; 
wait_time=5.e-3;
sig_time=0:1/fs:（signal_time-1/fs）;
sig=A*cos（2*pi*f0*sig_time）+A*cos（2*pi*f1*sig_time）+A*cos（2*pi*f2*sig_time）;
sig=[zeros（1wait_time*fs） sig zeros（1wait_time*fs）];
N=1:1:length（sig）;
D=256;
 % CW信号添加带限高斯白噪声  
f=[8000 9000 11000 12000];
bpass=fir1（512[f（1） f（4）]/（fs/2））;
sigma=sqrt（var（sig）/（10^（SNR/10）））;
noise0=wgn（1numel（sig）+D0）;
 noise1=filter（bpass1noise0）;
 noise2=noise1（D+1:numel（sig）+D）*sigma/std（noise1）;
 noise=noise2*sigma/std（noise2）;
 sig_noise=sig+noise;  
%-------------Notch filter-------------------------------------------------
% Notch滤波器中心频率10kHz，滤波器带宽250Hz对应自适应步长μ为0.01
f_notch1=10e3;
f_notch2=11e3;
X1c=cos（2*pi*f_notch1*N/fs）; 
X1s=sin（2*pi*f_notch1*N/fs）;
X2c=cos（2*pi*f_notch2*N/fs）; 
X2s=sin（2*pi*f_notch2*N/fs）;
BW=250;
mu=pi*BW/fs;
W1c=zeros（1length（sig）+1）; 
W1s=zeros（1length（sig）+1）;
W2c=zeros（1length（sig）+1）; 
W2s=zeros（1length（sig）+1）;
e=zeros（1length（sig））;
y=zeros（1length（sig））;
for k = 1:length（sig）
    y1（k） = X1c（k）*W1c（k） + X1s（k）*W1s（k）;
    y2（k） = X2c（k）*W2c（k） + X2s（k）*W2s（k）;
    y（k）=y1（k）+y2（k）;
    e（k） = sig_noise（k） - y（k）;
    W1c（k+1） = W1c（k） + 2*mu*e（k）*X1c（k）;
    W1s（k+1） = W1s（k） + 2*mu*e（k）*X1s