Digital Reactor
機械学習

カルマンフィルタ入門：状態空間モデルでノイズ下の真値を推定する

カルマンフィルタ入門：状態空間モデルでノイズ下の真値を推定する

はじめに

センサから届く位置の系列は、そのままではノイズだらけで使えません。ノイズを消そうと移動平均をかけると、今度は動きへの追従が遅れます。ノイズを消せば遅れが出て、遅れを嫌えばノイズが残る。この綱引きを解くのがカルマンフィルタです。今回の合成データでは、真の位置からの誤差（RMSE）が、生の観測でおよそ6.4だったところを、フィルタで半分近くに、全区間を使う平滑化ではさらにその半分以下まで下げられました。

カルマンフィルタは、状態空間モデルという枠組みの上で「予測」と「更新」を繰り返すだけの、線形ガウス下でのベイズ再帰の閉形式解です。難しく見えて、中身は2つの行列演算のループにすぎません。この記事では等速運動する物体の位置推定を題材に、フィルタと平滑化を自前で実装し、素朴な手法との差を数値で確かめます。

対象読者:

  • 時系列のフィルタリング・平滑化・予測に関わる方
  • ノイズの多いセンサ値や観測系列から状態を復元したい方
  • ベイズ更新の線形ガウス特殊ケースとしてカルマンフィルタを理解したい方

記事のポイント:

  • 状態方程式と観測方程式からなる状態空間モデルの枠組みを整理します
  • カルマンフィルタの予測・更新ステップをベイズ再帰として理解します
  • フィルタリングとRTS平滑化の違いを、同一データ上のRMSEで比較します

状態空間モデルとは

観測できるのは位置ですが、本当に知りたいのは物体の状態、つまり位置と速度です。状態空間モデルは、この「見えない状態」と「見える観測」を2本の式で結びます。

xt=Axt−1+wt,wt∼N(0,Q)x_t = A x_{t-1} + w_t, \qquad w_t \sim \mathcal{N}(0, Q) yt=Hxt+vt,vt∼N(0,R)y_t = H x_t + v_t, \qquad v_t \sim \mathcal{N}(0, R)

上が状態方程式、下が観測方程式です。xtx_t は潜在状態（今回は位置と速度の2次元）、yty_t は観測です（位置のみ）。AA は状態がどう時間発展するか、HH は状態から何が観測されるかを表します。等速運動なら次のステップの位置は「今の位置 + 速度」なので、AA は位置に速度を足し込む行列になります。wtw_t はモデルに載らない加速度などのプロセスノイズ、vtv_t はセンサの観測ノイズで、いずれもガウスと仮定します。

やりたいのは、観測 y1:ty_{1:t} から潜在状態 xtx_t の分布を推定することです。線形かつガウスという仮定の下では、この推定はベイズ更新の閉形式解として書けます。それがカルマンフィルタで、状態の推定値だけでなく、その不確実性（共分散）まで同時に運んでくれる点が実務上ありがたいです。

カルマンフィルタの2ステップ

各時刻でやることは予測と更新の2つだけです。予測は、前の時刻の推定を状態方程式で1ステップ進めます。

x^t∣t−1=Ax^t−1,Pt∣t−1=APt−1A⊤+Q\hat{x}_{t|t-1} = A \hat{x}_{t-1}, \qquad P_{t|t-1} = A P_{t-1} A^\top + Q

PP は推定の共分散、つまり「どれだけ自信がないか」を表します。時間を進めるとプロセスノイズ QQ の分だけ不確実性が増えます。次に、その時刻の観測でこの予測を補正します。

Kt=Pt∣t−1H⊤(HPt∣t−1H⊤+R)−1K_t = P_{t|t-1} H^\top \left( H P_{t|t-1} H^\top + R \right)^{-1} x^t=x^t∣t−1+Kt(yt−Hx^t∣t−1)\hat{x}_t = \hat{x}_{t|t-1} + K_t \left( y_t - H \hat{x}_{t|t-1} \right)

yt−Hx^t∣t−1y_t - H \hat{x}_{t|t-1} は予測と実測のずれ（イノベーション）で、これに掛かる KtK_t がカルマンゲインです。ゲインは、観測ノイズ RR が大きければ小さくなり（観測を信じない）、予測の不確実性 PP が大きければ大きくなります（予測を信じない）。つまりゲインは、プロセスノイズと観測ノイズの比で「観測をどれだけ信じるか」を自動調整します。QQ と RR をどう置くかでフィルタの挙動がほぼ決まる、という実務上の見極めどころは、ここに由来します。

このフィルタは、線形ガウスの状態空間でベイズ更新を閉じた形で回しているにすぎません。事前分布（予測）に尤度（観測）を掛けて事後分布（更新後の推定）を得る、その1周を毎ステップ繰り返しています。

平滑化とその先

フィルタは各時刻で「その時点までの観測」だけを使います。オンラインで逐次推定する用途にはこれでよいのですが、あとから全区間のデータが揃っているなら、未来の観測も使って過去の推定を改善できます。これがRTS平滑化で、フィルタの結果を後ろ向きに1回なぞるだけで実装できます。

線形ガウスという仮定が崩れる場合は、そのままでは使えません。非線形なら拡張カルマンフィルタ（EKF）や無香料カルマンフィルタ（UKF）、非ガウスまで踏み込むなら粒子フィルタが必要になります。一方で、欠測への対処はむしろ簡単で、観測が来なかった時刻は更新ステップを飛ばして予測だけ進めれば済みます。この扱いやすさも状態空間モデルの利点です。

合成データで検証する

等速運動する物体の真の軌道を既知として合成し、位置だけをノイズ込みで観測します。フィルタ推定が真値をどれだけ追えるかを、生観測・移動平均と比べます。乱数シードは42に固定しました。

カルマンフィルタ本体は、上の数式をそのまま行列演算に落とすだけです。

def kalman_filter(obs, A, H, Q, R, x0, P0):
    n, dim = len(obs), A.shape[0]
    xf = np.zeros((n, dim))       # filtered mean
    Pf = np.zeros((n, dim, dim))  # filtered covariance
    xp = np.zeros((n, dim))       # one-step prediction (smoother用に保持)
    Pp = np.zeros((n, dim, dim))
    x_pred, P_pred = x0, P0
    for t in range(n):
        if t > 0:                                    # 予測
            x_pred = A @ xf[t - 1]
            P_pred = A @ Pf[t - 1] @ A.T + Q
        xp[t], Pp[t] = x_pred, P_pred
        y = np.array([obs[t]]) - H @ x_pred          # イノベーション
        S = H @ P_pred @ H.T + R
        K = P_pred @ H.T @ np.linalg.inv(S)          # カルマンゲイン
        xf[t] = x_pred + (K @ y)                      # 更新
        Pf[t] = (np.eye(dim) - K @ H) @ P_pred
    return xf, Pf, xp, Pp

RTS平滑化は、フィルタが残した1ステップ予測を使って後ろから補正します。

def rts_smoother(xf, Pf, xp, Pp, A):
    n = len(xf)
    xs, Ps = xf.copy(), Pf.copy()
    for t in range(n - 2, -1, -1):
        C = Pf[t] @ A.T @ np.linalg.inv(Pp[t + 1])
        xs[t] = xf[t] + C @ (xs[t + 1] - xp[t + 1])
        Ps[t] = Pf[t] + C @ (Ps[t + 1] - Pp[t + 1]) @ C.T
    return xs, Ps

観測とフィルタ推定、その95%帯を重ねると、生の点群のばらつきに対して推定が滑らかに真値を追っているのが分かります。

観測・真値・カルマンフィルタ推定と95%帯

一部を拡大して4つの手法を並べます。移動平均（青破線）はノイズに引きずられて上下し、真値の変化にも遅れて追従します。フィルタ（赤）はそれより滑らかですが、その時点までの情報しか使えないぶん多少ぶれます。RTS平滑化（緑）は全区間を使えるので、真値にほぼ張り付いています。

生観測・移動平均・フィルタ・平滑化の比較

真の位置に対するRMSEで並べると差は明快でした。

手法RMSE遅延特徴
生観測6.40なしノイズそのまま
移動平均(w=5)5.09あり平滑化と引き換えに遅れる
カルマン(フィルタ)3.21小逐次・オンライン
RTS(平滑化)1.39全区間必要オフラインで最良

各手法のRMSE比較

フィルタと平滑化の使い分け

移動平均が生観測より少し良い程度にとどまったのは、平滑化と遅延がトレードオフの関係にあるからです。窓を広げればノイズは減りますが、真値の変化への追従はさらに遅れます。カルマンフィルタが移動平均に差をつけたのは、単純な平均ではなく、運動モデル（等速で動くはず）という事前知識を使って予測しているからにほかなりません。

フィルタから平滑化にすると、RMSEは3.21から1.39へ半分以下になりました。未来の観測を使える平滑化の強みですが、裏を返せばオンライン用途では使えません。リアルタイムに推定したいのか、あとから一括で最良の推定が欲しいのかで、使う道具が変わります。

もう一つ、95%帯の妥当性です。今回の1本の軌道では、真値が帯に入った割合は0.82とやや低めに出ました。ただしこれは1本ぶんのばらつきで、300回のシミュレーションで平均すると0.951とほぼ名目どおりでした。時系列の1本は点どうしが強く相関するため、単一系列での被覆率は大きく揺れます。フィルタの不確実性見積もり自体は正しく校正されています。

実務で効いてくるのは、プロセスノイズ QQ と観測ノイズ RR の設定です。ここが実データと合っていないと、フィルタは自信過剰にも過小にもなります。真値が分からない現場では Q,RQ, R を最尤やEMで推定するのがよく使われる手ですが、まずは観測ノイズ RR をセンサ仕様に基づいて置き、QQ をモデルの粗さの吸収代として調整する、という順番で当たりをつけることが多いです。加えて、線形ガウスの仮定が崩れる場面、たとえば急な方向転換や外れ値の混入では、この素のフィルタは崩れます。そこは非線形フィルタや外れ値に頑健な変種で対処する範囲になります。

まとめ

カルマンフィルタは、予測と更新という2ステップのベイズ再帰で、ノイズの下の状態を不確実性込みで推定します。今回の合成データでは、生観測のRMSE6.40を、フィルタで3.21、平滑化で1.39まで下げられました。トラッキングやセンサフュージョン、需要のトレンドとノイズの分離、金融の潜在ボラティリティ推定など、応用先は広いです。最初の一歩としては、手元の系列に等速・等加速のような単純な運動モデルを当て、RR をセンサ由来で固定して QQ だけ動かし、フィルタの追従と95%帯の妥当性を目視で確かめるところから始めるとよいでしょう。非線形・非ガウスに踏み込むなら、次は粒子フィルタを試すのが選択肢になります。

関連記事

← 技術ブログ一覧へ