離散後の伝達関数の実装方法 覚え書き(c2d -> 実装)

連続系伝達関数(s領域)の離散化はMATLABやPython.controlのc2dコマンドで簡単に出来るけれど、
でどうやって組み込みソフトの周期タスク内に落とし込む形にするのかという話のメモ書きです。

(手計算で離散化させれば実装まで持っていけるのに、c2dコマンドで出てきたものをどう扱ってよいか恥ずかしながら迷子に。)

離散化まで

例として単純な積分器$(\frac{1}{s})$、対象のタスクは1秒周期(sample_time=1)とします。
離散化はc2dコマンド一発で

MATLABの場合は

sample_time = 1;
s= tf('s');
integral_c = 1/s;
integral_d = c2d(integral_c, sample_time, "tustin")

Pythonの場合は

import control
from control import tf, c2d

sample_time = 1
s= tf('s')
integral_c = 1/s
integral_d = c2d(integral_c, sample_time, method='tustin')

出力はどちらを使っても

integral_d =
  0.5 z + 0.5
  -----------
     z - 1
Sample time: 1 seconds
Discrete-time transfer function.

こちらの出てきた式は出力$Y$,入力$U$とする離散系伝達関数で下記を指します。
$$ \frac{Y(z)}{U(z)} = \frac{0.5z + 0.5}{z - 1} $$
これをどう実装できる形にするのかが分からなくなるポイントだと思います。(少なくとも自分は)

実装可能な式へ

ここから$z$の次数を下げてから差分方程式にします。
(分母より分子の次数が高い場合は非プロパーで実装不可なので下には進めません)

まず、右辺の分母分子両方を(分母の)$z$の最高次数で割ってzの最高次数を0とし実装可能な形にします。
この場合は$z$で割るので、
$$ \frac{Y(z)}{U(z)} = \frac{\frac{0.5z}{z} + \frac{0.5}{z}}{\frac{z}{z} - \frac{1}{z}} $$
よって
$$ \frac{Y(z)}{U(z)} = \frac{0.5 + \frac{0.5}{z}}{1 - \frac{1}{z}} $$
左辺を$Y(z)$とするように式変形していくと
$$ Y(z) \times (1 - \frac{1}{z}) = (0.5 + \frac{0.5}{z} ) \times U(z) \\
Y(z) = Y(z) \times \frac{1}{z} + 0.5 \times U(z) + 0.5 \times \frac{1}{z} \times U(z) $$
次に、時間領域tへの書き換えを実施します。
これも単純で、$z$がかかった項は今回値の$t$となり、$z-1$がかかった項は前回値$t-1$になるというだけです。
(= $Y(z)$は$Y(t)$となり$Y(z)\times\frac{1}{z}$は$Y(t-1)$となります。$U(z)$も同様に$U(t)$となります。)
したがって、
$$ Y(t) = Y(t-1) + 0.5 \times U(t) + 0.5 \times U(t-1) $$

以上で実装できる形になりました。
C言語ライクに書くと下記です。

float Y = 0;　    //出力値
float Y_z1 = 0;  // 出力の前回値
float U = 0;     //入力値
float U_z1 = 0;  //入力の前回値

Task_1sec()　//サンプル時間ごとに実行される関数
{
    //入力の取得
    U = funcGetU();
    
    //Yの算出
    Y = Y_z1 + (0.5 * U) + (0.5 * U_z1);

    //前回値の更新
    U_z1 = U;
    Y_z1 = Y;
}