カルマンフィルタ

台車と距離計

線路を走る台車の位置 $p$ と速度 $v$ をまとめて、状態 $\mathbf{x}={[pv]}^{\mathrm{T}}$ とします。観測の間隔 $T$ の間、加速度 $a_k$ は一定で、刻みごとに独立な正規分布に従うものとします[5][4]。距離計は位置だけを測り、値に雑音 $w_k$ が乗ります。

${\sigma }_a$

加速度のゆらぎ（$a_k$ の標準偏差）

${\sigma }_z$

観測の雑音（$w_k$ の標準偏差）

加速度のゆらぎが 1 刻みで状態に足す分の共分散が、過程雑音の共分散 $Q$ です[5][4]。観測の雑音の分散が $R$ です。

1 次元: 予測と観測を掛け合わせる

まず位置だけを考えます。観測する前の見込み（予測）が平均 ${\mu }^{-}$・分散 ${\sigma }^{-2}$ の正規分布で、距離計の値 $z$ の雑音の分散が ${\sigma }_z^2$ のとき、2 つの分布を掛け合わせると、また正規分布になります[2]。これが観測した後の推定です。

• $K$ はカルマンゲインで、0 から 1 の間の値。推定は予測から観測へ $K$ の割合だけ寄る。位置の分布の図の下の目盛りが、この割合を示す
• 観測が予測より確か（${\sigma }_z^2<{\sigma }^{-2}$）なほど $K$ は 1 に近く、推定は観測に寄る
• 推定の分散の逆数は、予測と観測の分散の逆数の和。推定は予測と観測のどちらよりも確かになる

止まっていると分かっている台車（場面の「止まった台車」）では、推定をそのまま次の予測にして、観測を重ねるだけになります。$k$ 回観測した後の推定の分散の逆数は $\frac{1}{{\sigma }_{p0}^2}+\frac{k}{{\sigma }_z^2}$ なので、$k$ 回目の $K$ は $1/(k+{\sigma }_z^2/{\sigma }_{p0}^2)$ です。観測を重ねるほど新しい観測の重みは小さくなり、推定は観測の平均に近づきます。

予測と更新

位置と速度の 2 次元では、平均がベクトル、分散が 2 × 2 の共分散行列 $P$ になります。フィルタは観測の間隔ごとに、予測と更新をくり返します[1][3]。

1. 予測: 等速の模型で先へ進め、過程雑音の分だけ不確かさを広げる
2. 観測: 距離計で位置を測る
3. 更新: 予測と観測を掛け合わせる

$\hat{Q},\hat{R}$

フィルタが使う過程雑音と観測の雑音の共分散（見積り）。本当の $Q$・$R$ と同じとは限らない

$y_k,S_k$

イノベーション（観測と予測の差）と、フィルタが見積るその分散

$K_k$

カルマンゲイン。位置の分 $K_p$ と速度の分 $K_v$ の 2 つ

$H=[10]$ なので、$S_k$ は予測の位置の分散と $\hat{R}$ の和です。位置のカルマンゲイン $K_p$ は 1 次元の $K$ と同じ形になります。

楕円が予測で膨らみ、観測で縮む

位置と速度の平面の楕円は、共分散 $P$ の正規分布で 95 % の確率で状態が入る範囲です。2 次元の正規分布では、平均からのマハラノビス距離の 2 乗が自由度 2 のカイ 2 乗分布に従うので、その 95 % 点 5.991 で楕円を描きます[6][4]。

• 予測の $FP{F}^{\mathrm{T}}$ は、速さの分だけ位置がずれるので楕円を傾ける。$F$ の行列式は 1 なので、面積は変えない
• 面積を増やすのは $\hat{Q}$ を足す分。加速度のゆらぎがあるので、先の位置と速度はそのぶん不確かになる
• 観測は位置の向きにだけ楕円を縮める。速度は測っていないが、楕円の傾き（位置と速度の相関）を通して、速度の幅も縮む。$K_v$ が位置のずれから速度を直す割合

位置の分布の図は、楕円を位置の軸に映したものです。予測の山と観測の山を掛け合わせると推定の山になる、1 次元と同じことが起きています。

雑音の設定を誤ると

フィルタは、$\hat{Q}$・$\hat{R}$ が本当の値に合っていると仮定して不確かさ $P$ を見積ります。見積りが外れると、$P$ は本当の誤差の大きさを表さなくなります[4][3]。

見積りが合っているかは、観測だけから確かめられます。イノベーションを、フィルタが見積った分散で割った 2 乗が正規化イノベーション 2 乗（NIS）です[5][4]。

• 見積りが合っていれば、${\epsilon }_k$ は自由度 1 のカイ 2 乗分布に従い、平均は 1
• 1 回ごとの値はばらつきが大きいので、$N$ 回の平均で判定する。$N$ 回の和は自由度 $N$ のカイ 2 乗分布に従う
• 平均が 95 % の範囲より大きければ、誤差が見積りより大きい（過信）。小さければ、見積りが広すぎる（慎重すぎ）

このツールは、直近 12 s の NIS の平均を、自由度 $N$ のカイ 2 乗分布の両側 95 % の範囲を $N$ で割った値と比べます。分位点は Wilson と Hilferty の近似で求めます[7]。

今の値を代入した式

観測の間隔と、フィルタが使う見積りを代入した行列と、予測と更新をくり返して落ち着いたカルマンゲイン $K_{\infty }$ です。

このツールで決めたこと

• 台車の加速度のゆらぎ・観測の雑音・観測の間隔・初めの速さの初期値
• 本当の初めの位置は、フィルタの初めの位置の不確かさと同じ広さの正規分布から選ぶ。フィルタは位置 0・速度 0 から推定を始める
• 乱数の種（同じ設定なら毎回同じ動きと観測になる）
• 場面ごとに、選んだときに先に進めておく回数（止まった台車 3 回、走る台車 15 回、雑音の設定の誤り 120 回）
• 楕円は 95 % の範囲、誤差の帯は ±2σ
• NIS の平均は直近 12 s で、15 回以上観測してから判定する
• 位置と速度の平面の表示の範囲は、落ち着いたときの予測の標準偏差の 4 倍と観測の雑音の 3 倍の大きい方。本当の状態が外に出るときは広げる

出典

1. カルマンフィルタR. E. Kalman, “A New Approach to Linear Filtering and Prediction Problems,” Journal of Basic Engineering, vol. 82, no. 1, pp. 35–45, 1960, doi:10.1115/1.3662552
2. 1 次元の正規分布の積（式 (12)・(13)）R. Faragher, “Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation,” IEEE Signal Processing Magazine, vol. 29, no. 5, pp. 128–132, 2012, doi:10.1109/MSP.2012.2203621
3. 予測と更新の式、止まった量の推定と観測の雑音の見積りを誤った例（3 章）G. Welch and G. Bishop, An Introduction to the Kalman Filter, TR 95-041, University of North Carolina at Chapel Hill, updated April 5, 2004
4. 1 次元のカルマンゲインと過程雑音の見積りの誤り（4 章 One Dimensional Kalman Filters）、共分散の楕円（5 章）、過程雑音の共分散と、小さすぎるときの発散（7 章 Kalman Filter Math）R. R. Labbe Jr., Kalman and Bayesian Filters in Python（CC BY 4.0）
5. 離散白色雑音加速度の模型（6.3.2 節）、フィルタの整合性と NIS（5.4 節）Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with Applications to Tracking and Navigation, Wiley, 2001, doi:10.1002/0471221279
6. カイ 2 乗分布の分位点NIST/SEMATECH, e-Handbook of Statistical Methods, 1.3.6.7.4 Critical Values of the Chi-Square Distribution
7. カイ 2 乗分布の分位点の近似E. B. Wilson and M. M. Hilferty, “The Distribution of Chi-Square,” Proceedings of the National Academy of Sciences, vol. 17, no. 12, pp. 684–688, 1931, doi:10.1073/pnas.17.12.684