はじめに

初学者が書いているので間違えがある可能性があります。
振動制御によく用いられるノッチフィルタを離散系でCで実装してみました。

環境
双一次変換
ノッチフィルタの離散系
プリワープ処理後の離散ノッチの確認
ノッチフィルタ実装(c言語)

環境

M1 Mac Monterey 12.4
gcc version 10.2
- Cで実装します
Matlab(R2021a)
*　ボード線図が見たいため
gnuplot(version 5.4)
- 描画用

参考文献

ノッチフィルタ

ある周波数だけを落とすように作られたフィルタ

ノッチフィルタはバンドストップフィルタの一種で、これは特定範囲内の周波数を減衰する一方、他のすべての周波数は変化させることなく通過させるフィルタである。ノッチフィルタの場合、この周波数範囲が非常に狭い。電気/電子用語の定義：NOTCH FILTER

連続時間伝達関数

2次ノッチフィルタの連続時間伝達関数は$$G(s) = \frac{ s^2 + 2d\xi\omega_{N}s + \omega_{N}^2 }{s^2 + 2\xi\omega_{N}s + \omega_{N}^2}$$

となります。ここで$s$はラプラス演算子，$ \xi $はノッチの幅，$d$はノッチの深さ,$\omega$はカットしたい周波数です。

とりあえずノッチフィルタのボード線図

Matlabでボード線図を書いてみます。ノッチフィルタは高域で用いられることが多いのでサンプリング周期10 kHzとした場合のナイキスト周波数5000[Hz]の0.1倍として除去したい周波数を決めました。
*　$\omega = 3.14*10^3$, $\xi = 1$, $d = 0.001$とした場合

%ノッチフィルタ確認
Ts    = 100e-6               %サンプリング周期
f     = 1/Ts                 %サンプリング周波数
nyquistfreq = f/2            %ナイキスト周波数
omega = nyquistfreq*0.1*2*pi %ナイキストの0.1倍をカットオフ周波数とする
d     = 0.001                %ノッチ深さ
xi    = 1                    %ノッチ幅

num = [1 2*d*xi*omega omega*omega]
den = [1 2*xi*omega omega*omega]

%連続時間ノッチ
notch_s = tf(num, den)
%ボード線図
bode(notch_s)
legend("連続ノッチ")

$3.14*10^3$ rad/sでゲインが落ちていることが確認できます。

離散時間伝達関数

z変換

z変換は連続時間を離散時間に変換するのに用いられます。細かいことはあまりわかりませんがラプラス領域とz領域の関係は$$z = e^{sT} \\ s = \frac{ln(z)}{T}$$

詳細は以下URL参考
フィルタの離散化について

双一次変換

連続時間を離散時間に変換するには基本的には双一次変換を用います。
s領域の極が左反平面にあれば安定であるという現象が，z領域では単位円内にあれば安定であるというのに対応するからです。
双一次変換は$$s = \frac{2}{T}\frac{1 – z^{-1} }{1+z^{-1}}$$

これを連続時間の伝達関数sに代入してやることで離散系に直すことができます。

ノッチフィルタの離散系

双一次変換はz変換の近似であるため(離散系でlogを表現することができない)，ノッチフィルタでは角周波数が離散系にするとずれてしまう（周波数歪み）恐れがあります。特にナイキスト周波数に近づくほど誤差が大きくなります。
ノッチの周波数を 500 Hzとした場合の離散系の結果は以下の通りとなります。

    %連続から離散変換
    Hd = c2d(notch_s,Ts)
    bode(notch_s,Hd)
    legend("連続ノッチ","プリワープ処理なし離散ノッチ")

周波数ひずみによりノッチの周波数がずれていることがわかります。
そのため，あらかじめ$w$を補正しておく必要があります（プリワープ処理）$$ \omega_{修正} = \frac{2}{T}\tan{\left(\omega_{N} \frac{T}{2}\right)}$$

プリワープした周波数をノッチのカットオフ周波数と見なして双一次変換すれば良いです。

双一次変換後のノッチフィルタの伝達関数
- 地道に代入していきます$$ G[z] = \frac{s^2 + 2d\xi\omega_{修正}s + \omega_{修正}^2}{s^2 + 2 \xi \omega_{修正}s + \omega_{修正}^2 } $$

ここで以下とおく。$$ \omega_{修正} = \frac{2}{T}\tan{(\omega_N \frac{T}{2})} = \frac{2}{T} k\\ k = \tan{ \left(\omega_{N} \frac{T}{2} \right)}$$

上記と双一次変換の式を代入して$$G[z]= \frac{s^2 + 2d\xi(\frac{2}{T} k)s + (\frac{2}{T} k)^2}{s^2 + 2 \xi (\frac{2}{T} k)s + (\frac{2}{T} k)^2 } \\ = \frac{（\frac{2}{T}\frac{1 – z^{-1} }{1+z^{-1}}）^2 + 2d\xi(\frac{2}{T} k)(\frac{2}{T}\frac{1 – z^{-1} }{1+z^{-1}}) + (\frac{2}{T} k)^2}{(\frac{2}{T}\frac{1 – z^{-1} }{1+z^{-1}})^2 + 2 \xi (\frac{2}{T} k)(\frac{2}{T}\frac{1 – z^{-1} }{1+z^{-1}}) + (\frac{2}{T} k)^2 } \\ = \frac{4(1-z^{-1})^2 + 8d\xi k(1-z^{-1})(1+z^{-1}) + 4k^2(1+z^{-1})^2}{4(1-z^{-1})^2 + 8\xi k(1-z^{-1})(1+z^{-1}) + 4k^2(1+z^{-1})^2}\\ =\frac{ (1 – 2d \xi k + k^2)z^{-2} + (2 k^2 -2)z^{-1} + (1 + 2d\xi k + k^2) }{(1 – 2\xi k + k^2)z^{-2} + (2k^2 -2)z^{-1} + (1 + 2\xi k + k^2)} \\$$

$\omega_N$はプリワープ処理する前のカットオフ周波数であることに注意

プリワープ処理後の離散ノッチの確認

連続時間のノッチフィルタと計算したプリワープ処理後のノッチフィルタを比較します。
実はこんなややこしい計算をしなくてもmatlabには便利な機能があるので，そちらとも比較します。
- matlabのc2dオプションの’prewarp’と比較します。
- 参考↓
- 特に高次の伝達関数の場合は’matched’オプションを用いるのが良い(ナノスケールサーボ)

%%離散系ノッチフィルタ
%プリワープ処理 omega_n をomega_pとみなして計算
omega_p = 2.0/Ts*tan(omega*Ts/2.0) % wp = 2/T*tan(w*T/2) = 2/T*k

k = tan(omega*Ts/2.0)
%分子多項式
N2 = 1.0 - 2*d*xi*k + k*k;
N1 = 2.0*k*k - 2.0;
N0 = 1.0 + 2*d*xi*k + k*k;
%分母多項式
D2 = 1.0 - 2*xi*k + k*k;
D1 = 2.0*k*k - 2.0;
D0 = 1.0 + 2*xi*k + k*k;    

num_d = [N0 N1 N2]
den_d = [D0 D1 D2]
%プリワープ処理後の離散系ノッチ伝達関数
notch_d = tf(num_d,den_d,Ts)
% From matlab関数
Hdp = c2d(notch_s,Ts,'prewarp',omega);

%ボード線図比較
bode(notch_s,"-bo",notch_d,"-rx",Hdp,"-c^")
%xlim([3130, 3150])

legend("連続ノッチ","プリワープ処理後離散ノッチ","Matlab　プリワープノッチ")

プリワープ処理した離散系ノッチのノッチ周波数が連続系ノッチのそれとほぼ一致している$\omega_N = 3.14*10^3 $
高域だと離散系ノッチの位相が連続系ノッチと少しずれる
matlabに搭載のc2dの’prewarp’とほぼ同じ結果が得られたと思う。

ノッチフィルタ実装(c言語)

上記のノッチフィルタの関数（信号，除去したい周波数，ノッチ深さ，ノッチ幅）

double Notch_Jisaku(double u, double omega, double d, double xi){
    double y;

    k = tan(omega*Ts/2.0);
    // 分子多項式
    N2 = 1 - 2*d*xi*k + k*k;
    N1 = 2.0*k*k - 2.0;
    N0 = 1.0 + 2*d*xi*k + k*k;
    // 分母多項式
    D2 = 1 - 2*xi*k + k*k;
    D1 = 2.0*k*k - 2.0;
    D0 = 1.0 + 2*xi*k + k*k;    

    y = ( N2*uZ2 + N1*uZ1 + N0*u - D2*yZ2 - D1*yZ1 )/D0;

    uZ2=uZ1;
    uZ1=u;
    yZ2=yZ1;
    yZ1=y;

    return y;
}

ノッチフィルタ動作確認
- 周波数1 Hzの正弦波に除去したい周波数を入れてみて，ノッチフィルタを使うことで除去できるか確認してみます。
- パラメータは上記と同様。$d=0.001$，$ \xi=1$, $\omega = 3.14*10^3$ [rad/s]としてみました。 $$u(t) = sin(2\pi t) + sin(\omega_N t) $$

    // notch
    const double d         = 0.001;
    const double xi        = 1;
    double omega = 1/Ts/2*0.1*2*3.14159;

    notest_u = sin(2.0*3.14*t) + sin(omega*t);//除去したい周波数が入っている
    noreal_u = sin(2.0*3.14*t);//元信号
    // ノッチ自作
    notest_y = Notch_Jisaku(notest_u, omega, d, xi);