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