カルマンフィルタのゲイン導出メモと共分散行列の遷移

注：これは個人用メモです。

はじめに

もう一度いいますがこれは個人用メモです。

わかりやすい説明などは以下のブログ様などを参考にしたほうが良いです。

hamachannel.hatenablog.com

差別点は

• 多変量の場合を考慮している点
• 変分ではなく平方完成で導出を行っている点

でしょうか。自分用に覚書がほしかった箇所をメモっています。

本記事の流れ

以下の流れで行きます。

• オブザーバ（濾波型）の推定式からk+1の時の誤差共分散を導出
• 誤差共分散行列を平方完成して最適ゲイン＝カルマンゲインを導出
• その時の共分散の更新則を導出

推定に依る誤差共分散の式

• 予測ステップは以下の通りとします。uは入力，vはシステムノイズです。

• 修正ステップは以下の様に書けます。ここでFはフィードバックゲインです。

また，観測方程式からですのでこれを代入して

したがって，誤差共分散は各々の要素が独立だとして以下のように書けます。

そろそろ面倒なのでここからとして書いてしまいます。

• 
• の関係を用いて書き下すと，

となります。

誤差共分散を最小化するゲインの導出

後は，変分法でもいいのですが今回は平方完成を用います。

簡単のため，,そしてと置きます。

先ほどの式をFに関する2次系にすると

となります。

したがってこれを最小化するゲインFは自明に

となるわけです。（公式と見比べるためにとしました。）

またこの時，最小値は

となり，公式と同様の値が導出できます。

最適ゲインでないゲインを用いた場合の共分散

最適でないゲインを用いた際の誤差共分散は

という簡単な式ではなく，

をきちんと計算しなくてはいけません。

カルマンフィルタの公式だけを眺めていて一度間違った式を使っていたことがあります。

疑問

ゲインFに何らかの制約があった時に，以下の式

を最小化（ノルムを？）するにはどうすればいいんでしょう？高校数学的な解法は線形代数の場合はどの様に計算するんでしょうね？？？

追記：LMIの目的関数にしてYalmipで解いてみましたがソルバがないと言われてしまいました。目的関数は非線形になるんですが，凸っぽいような…？

参考文献

今回の導出は足立先生の本にのっています。ベイズ統計などは確率ロボティクスなどを読むとよいです。 ベイズの定理に従うと勝手に最尤値を計算してしまうので今回のような導出にはなりませんが。

カルマンフィルタの基礎

確率ロボティクス (ROBOT books)

線形代数は以下の本がいいそうで…

統計のための行列代数 上

統計のための行列代数 下