カルマンフィルタをPythonで動かす

目次 — 知りたいところから読む

入力を変えて確かめる

実験パネルを開き、実行を押すとPythonを読み込みます。停止と初期値へのリセットができます。結果はこの端末内で計算します。Pythonのインストールは不要です。

下にあるローカル実行の手順は、配布ソースを再現するための任意の手順です。ブラウザ実験には不要です。

実験パネルを別タブで開く

再現用ソースをダウンロード

前提と読み方

前提知識 行列の積と、カルマンフィルタの予測・更新の概要。

実装・実験 · 本文の条件と確認項目に沿って、計算例や比較表を追う。

カルマンフィルタの設定は、グラフを滑らかにする操作ではない。「運動モデルがどれだけ外れ得るか」と「観測がどれだけ散らばるか」を数値で伝える操作である。ここでは位置と速度の2状態を推定し、欠測中の予測、外れ値への反応、過信による観測の棄却を比べる。データはすべて合成で、実機の精度を示すものではない。

モデルと単位をそろえる

状態は位置[m]と速度[m/s]、観測は位置だけとする。時間間隔は0.1 s。等速モデルと観測式は次のとおり。

x_k=F x_{k-1}+w_k,\qquad F=\begin{bmatrix}1&0.1\\0&1\end{bmatrix},\qquad z_k=H x_k+v_k,\quad H=\begin{bmatrix}1&0\end{bmatrix}

観測ノイズの標準偏差は0.7 mなので、分散Rは0.49 m²となる。Qは各ステップで独立な加速度ノイズを仮定し、g=(dt²/2, dt)としてq ggᵀで作る。q=0.4の単位は(m/s²)²であり、連続時間の白色雑音のスペクトル密度とは異なる。同じ「加速度ノイズ」という名前でも、別の実装に値をそのまま移してはいけない。予測・更新の導出はカルマンフィルタの基礎で確認できる。

理論の一次資料として、Simo Särkkä (2013), Bayesian Filtering and Smoothing 第4.3節も参照できる。本実験の軌跡・ゲート・比較条件は独自に設定した。

同じ入力で四つの設定を比べる

実行コードを空の作業フォルダに保存し、次を実行する。Python 3.12.3、NumPy 1.26.4、Matplotlib 3.6.3で実行を確認した。このファイルはICPなど他の記事の合成実験もまとめて出力する。

python3 engineering_labs.py

乱数seedは42、標本数は300。真値の速度は12 sで1から1.4 m/sに変わる。7.0〜9.9 sの観測30個を欠測にし、16 sの観測に12 mを加える。初期状態は位置0 m・速度1 m/s、初期共分散の対角は1 m²と1 (m/s)²。各設定に同じ観測列を渡すため、結果の違いを設定に結び付けられる。

外れ値を距離だけで判定しない

更新前の観測残差をe、予測した残差分散をSとすると、この実験では次の条件で観測を棄却する。

e=z-H\hat{x}^{-},\qquad S=HP^{-}H^T+R,\qquad e^2/S>9

これは1次元の正規化残差に対する判定であり、多次元観測の閾値9を正当化するものではない。欠測と棄却では予測だけを行い、更新しない。共分散の更新にはJoseph形式を使い、実行時に半正定値性も確認する。欠測後は位置の不確かさが増えるため、固定の「何m離れたら異常」という判断とは挙動が異なる。

実行結果から過信を見つける

設定	位置RMSE [m]	棄却数
q=0.4、R=0.49、棄却なし	0.262	0
q=0.4、R=0.49、判定あり	0.178	2
q=0.002、R=0.49、判定あり	0.393	4
q=0.4、R=0.0049、判定あり	11.251	237

図は比較のため縦軸を−3〜4 mに制限している。12 mの外れ値と、Rが小さすぎる設定の大きな誤差は図の外に出る。全値は判定ありのCSV、小さいRのCSV、全設定の集計で確認できる。欠測はCSVではnanで表し、棄却数には含めない。

判定ありでも棄却は1個ではなく2個になった。閾値は「人工的に加えた外れ値だけを必ず取り除く」保証ではない。Rを実際の分散の100分の1にすると、フィルタは正常な散らばりも説明できなくなり、237個を棄却した。更新を失った推定が運動変化についていけず、さらに残差が増える。これはゲートを追加するだけでは頑健にならない具体例である。

自分のログへ移すときの順序

まず時刻・単位・座標系を確認し、静止区間で観測の分散を見積もる。次に運動中のモデル誤差と残差の時間変化を見る。最後にゲートを導入し、RMSEだけでなく欠測率、棄却率、不確かさの幅を並べる。今回のRMSEには真値を使えるが、実機では外部基準や独立した検証が必要になる。

Qを小さくした結果が滑らかでも、速度変化への遅れが増えていれば良い設定とは限らない。逆にQを増やせば常に改善するわけでもない。今回の順位は、この軌跡・初期値・雑音・評価区間に限る。

次の確認

CSVのsigma_mは推定位置の標準偏差である。推定値±2倍のsigmaを描き、欠測中と速度変化直後を比較すると、誤差と自己評価の違いを追える。センサ融合の切り分けへ進み、共分散を調整する前に時刻やTFを疑う手順とつなげよう。

同じ計算を手元で実行する

実行用コード一式（ZIP）を展開します。実験入口、生成された計算部分、PID原本を同じフォルダへ個別に保存しても実行できます。PIDはPython標準ライブラリのみ、Kalman・ICPはNumPyを使います。従来の図生成コマンドも引き続き利用できます。

python3 -m pip install numpy==1.26.4
python3 experiment.py kalman --parameters '{"q":0.4,"r":0.49,"gate":false}' > result.json

gateを切り替えて外れ値の影響を比較し、qを0.002またはrを0.0049にしてRMSEと棄却数を見る。観測列は保存CSVから固定し、入力条件を揃えて比較する。

コード・データと再現手順

ブラウザと同じ計算コード、合成データ、比較条件をまとめたZIPです。実測データではありません。

再現パッケージをダウンロード · 実行手順（日英） · ファイルと実験条件（JSON）

ZIPを新しいフォルダへ展開して実行します。

python3 reproduce.py --experiment kalman

このLabは4条件を比較します。PIDはPython標準ライブラリ、Kalman/ICPはNumPyが必要です。依存関係と版は実行手順に記載しています。