実装読解 — 1 次元の最小実装を追う

ここまでの内容をコードに落とすと、1 次元のカルマンフィルタは非常に短く書けます。大事なのは「行数」ではなく、各行がどの式に対応しているかを追えることです。

Python 最小実装

状態（x_hat, P）とパラメータ（Q, R）はクラスにまとめます。global を使ってモジュール変数を書き換えると、複数のフィルタを同時に動かせず、テストもしにくくなるため避けます。

class KalmanFilter1D:
    def __init__(self, x_hat=0.0, P=10.0, Q=0.05, R=2.0):
        self.x_hat = x_hat
        self.P = P
        self.Q = Q
        self.R = R

    def step(self, z):
        x_pred = self.x_hat
        P_pred = self.P + self.Q

        K = P_pred / (P_pred + self.R)
        self.x_hat = x_pred + K * (z - x_pred)
        self.P = (1.0 - K) * P_pred
        return self.x_hat, self.P

(1.0 - K) と書いているのは、整数で計算されないようにするためです。Python では 1 - K でも K が float なら結果は float になるため動作上は同じですが、C など他言語に移植したときの挙動を揃える意図で 1.0 と明示しています。

C 最小実装

C 版も状態とパラメータを構造体にまとめ、関数はその構造体を受け取って書き換える形にします。グローバル変数は使いません。

typedef struct {
    double x_hat;
    double P;
    double Q;
    double R;
} KalmanFilter1D;

double kf_step(KalmanFilter1D *kf, double z) {
    double x_pred = kf->x_hat;
    double P_pred = kf->P + kf->Q;

    double K = P_pred / (P_pred + kf->R);
    kf->x_hat = x_pred + K * (z - x_pred);
    kf->P = (1.0 - K) * P_pred;
    return kf->x_hat;
}

使用例: KalmanFilter1D kf = { .x_hat = 0.0, .P = 10.0, .Q = 0.05, .R = 2.0 }; と初期化してから、観測ごとに kf_step(&kf, z) を呼びます。

式と変数の対応

読み解くときのポイント

1. 予測の 2 行と更新の 3 行をまず分ける
2. K が何に依存しているか確認する
3. 最後に分散 P がちゃんと更新されているか確かめる