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

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

モデルと単位をそろえる

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

観測ノイズの標準偏差は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とすると、この実験では次の条件で観測を棄却する。

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

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

図1 · ボタンで表示を切り替え

図は比較のため縦軸を−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を疑う手順とつなげよう。