簡単なレーダー例で理解するカルマンフィルタ
(kalmanfilter.net)- カルマンフィルタは、ノイズの多い環境でシステムの状態を推定し、未来を予測する最適状態推定アルゴリズム
- 航空機追跡レーダーを例に、距離と速度の測定値を使って予測と更新の段階を繰り返しながら精度を高める過程を説明
- 各段階で状態ベクトル、共分散行列、カルマンゲイン (Kalman Gain) を計算し、測定値と予測値を重み付きで結合
- 測定の不確実性とモデルの不確実性を同時に考慮し、時間が経つにつれて推定誤差（不確実性）が減少することを数値で提示
- 直感的な数値例と段階的な計算を通じて、自分でフィルタを設計・実装できる理解の基盤を提供
カルマンフィルタの紹介
-
Kalman Filterは、測定ノイズや外部要因など不確実性が存在する環境で、システムの状態を推定し予測する状態推定アルゴリズム
- 物体追跡、航法、ロボティクス、制御など、さまざまな分野で中核的なツールとして使われる
- たとえば、マウス軌跡のノイズを減らして滑らかな動きを得たり、金融データのトレンド検出、気象予測などに活用される
- 多くの教育資料が数学的導出に偏り、実例が不足している点を指摘し、本資料では数値例中心の直感的な説明を提供
- 設計を誤るとフィルタが追跡に失敗する事例も扱い、それを補正する方法も示す
- 目標は、読者が自分でカルマンフィルタを設計し実装できるよう理解を確立すること
学習パス
- 単一ページの概要: 中核概念と主要な数式を簡潔に紹介し、統計と線形代数の基礎知識のみを前提とする
- 無料Webチュートリアル: 段階的な数値例で直感を養うオンラインチュートリアルで、事前知識は不要
- Kalman Filter from the Ground Up（書籍）: 14の完全な数値例、非線形フィルタ（Extended/Unscented）やセンサーフュージョン、Python・MATLABコードを収録
予測の必要性
- 航空機追跡レーダーの例を通じて、状態推定と予測の必要性を説明
- システム状態は航空機の位置（距離 (r)）であり、レーダーはパルスの反射時間を測定して距離を計算する
- 速度 (v) はドップラー効果で測定可能
- 一定時間間隔 (\Delta t) 後の位置予測は動的モデルによって行う
- 例: (r_{t_1} = r_{t_0} + v \cdot \Delta t)
- (\Delta t = 5s), (r_{t_0}=10,000m), (v=200m/s) → (r_{t_1}=11,000m)
- 実環境では、測定ノイズ（Measurement Noise）とモデルの不確実性（Process Noise）が存在する
- 複数のレーダーが同時に測定しても、結果は少しずつ異なる
- 風などの外部要因により、速度一定という仮定が崩れる
- カルマンフィルタは現在の状態推定と未来の状態予測を同時に行い、各推定の**不確実性（分散）**もあわせて提供する
- 状態推定の不確実性を最小化する最適アルゴリズム
カルマンフィルタの例
-
1次元レーダーが航空機の距離 (r) と速度 (v) を測定
- 状態ベクトル (\boldsymbol{x} = [r, v]^T)
- ベクトルと行列を使ってシステムを表現
-
Iteration 0 — 初期化と予測
-
初期化
- 最初の測定値でフィルタを初期化 (\boldsymbol{z}_0 = [10{,}000, 200]^T)
- 測定の不確実性（標準偏差）: 距離 4m、速度 0.5m/s (\boldsymbol{R}_0 = \begin{bmatrix}16 & 0 \ 0 & 0.25\end{bmatrix})
- 初期状態推定値 (\hat{\boldsymbol{x}}_{0,0} = \boldsymbol{z}_0)
- 初期共分散 (\boldsymbol{P}_{0,0} = \boldsymbol{R}_0)
-
予測段階
- 時間間隔 (\Delta t = 5s)
- 状態遷移行列 (\boldsymbol{F} = \begin{bmatrix}1 & 5 \ 0 & 1\end{bmatrix})
- 予測状態 (\hat{\boldsymbol{x}}{1,0} = \boldsymbol{F}\hat{\boldsymbol{x}}{0,0} = [11{,}000, 200]^T)
- 共分散予測（プロセスノイズを除く）: (\boldsymbol{P}{1,0} = \boldsymbol{F}\boldsymbol{P}{0,0}\boldsymbol{F}^T = \begin{bmatrix}22.25 & 1.25 \ 1.25 & 0.25\end{bmatrix})
- プロセスノイズを追加 ((\sigma_a = 0.2m/s^2)): (\boldsymbol{Q} = \begin{bmatrix}6.25 & 2.5 \ 2.5 & 1\end{bmatrix})
- 最終的な予測共分散: (\boldsymbol{P}_{1,0} = \begin{bmatrix}28.5 & 3.75 \ 3.75 & 1.25\end{bmatrix})
-
Iteration 0 のまとめ
- 最初の測定で状態と共分散を初期化
- 状態遷移モデルを使って次の状態と不確実性を予測
- 予測式
- 状態予測: (\hat{\boldsymbol{x}}{n+1,n} = \boldsymbol{F}\hat{\boldsymbol{x}}{n,n} + \boldsymbol{G}\boldsymbol{u}_n)
- 共分散予測: (\boldsymbol{P}{n+1,n} = \boldsymbol{F}\boldsymbol{P}{n,n}\boldsymbol{F}^T + \boldsymbol{Q})
-
Iteration 1 — 更新と予測
-
フィルタ更新
- 2回目の測定: (\boldsymbol{z}_1 = [11{,}020, 202]^T)
- 測定の不確実性が増加（標準偏差: 距離 6m、速度 1.5m/s） (\boldsymbol{R}_1 = \begin{bmatrix}36 & 0 \ 0 & 2.25\end{bmatrix})
- 予測共分散 (\boldsymbol{P}_{1,0}) と比べると、予測の不確実性のほうが小さい
- カルマンフィルタは測定と予測を重み付き平均として結合する
- 重み (K_1): Kalman Gain
- 状態更新式: (\hat{\boldsymbol{x}}{1,1} = \hat{\boldsymbol{x}}{1,0} + \boldsymbol{K}_1(\boldsymbol{z}1 - \boldsymbol{H}\hat{\boldsymbol{x}}{1,0}))
- 観測行列 (\boldsymbol{H} = \boldsymbol{I})
- カルマンゲインの計算: (\boldsymbol{K}1 = \boldsymbol{P}{1,0}\boldsymbol{H}^T(\boldsymbol{H}\boldsymbol{P}_{1,0}\boldsymbol{H}^T + \boldsymbol{R}_1)^{-1}) 結果: (\boldsymbol{K}_1 = \begin{bmatrix}0.4048 & 0.6377 \ 0.0399 & 0.3144\end{bmatrix})
- イノベーション (innovation): (\boldsymbol{z}1 - \hat{\boldsymbol{x}}{1,0} = [20, 2]^T)
- 補正値: (\boldsymbol{K}_1[20, 2]^T = [9.37, 1.43]^T)
- 更新後の状態: (\hat{\boldsymbol{x}}_{1,1} = [11{,}009.37, 201.43]^T)
-
共分散更新
- 単純化した形を使用: (\boldsymbol{P}_{1,1} = (\boldsymbol{I} - \boldsymbol{K}1)\boldsymbol{P}{1,0})
- 結果: (\boldsymbol{P}_{1,1} = \begin{bmatrix}14.57 & 1.43 \ 1.43 & 0.71\end{bmatrix})
- 更新後の不確実性は予測および測定の不確実性より小さい → 測定と予測を組み合わせると常に不確実性は減少する
-
予測段階
- 次の時刻 (t_2) を予測
- 状態予測: (\hat{\boldsymbol{x}}{2,1} = \boldsymbol{F}\hat{\boldsymbol{x}}{1,1} = [12{,}016.5, 201.43]^T)
- 共分散予測: (\boldsymbol{P}{2,1} = \boldsymbol{F}\boldsymbol{P}{1,1}\boldsymbol{F}^T + \boldsymbol{Q} = \begin{bmatrix}52.86 & 7.47 \ 7.47 & 1.71\end{bmatrix})
- 時間が経つにつれて、測定がなければ不確実性は再び増加する
- 次の時刻 (t_2) を予測
-
Iteration 1 のまとめ
- 更新段階: 予測と測定をカルマンゲインで結合
- 予測段階: 更新した状態を次の時点へ伝搬
- 主要な数式
- 状態更新: (\hat{\boldsymbol{x}}{n,n} = \hat{\boldsymbol{x}}{n,n-1} + \boldsymbol{K}_n(\boldsymbol{z}n - \boldsymbol{H}\hat{\boldsymbol{x}}{n,n-1}))
- 共分散更新（Joseph form）: (\boldsymbol{P}_{n,n} = (\boldsymbol{I} - \boldsymbol{K}n\boldsymbol{H})\boldsymbol{P}{n,n-1}(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})^T + \boldsymbol{K}_n\boldsymbol{R}_n\boldsymbol{K}_n^T)
- カルマンゲイン: (\boldsymbol{K}n = \boldsymbol{P}{n,n-1}\boldsymbol{H}^T(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T + \boldsymbol{R}_n)^{-1})
例のまとめ
- カルマンフィルタの3段階: 初期化 → 予測 → 更新
- 以後は予測-更新ループを繰り返し実行
- 新しい測定が追加されるたびに不確実性が減少し、システム状態の推定が次第に精緻になる
- 追加の学習資料
- 無料オンラインのチュートリアル: 段階的な数値例を提供
- 書籍 Kalman Filter from the Ground Up: 線形・非線形フィルタ、実装ガイド、Python/MATLABコードを収録
1件のコメント
Hacker Newsのコメント
投稿者です。最近、自分のKalman Filterチュートリアルのホームページを更新しました。簡単なレーダー追跡の例を追加して、統計と線形代数の基礎だけ知っている人でも理解できるように構成しました。
数式の導出は最小限にしつつ、測定ノイズ・予測モデル・フィルタの結合過程を直感的に示そうとしました。数学的なレベルや説明の明快さについてフィードバックをもらえるとうれしいです。
私が考える直感的なガイドは次の通りです。
結局のところ核心は最小二乗法であり、特定の仮定の下ではこれが最適であることを証明できます。
"How a Kalman Filter Works in Pictures" という記事が本当によかったです。
たとえばローパスフィルタも一種の推定器ですが、Kalmanフィルタはそこにプロセスモデルと共分散計算を加え、動的に調整される形になっています。
位置や速度のような相関した変数どうしの関係を反映し、ある一つの測定値が別の変数の推定にも影響するようにします。
ただし線形代数ベースなのでガウシアンノイズを仮定する必要があり、非線形問題にはEKFやUKFのような派生形が必要です。
Kalmanフィルタを理解するには、次の4つを知る必要があります。
フィルタは測定値に基づいてシステムの内部状態と共分散を最適推定する数学的な解法です。
Alex Beckerの本はこのテーマの優れた入門書だと思います。例が豊富で、直感を育てるのに向いています。
Kalmanフィルタを学ぶときは、ノイズ特性の異なる入力が2つ以上ある場合のほうが、ずっと直感的に理解できます。ですが、ほとんどのチュートリアルは単一入力しか扱いません。
Kalmanフィルタは実際によく使われています。たとえばSendspinではスピーカー同期のために使っています。
ライブデモ と 実装ドキュメント を参照できます。
Kalmanフィルタの概念を人の証言にも適用できるのではないかと気になります。
thekalmanfilter.com でも簡単な説明を見ることができます。
この記事は高価な本の宣伝のように見えました。すでに無料の資料はたくさんあり、たとえば Kalman and Bayesian Filters in Python のようなものもあります。この本ならではの利点があるのか気になります。