比例・微分・積分のストリーミング処理

filer.py

"""比例・微分・積分のストリーミング処理（PID 部品）。

ABC な基底 `Filter[T]` を step() でオーバーライドする実装群:
    Proportional[T]  : gain * value（状態なし）
    Differentiator[T]: 1次LPF付き前進差分（dirty derivative）
    Integrator[T]    : 台形則 + Kahan 補償加算
    Compose[T]       : 複数 Filter の直列合成（自身も Filter）

型変数 T は `Numeric` プロトコル（float / numpy.ndarray 互換）に束縛。
共通 IF: step(value: T) -> T ／ __call__ も同義 ／ reset() で状態初期化。
依存: 標準ライブラリのみ（デモ部のみ math）。Python 3.12+。
"""
from __future__ import annotations

from abc import ABC, abstractmethod
from typing import Protocol, Self, override


class Numeric(Protocol):
    """逐次演算に必要な算術だけを満たす型（float, numpy.ndarray など）。"""

    def __add__(self, other: Self, /) -> Self: ...
    def __radd__(self, other: float, /) -> Self: ...
    def __sub__(self, other: Self, /) -> Self: ...
    def __mul__(self, other: float, /) -> Self: ...
    def __truediv__(self, other: float, /) -> Self: ...


class Filter[T: Numeric](ABC):
    """逐次値処理の基底クラス。step() の実装を強制する。"""

    @abstractmethod
    def step(self, value: T) -> T:
        """1 サンプルを受け取り結果を返す。"""
        ...

    def __call__(self, value: T) -> T:
        return self.step(value)

    def reset(self) -> None:
        """内部状態を初期化（状態を持つ実装でオーバーライド）。"""


class Proportional[T: Numeric](Filter[T]):
    """比例: step(value) = gain * value。状態を持たない。"""

    def __init__(self, gain: float = 1.0):
        self.gain = gain

    @override
    def step(self, value: T) -> T:
        return value * self.gain


class Differentiator[T: Numeric](Filter[T]):
    """1次ローパス付き微分器（dirty derivative）。

    前進差分はノイズを 1/dt 倍に増幅するため、時定数 tau の一次 LPF で
    平滑化する。tau=0 で純粋な前進差分。prev の初期値は最初の value
    （→ 初回 step は 0 を返す）。

    Args:
        dt:  サンプリング周期 [s] (>0)
        tau: フィルタ時定数 [s] (>=0)。目安 3~10*dt。
        init_value: prev の初期値。None なら最初の step の値。
    """

    def __init__(self, dt: float, tau: float = 0.0, init_value: T | None = None):
        if dt <= 0:
            raise ValueError("dt must be positive")
        if tau < 0:
            raise ValueError("tau must be non-negative")
        self.dt = dt
        self._a = dt / (tau + dt)          # LPF 係数（tau=0 で 1 → 素通し）
        self._prev = init_value
        self._d: T | None = None

    @override
    def step(self, value: T) -> T:
        if self._prev is None:
            self._prev = value             # 初回: prev = 今回値 → 微分 0
        raw = (value - self._prev) / self.dt
        self._prev = value
        if self._d is None:
            self._d = raw                  # 状態を T-zero で確定
        else:
            self._d = self._d + (raw - self._d) * self._a
        return self._d

    @override
    def reset(self, init_value: T | None = None) -> None:
        self._prev = init_value
        self._d = None


class Integrator[T: Numeric](Filter[T]):
    """台形則 + Kahan 補償加算による積分器。

    台形則は O(dt^2) で毎ステップ更新可能。長時間積分の浮動小数点丸め誤差
    （累積ドリフト）を Kahan 加算で抑える。初回 step は区間未確定のため
    加算せず init_value を返す。

    （アンチワインドアップのクランプは順序比較を要するためジェネリックな
      T には載らない。必要なら Integrator[float] を継承した具象クラスで。）

    Args:
        dt: サンプリング周期 [s] (>0)
        init_value: 積分定数。numpy 状態なら np.zeros(...) を渡す（0.0 も可）。
    """

    def __init__(self, dt: float, init_value: T | float = 0.0):
        if dt <= 0:
            raise ValueError("dt must be positive")
        self.dt = dt
        self._init = init_value
        self._acc: T | float = init_value
        self._comp: T | None = None        # Kahan 補償項（初回積分で T 化）
        self._prev: T | None = None

    @override
    def step(self, value: T) -> T:
        if self._prev is None:
            self._prev = value             # 左端を確定（加算なし）
            return self._acc               # type: ignore[return-value]
        increment = (value + self._prev) * (0.5 * self.dt)   # 台形則
        self._prev = value
        if self._comp is None:             # 初回積分で内部状態を T に昇格
            zero = increment - increment
            self._acc = self._acc + zero   # float + T -> T（以後 _acc は T）
            self._comp = zero
        # Kahan summation
        y = increment - self._comp
        t = self._acc + y
        self._comp = (t - self._acc) - y
        self._acc = t
        return self._acc

    @override
    def reset(self) -> None:
        self._acc = self._init
        self._comp = None
        self._prev = None


class Compose[T: Numeric](Filter[T]):
    """複数の Filter を直列に適用する合成フィルタ。

    step(value) は登録順に各 filter へ通す: f_n( ... f_2(f_1(value)) )。
    Compose 自身も Filter[T] なので入れ子にできる。

    例: Compose(Differentiator(dt, tau), Proportional(kd)) で D 項を構成。
    """

    def __init__(self, *filters: Filter[T]):
        self.filters = filters

    @override
    def step(self, value: T) -> T:
        for f in self.filters:
            value = f.step(value)
        return value

    @override
    def reset(self) -> None:
        for f in self.filters:
            f.reset()


if __name__ == "__main__":
    import math

    dt, n = 0.02, 500
    p: Proportional[float] = Proportional(2.0)
    d: Differentiator[float] = Differentiator(dt)   # tau=0 → 前進差分
    i: Integrator[float] = Integrator(dt)

    e_d = e_i = 0.0
    for k in range(n + 1):
        t = k * dt
        assert p.step(t) == 2.0 * t                 # 比例は厳密
        e_i = max(e_i, abs(i.step(math.cos(t)) - math.sin(t)))   # ∫cos = sin
        val = d.step(math.sin(t))                                # d/dt sin = cos
        if k >= 1:
            e_d = max(e_d, abs(val - math.cos(t)))

    print(f"proportional : exact (gain*value)")
    print(f"integral   max error: {e_i:.2e}")
    print(f"derivative max error: {e_d:.2e}")

    # Compose: 微分 → 積分 で入力を復元（∫ d/dt x = x - x0）
    rt: Compose[float] = Compose(Differentiator(dt), Integrator(dt))
    e_rt = max(abs(rt.step(math.sin(k * dt)) - math.sin(k * dt)) for k in range(n + 1))
    print(f"compose(d,∫) reconstruction max error: {e_rt:.2e}")