%***************************************%
% wavファイル処理                       %
% 適応ノッチフィルタの縦続接続          %
%         by Arata Kawamura (2021.06.5) %
%***************************************%
clear;
%************************************%
%       信号処理用変数設定           %
%************************************%
Fs = 16000;
L = 2*Fs;
x = zeros(L, 1);                     % 入力信号
y = zeros(L, 1);                     % 出力信号
n   = 1;                             % 時刻インデクスの初期値
N = 10;                              % ノッチフィルタの縦続接続数
r = 0.9;                             % 極半径の2乗
a = -0.99*(1+r)*ones(1,N);           % フィルタ係数の初期値
mu= 0.01;                          % ステップ・サイズ
u1=zeros(1,N); u2=zeros(1,N);        % 信号の初期化
e = zeros(1,N+1);                    % ノッチフィルタ出力
%************************************%
for n=1:L
    for i=1:N/2
        x(n) = x(n) + 1/N * sin(pi*i/N*n); % 現在の入力信号=正弦波
    end
    e(1) = x(n);
    for k=1:N
    	u0(k) = e(k) - a(k)*u1(k) - r*u2(k); % 内部信号の作成
        alp= r*u0(k) + a(k)*u1(k) + u2(k);   % オールパス・フィルタ出力
        e(k+1)  = (e(k) + alp)/2;            % k段目ノッチ・フィルタ出力
        a(k)  = a(k) - mu*u1(k)*e(N+1);      % フィルタ係数更新
        if abs(a(k)) >= (1+r)                % 係数の発散を防ぐ
            a(k) = 0;
            u0(k)=0; u1(k)=0;
        end
        u2(k)=u1(k);                         % 信号遅延
        u1(k)=u0(k);                         % 信号遅延
    end
    y(n) = e(N+1);                           % 出力信号
end
% wavを書き出す
audiowrite("output.wav", y, Fs);
