カルマンフィルタのチュートリアル
(kalmanfilter.net)- カルマンフィルタは、ノイズの混じったセンサー測定と不完全な動的モデルをあわせて使い、現在の状態と次の状態の不確実性まで推定するアルゴリズム
- このチュートリアルでは、航空機のレーダー追跡を例に、距離 (r) と速度 (v) を状態ベクトルとして、予測値と測定値を結合する過程を数値で追っていく
- 初期測定値 (10,000m)、(200m/s) とサンプリング間隔 (5s) を使うと、等速モデルでは次の位置は (11,000m) と予測され、測定ノイズ (R) と プロセスノイズ (Q) が共分散に反映される
- 2回目の測定値 (11,020m)、(202m/s) はより不確かだが、カルマンゲイン (K) が予測と測定を重み付きで結合し、更新後の状態を (11,009.37m)、(201.43m/s) と計算する
- 初期化の後は 予測-更新ループ が繰り返され、実際の実装では Joseph form のような安定した共分散更新式や外れ値測定への対処まで考慮する必要がある
カルマンフィルタが解く推定問題
- カルマンフィルタは、不確実性のある環境でシステムの状態を推定し予測するアルゴリズム
- 測定ノイズを含むセンサーデータ
- 未知の外乱
- 動的モデルと実際の運動の差
- 物体追跡、ナビゲーション、ロボティクス、制御、金融市場分析、気象予測などに使われる
- コンピュータマウスの軌跡推定に適用すると、ノイズを減らし手ぶれを補正して、より安定した移動経路を作れる
- このチュートリアルは、複雑な数学的説明よりも 数値例 と直感的な説明でカルマンフィルタを理解できるよう構成されている
- 状況設定が不適切だとカルマンフィルタが物体をうまく追跡できない例や、それを補正する方法も含まれている
学習パス
- このプロジェクトは、カルマンフィルタを3つの深さで学べるよう構成されている
- 単一ページの概要: 核となるアイデアと必須方程式を導出なしで説明し、基本的な統計と線形代数の知識を前提とする
- 無料の例題ベースWebチュートリアル: 数値例で直感を養い、カルマンフィルタ方程式の導出まで段階的に扱い、事前知識は不要としている
- Kalman Filter from the Ground Up: 14個の完全解説付き数値例、性能プロットと表、Extended Kalman Filter、Unscented Kalman Filter、センサーフュージョン、実装ガイドラインを含む
レーダー追跡の例で見る予測の必要性
- 航空機を追跡するレーダーでは、航空機がシステムであり、推定すべき位置がシステム状態となる
- レーダーは細いビームを航空機の方向へ向けるため、次のビームをどこへ送るか決めるには未来位置を予測する必要がある
- 予測に失敗すると、ビームが誤った方向を向いて追跡を失う可能性がある
- 時間に応じたシステムの運動を表す 動的モデル が必要になる
- 単純化した1次元の例では、航空機がレーダーに向かうか遠ざかる直線方向に動くと仮定する
- レーダーはパルスの送受信時間から距離 (r) を計算する
- ドップラー効果で速度 (v) も測定できる
- (t_0) で距離 (10,000m)、速度 (200m/s) が非常に正確に測定され、サンプリング間隔 (\Delta t=5s) で速度一定と仮定すると、次の位置は (11,000m) になる
- (\Delta r = v \cdot \Delta t)
- (r_{t_1}=10,000+200\cdot5=11,000m)
測定ノイズとプロセスノイズ
- 実際のレーダー測定は完全に精密ではなく、同じ瞬間に複数のレーダーで測定しても少しずつ異なる値になることがある
- このばらつきは 測定ノイズ で表される
- 状態推定値だけでなく、その推定がどれだけ信頼できるかも計算しなければならない
- 動的モデルも完全ではない
- 航空機が一定速度で動くと仮定しても、風のような外部要因で実際の動きが変わることがある
- このような予測不能な影響が プロセスノイズ である
- カルマンフィルタは、現在の状態推定、未来状態の予測、そしてそれぞれの 不確実性 をあわせて提供する
- システムとノイズがモデルの仮定に従う条件では、状態推定の不確実性を最小化する最適なアルゴリズムである
状態ベクトルと初期化
- この例のシステム状態は、航空機の距離 (r) と速度 (v) で構成される
[ \boldsymbol{x}= \begin{bmatrix} r\ v \end{bmatrix} ]
- 1回目の測定値は (t_0) で次のとおり
[ \boldsymbol{z}_0= \begin{bmatrix} 10{,}000\ 200 \end{bmatrix} ]
- 測定値には不確実性があるため、各測定には分散の形で 測定不確実性 が付随する
- 距離測定の標準偏差: (4m)
- 速度測定の標準偏差: (0.5m/s)
- 分散は標準偏差の2乗
[ \boldsymbol{R}_0= \begin{bmatrix} 16 & 0\ 0 & 0.25 \end{bmatrix} ]
- この例では、距離と速度の測定誤差は互いに無関係と仮定し、共分散行列の非対角要素を 0 とする
- 初期化段階では、測定値とシステム状態が同じ物理量 (r)、(v) を表しているため、最初の測定値を初期状態推定値として使える
[ \hat{\boldsymbol{x}}_{0,0}= \boldsymbol{z}_0= \begin{bmatrix} 10{,}000\ 200 \end{bmatrix} ]
- この方法は 初期化段階でのみ 使える
予測段階: 状態と共分散の伝播
- 予測では、現在の状態と 状態遷移行列 (\boldsymbol{F}) を使って次時刻の状態を計算する
- 等速モデルでは次の式を使う
[ v_1=v_0=v ]
[ r_1=r_0+v_0\Delta t ]
- 行列形式の状態予測式は次のとおり
[ \hat{\boldsymbol{x}}_{n+1,n}
\boldsymbol{F} \hat{\boldsymbol{x}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n ]
- (\boldsymbol{u}_n): 入力変数
- (\boldsymbol{G}): 入力遷移行列
- この単純な例では入力がないため (\boldsymbol{u}_n=0)
- (\Delta t=5s) のとき状態遷移行列は次のようになり、予測結果は (11,000m)、(200m/s) となる
[ \boldsymbol{F}= \begin{bmatrix} 1 & 5\ 0 & 1 \end{bmatrix} ]
[ \hat{\boldsymbol{x}}_{1,0}
\begin{bmatrix} 11{,}000\ 200 \end{bmatrix} ]
- 共分散予測は単に (\boldsymbol{F}\boldsymbol{P}) ではなく、(\boldsymbol{F}\boldsymbol{P}\boldsymbol{F}^T) を使う
[ \boldsymbol{P}_{n+1,n}
\boldsymbol{F} \boldsymbol{P}_{n,n} \boldsymbol{F}^T + \boldsymbol{Q} ]
- プロセスノイズを除けば、予測共分散は次のとおり
[ \boldsymbol{P}_{1,0}
\begin{bmatrix} 22.25 & 1.25\ 1.25 & 0.25 \end{bmatrix} ]
- 速度分散は等速モデルのため (0.25) のまま維持される
- 距離分散は、速度の不確実性が時間とともに距離の不確実性を増やすため、(16) から (22.25) に増加する
プロセスノイズの反映
- 実際の航空機速度は、風のような予測不能な外部要因の影響を受ける可能性があるため、プロセスノイズ (\boldsymbol{Q}) を共分散予測に加える
- 例ではランダム加速度の標準偏差を (\sigma_a=0.2m/s^2) と仮定する
- 分散は (\sigma_a^2=0.04m^2/s^4)
- (\Delta t=5s) のとき、プロセスノイズ行列は次のとおり
[ \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} ]
更新段階: 予測と測定の重み付き結合
- (t_1) での2回目の測定値は次のとおり
[ \boldsymbol{z}_1= \begin{bmatrix} 11{,}020\ 202 \end{bmatrix} ]
- この測定は強いノイズスパイクで信号対雑音比が低下しており、1回目の測定より不確実性が大きいと仮定する
- 距離の標準偏差: (6m)
- 速度の標準偏差: (1.5m/s)
[ \boldsymbol{R}_1= \begin{bmatrix} 36 & 0\ 0 & 2.25 \end{bmatrix} ]
- 予測共分散 (\boldsymbol{P}_{1,0}) の対角要素は測定共分散 (\boldsymbol{R}_1) より小さいため、予測側の不確実性のほうが低い
- カルマンフィルタは予測だけ、あるいは測定だけを使うのではなく、不確実性の低い側により大きな重み を与えて結合する
- 1次元形式の重み付き平均は次のとおり
[ \hat{x}_{1,1}
K_1 z_1 + (1-K_1)\hat{x}_{1,0} ]
- (\boldsymbol{K}) は カルマンゲイン であり、更新後の推定値の不確実性を最小化するように測定と予測の重みを決める
イノベーション、観測行列、カルマンゲイン
- 状態更新式は、予測値に補正項を加える形で書ける
[ \hat{\boldsymbol{x}}_{1,1}
\hat{\boldsymbol{x}}_{1,0} + \boldsymbol{K}_1 ( \boldsymbol{z}_1
\boldsymbol{H}\hat{\boldsymbol{x}}_{1,0} ) ]
- (\boldsymbol{z}1-\boldsymbol{H}\hat{\boldsymbol{x}}{1,0}) は イノベーション (innovation) または残差 (residual) で、新しい測定が与える情報を表す
- (\boldsymbol{H}) は 観測行列 または測定行列で、状態変数を実際に測定される物理量へマッピングする
- この例では、状態と測定がどちらも距離と速度なので (\boldsymbol{H}=\boldsymbol{I})
- 一般には、デジタル温度計のように、測定値と状態が異なる物理領域に属することもある
- 多変量のカルマンゲインは次のとおり
[ \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} ]
- 例で計算されたカルマンゲインは次のとおり
[ \boldsymbol{K}_1= \begin{bmatrix} 0.4048 & 0.6377\ 0.0399 & 0.3144 \end{bmatrix} ]
- 行列の逆行列計算は MATLAB の
inv(A)や Python のnumpy.linalg.inv(A)で可能だが、実装では明示的な逆行列よりA\bやnumpy.linalg.solve(A, b)のように線形系を直接解く方法のほうが一般に望ましい
更新結果と共分散の減少
- この例のイノベーションは次のとおり
[ \boldsymbol{z}1-\hat{\boldsymbol{x}}{1,0}
\begin{bmatrix} 20\ 2 \end{bmatrix} ]
- カルマンゲインで補正項を計算すると次のようになる
[ \boldsymbol{K}_1 \begin{bmatrix} 20\ 2 \end{bmatrix}
\begin{bmatrix} 9.37\ 1.43 \end{bmatrix} ]
- 更新後の状態推定値は次のとおり
[ \hat{\boldsymbol{x}}_{1,1}
\begin{bmatrix} 11{,}009.37\ 201.43 \end{bmatrix} ]
- 多変量の共分散更新には、数値的に安定な 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{P}_{n,n}
(\boldsymbol{I}-\boldsymbol{K}n\boldsymbol{H}) \boldsymbol{P}{n,n-1} ]
- 厳密な算術では両者は同じ結果を与えるが、コンピュータ実装では Joseph form のほうが一般に数値的に安定している
- 例で単純化した式により計算した更新共分散は次のとおり
[ \boldsymbol{P}_{1,1}
\begin{bmatrix} 14.57 & 1.43\ 1.43 & 0.71 \end{bmatrix} ]
- 更新共分散の対角要素は、予測共分散 ((28.5, 1.25)) と測定共分散 ((36, 2.25)) の両方より小さい
- 新しい情報は不確実性が高くても推定不確実性を減らし、理論上は新しい測定を無視すべきではない
- 実際の実装では信頼できない測定値を棄却すべき場合もあり、外れ値処理の方法は書籍の Outlier Treatment 章で扱われている
次の予測と反復ループ
- Iteration 1 の予測段階は Iteration 0 と同じだが、開始点が更新後の (\hat{\boldsymbol{x}}{1,1}) と (\boldsymbol{P}{1,1}) に変わる
- 状態予測結果は次のとおり
[ \hat{\boldsymbol{x}}_{2,1}
\boldsymbol{F} \hat{\boldsymbol{x}}_{1,1}
\begin{bmatrix} 12{,}016.5\ 201.43 \end{bmatrix} ]
- 共分散予測結果は次のとおり
[ \boldsymbol{P}_{2,1}
\begin{bmatrix} 52.86 & 7.47\ 7.47 & 1.71 \end{bmatrix} ]
- 新しい測定なしに時間が経過すると、不確実性は自然に大きくなるため、予測段階で分散は再び増加する
- 速度の不確実性は距離の不確実性をさらに拡大する
- そのため、距離分散は速度分散より速く増加する
- この例はカルマンフィルタの3つの段階を示している
- 初期化: 開始時に1回だけ実行
- 予測: 動的モデルで次の状態と不確実性を伝播
- 更新: 新しい測定と予測をカルマンゲインで結合
- 初期化の後、カルマンフィルタは継続して 予測-更新ループ として動作する
1件のコメント
Hacker News のコメント
カルマンフィルターを単独で学ぶのは順序が逆で、周辺理論が開いてくれる大きな気づきを見落としやすい、といつも言うことになる
きちんと理解するには、最小二乗法（線形回帰）、再帰的最小二乗法、情報フィルター（KF の別の定式化）を順に見るのがよい
そうすると、KF は更新ステップの効率を優先するように再定式化した再帰的最小二乗法にすぎないことが分かる
この PDF が簡潔な概要を示している: http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/...
それでも知的好奇心はあるので、好奇心を保ちながら少しずつ理解へ進む道筋が必要だ
The Six (Not So) Easy Pieces を読み返しても理解はできないが、それでも価値はあり、Arnold の猫で遊ぶことで、厳密な科学的手順なしでも、裸の類人猿のような好奇心だけで、本来は文脈という扉の向こうにあった概念を体験できる
http://gerdbreitenbach.de/arnold_cat/cat.html
1 次元では、線形予測 X'1 = X0*a + b から事前分布を得て、mean(X'1) = mean(X0)*a + b、var(X'1) = var(X0)*a^2 となり、a と b は仮定したダイナミクスを表す
ガウスの事後分布は事前分布と観測値の精度加重平均なので、X1 = (1 - K)X'1 + YK であり、K = (1/var(X'1))/(1/var(X'1) + 1/var(Y))、Y はガウス観測値である
これを繰り返すとカルマンフィルターになり、多次元ガウスの線形性を知っていれば多次元へ一般化するのも直感的だ
ただし、多次元ガウスの線形性とガウス事後分布そのものは、簡単な内容ではないかもしれない
この話題が出るたびにこの資料も一緒に出てくるし、その逆も同じ: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Pyt...
Jupyter Notebook を使っている点もとても良い
確率分布のための記号計算ツールはまだないように思う
たとえば、多変量ガウス確率密度関数を 2 つ掛け合わせて共分散行列を得たり、カルマンフィルターの全構成要素（予測モデルと観測過程）を定義すると、必要な公式を sympy の lambdify のように取り出してくれるツールのことだ
ただ、Sympy がカルマンフィルターに必要な条件付き分布、つまりベイズ事後分布まで処理できるかは分からない
いずれにせよ Sympy でカルマンフィルターをいじりたいなら、平均と分散、または共分散行列を直接扱うほうがよい
参考: https://reference.wolfram.com/language/howto/WorkWithStatist...
それから: https://reference.wolfram.com/language/ref/MultinormalDistri...
https://www.squiggle-language.com/docs
Q と R が定数なら、通常そうであるように、ゲインは素早く収束し、カルマンフィルターは予測ステップ付きの指数フィルターとほとんど同じになる
多くの人にとってはこちらの説明のほうがずっと分かりやすく、実際の使い方にもよく合っている
通常、Q と R を手作業で調整して「よさそうに見える」まで合わせ、その後は変更しないからだ
さらに、Q や R のような複数の値を調整する代わりに、ゲイン 1 つだけを手作業で調整すればよい
結果がそれらしく見えるまでただ調整するのか？ それなら完全に過学習していない状況でも、どうしてきちんと動くのか分からない
たとえば映像で鳥を追跡するなら、ある Q を選ぶことはできるだろうが、時間帯によってノイズ統計が変わることがある。そういう場合はどうすればよいのか？
関連記事: Kalman filter from the ground up - https://news.ycombinator.com/item?id=37879715 - 2023 年 10 月、コメント 150 件
上のタイトルに入れるのに最もよい年が何年なのかも気になる
カルマンフィルターは、より一般的な主題である David G. Luenberger の Optimization by Vector Space Methods, John Wiley and Sons, Inc., New York, 1969 に含まれている
ふとこんなことを考えた。目撃証言しかない事件も、何らかの形でベクトルに符号化したうえでカルマンフィルターで扱い、観測の証拠価値を強めることはできるだろうか？
嘘と不正確さをどちらも「誤差」として扱うやり方だ
フェニックス・ライトや UFO 全般、幽霊、臨死体験、もっと日常的にはレイプの申し立てのようなものを思い浮かべている
最高の資料はほとんどいつもこれだ: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Pyt...
Python を使わない人にとっても素晴らしく、全体を本当によく概観してくれる
このテーマを学ぶとき、蝶ネクタイをした Michael van Biezem のカルマンフィルター講義を見た人はほかにもいる？
https://www.youtube.com/watch?v=CaCcOwJPytQ&list=PLX2gX-ftPV...
本当に知っておくべき一文はこれだ: 「このフィルターは Rudolf E. Kálmán（1930 年 5 月 19 日〜2016 年 7 月 2 日）にちなんで名付けられた。1960 年、Kálmán は離散データ線形フィルタリング問題の再帰的解法を説明した有名な論文を発表した」