ベイズの定理からカルマンフィルタを導出する
目次
はじめに
こんにちは.ササキ(@saitosasaki)です.
私はカルマンフィルタを雰囲気で使っていたため,前々からベイズの定理からカルマンフィルタがどうやって導出されるのかもよくわかっていませんでした.(最小分散推定からの導出は知ってたんですが)
しかし,ふと『確率ロボティクス』を読み返してみると普通に書いてありました.最初に読むとき飛ばして良いと書いてあったので,飛ばして読んでいたみたいです.読んでみると非常に明快で、自分の中の理解が大いに深まりました。
今回は自分の記憶に刻むために状態空間モデルが時不変かつ一次元という簡単な条件でベイズの定理からカルマンフィルタを導出してみます。
追記:制御/ロボティクス系の書籍では、片山先生の『非線形カルマンフィルタ』でもベイズの定理からカルマンフィルタの導出をしています。
そもそもベイズとは
ベイズ(確率)というものを理解するには以下のスライドがわかりやすいと思います.
5分でわかるベイズ確率
ベイズの定理に関しては以下のスライドがわかりやすいと思います.
15分でわかる(範囲の)ベイズ統計学
そもそもカルマンフィルタとは
以下の記事がグラフィカルでわかりやすいと思います.
シンプルなモデルとイラストでカルマンフィルタを直観的に理解してみる - Qiita
導出に出てくる用語について
用語は基本、以下の確率ロボティクスの展望という論文に準拠しています。
以下の論文の「2.おさらい」のマルコフ過程とベイズの公式をチェックしてください。
確率ロボティクスの展望
カルマンフィルタの導出
モデル
- 状態遷移モデル
ここでと
は時刻tにおける状態量で,
は同時刻における制御量です。また,
は平均0,分散
の正規分布に従うノイズです。
- 計測モデル
ここでは時刻tにおける観測量です。また,
は平均0,分散
の正規分布に従うノイズです。
予測更新
(のせいで式変形が面倒になっているため、最初は計測更新の方から読んだ方が良いかもしれないです。もしくは予測更新の部分はベイズの定理使ってないですし、飛ばしても大丈夫です。)
予測更新は1次のマルコフ性を仮定することで、以下で表せられます。
ここで
状態遷移確率:
(平均,分散
の正規分布)
1つ前のデータの事後信念:
(平均,分散
の正規分布)
です.
信念
が正規分布であることを確認する
まず,式(1)を式(2),(3)を用いて正規分布で表わすと
となります.ここで、exponentialの係数は、まとめて正規化定数ηを用いて表しています.
略記して
とします。ここでは以下で表せます.
式(5)はについての積分を含んでおり、これを解くためには
を式変形します。具体的には
のように分解します。こうすることでに無関係な変数を、
についての積分から切り離すことができます。
このことに注意してを式変形すると、
となります。ここでが
に依存しないように関数
を選べば、正規化定数
に
を組み込むことができます。そこで
に依存しないように関数
を決定していきます。
まず、の一次、二次の導関数を計算すると、
となります。の時、
の平均値が得られるので、この等式を
に対して解くと、
となります。この結果を利用してを以下のように決めます。
このように定義すれば、
は変数に対する確率密度関数になります。つまりこれは
が成り立つということです。よっては
に依存しないため
の正規化定数
に
を組み込むことができることがわかりました。よって
は以下のように変形できます。
次にまだを決定していないので決定します。
は以下で決定できます。
ここでを用いて
を消去すると
となります。は
の二次関数なので、
は正規分布だとわかります。この分布の平均値と共分散は「
が最小値の時の
」と「
の曲率」にあたります。
(ここ、「なんで?」って思う方がいるんですが、正規分布の式を考えれば自然とわかると思います。)
信念
の平均値と共分散を求める
まず、この分布の平均値を求めていきます。の一次導関数は
となります。の最小値は
の時に得られるので、この等式を
について解くと
となります。
次に、の共分散を求めます。
の逆数が共分散になるので、二次導関数は式(18)を微分して
となります。以上より予測更新の式は
となります。
計測更新
計測は過去には依存しないという仮定の基、計測更新はベイズの定理から正規化定数を用いて以下で表せられます。
ここで
計測確率:
(平均,分散
の正規分布)
予測更新で求めた事前信念:
(平均,分散
の正規分布)
です。
を正規分布で表わすと
ここで、は以下の通りです。
は
の二次関数なので、
は正規分布だとわかります。なので今回も
の一次導関数を求めて
の平均値
を求めます。
を解くと、
ここでカルマンゲイン
を定義するとは以下のようになります。
次に、の共分散を求めます。
の逆数が
の共分散になるので、二次導関数は
となります。以上よりの共分散
は
先ほど求めたカルマンゲインですが、事後信念の共分散を用いるのは計算上不便なので、上式より事前信念の共分散
を用いた式に書き換えます。
またカルマンゲインを用いて、を書き換えると、
となります。
以上より、観測更新の式は
となります.
カルマンフィルタの式まとめ
まとめると以下のようになります。
予測更新
平均値:
分散:
計測更新
カルマンゲイン:
平均値:
分散:
参考文献
- 訳:上田、著:S.スラン et all,"確率ロボティクス 3.2.4 カルマンフィルタの数学的導出"