ssk tech blog

自律移動ロボットの話を書きます

ベイズの定理からカルマンフィルタを導出する

Robot ベイズ

目次

はじめに

こんにちは．ササキ(@saitosasaki)です．
私はカルマンフィルタを雰囲気で使っていたため，前々からベイズの定理からカルマンフィルタがどうやって導出されるのかもよくわかっていませんでした．(最小分散推定からの導出は知ってたんですが）
しかし，ふと『確率ロボティクス』を読み返してみると普通に書いてありました．最初に読むとき飛ばして良いと書いてあったので，飛ばして読んでいたみたいです．読んでみると非常に明快で、自分の中の理解が大いに深まりました。
今回は自分の記憶に刻むために状態空間モデルが時不変かつ一次元という簡単な条件でベイズの定理からカルマンフィルタを導出してみます。

追記：制御/ロボティクス系の書籍では、片山先生の『非線形カルマンフィルタ』でもベイズの定理からカルマンフィルタの導出をしています。

そもそもベイズとは

ベイズ(確率)というものを理解するには以下のスライドがわかりやすいと思います．
5分でわかるベイズ確率

ベイズの定理に関しては以下のスライドがわかりやすいと思います．
15分でわかる(範囲の)ベイズ統計学

そもそもカルマンフィルタとは

以下の記事がグラフィカルでわかりやすいと思います．
シンプルなモデルとイラストでカルマンフィルタを直観的に理解してみる - Qiita

導出に出てくる用語について

用語は基本、以下の確率ロボティクスの展望という論文に準拠しています。 以下の論文の「2.おさらい」のマルコフ過程とベイズの公式をチェックしてください。
確率ロボティクスの展望

カルマンフィルタの導出

モデル

  • 状態遷移モデル
    x_t = a x_{t-1} + b u_t + \epsilon_t
    ここでx_tとx_{t-1}は時刻tにおける状態量で，u_tは同時刻における制御量です。また，\epsilon_tは平均0，分散rの正規分布に従うノイズです。
  • 計測モデル
    z_t=c x_t + \delta_t
    ここでz_tは時刻tにおける観測量です。また，\delta_tは平均0，分散qの正規分布に従うノイズです。

予測更新

(x_{t-1}のせいで式変形が面倒になっているため、最初は計測更新の方から読んだ方が良いかもしれないです。もしくは予測更新の部分はベイズの定理使ってないですし、飛ばしても大丈夫です。)
予測更新は1次のマルコフ性を仮定することで、以下で表せられます。

\overline{bel}(x_t) = p(x_t \vert z_{1:t-1},u_{1:t})
=\int p(x_t \vert x_{t-1},z_{1:t-1},u_{1:t}) bel(x_{t-1})dx_{t- 1}
=\int p(x_t \vert x_{t-1},u_t) bel(x_{t-1})dx_{t-\ 1}\tag{1}

ここで
状態遷移確率： p(x_t \vert x_{t-1},u_t) \sim N(x_t,a x_{t-1} + b u_t,r) \tag{2}
(平均a x_{t-1} + b u_t,分散rの正規分布)
1つ前のデータの事後信念： bel(x_{t-1}) \sim N(x_{t-1},\mu_{t-1},\sigma_{t-1}^{2}) \tag{3}
(平均\mu_{t-1},分散\sigma_{t-1}^{2}の正規分布)
です．

信念 \overline{bel}(x_t)が正規分布であることを確認する

　まず，式(1)を式(2),(3)を用いて正規分布で表わすと

\overline{bel}(x_t)=　\eta \int exp(-\frac{1}{2} r^{-1}(x_t -ax_{t-1}-bu_t)^{2}) \cdot exp(-\frac{1}{2} \sigma_{t-1}^{-2}(x_{t-1} -\mu_{t-1})^{2})dx_{t-1} \tag{4}

となります．ここで、exponentialの係数は、まとめて正規化定数ηを用いて表しています．
略記して

\overline{bel}(x_t)=\eta \int exp(-L_t)dx_{t-1} \tag{5}

とします。ここで L_t は以下で表せます．

L_t = \frac{1}{2}r^{-1}(x_t - a x_{t-1} -b u_t)^{2}+ \frac{1}{2}\sigma_{t-1}^{-2}(x_{t-1}-\mu_{t-1})^{2} \tag{6}

　式(5)はx_{t-1}についての積分を含んでおり、これを解くためには L_tを式変形します。具体的には

L_t=L_t(x_{t-1},x_t)+L_t(x_t) \tag{7}

のように分解します。こうすることでx_{t-1}に無関係な変数を、x_{t-1}についての積分から切り離すことができます。
このことに注意して\overline{bel}(x_t)を式変形すると、

\overline{bel}(x_t)=\eta \int exp(-L_t)dx_{t-1}
= \eta \int exp(-L_t(x_{t-1},x_t)-L_t(x_t))dx_{t-1}
= \eta \cdot exp(-L_t(x_t))\int exp(-L_t(x_{t-1},x_t))dx_{t-1} \tag{8}

となります。ここで\int exp(-L_t(x_{t-1},x_t))dx_{t-1}がx_tに依存しないように関数L_t(x_{t-1},x_t)を選べば、正規化定数\etaに\int exp(-L_t(x_{t-1},x_t))dx_{t-1}を組み込むことができます。そこでx_tに依存しないように関数L_t(x_{t-1},x_t)を決定していきます。
　まず、L_tの一次、二次の導関数を計算すると、

\frac{\partial L_t}{\partial x_{t-1}}=-ar^{-1}(x_t-ax_{t-1}-bu_t)+\sigma_{t-1}^{-2}(x_{t-1} -\mu_{t-1}) \tag{9}
\frac{\partial^{2} L_t}{\partial x_{t-1}^{2}}=a^{2}r^{-1}+\sigma_{t-1}^{-2}:=\Psi_t^{-1} \tag{10}

となります。\frac{\partial L_t}{\partial x_{t-1}}=0の時、 \overline{bel}(x_t)の平均値が得られるので、この等式をx_{t-1}に対して解くと、

ar^{-1}(x_t-ax_{t-1}-bu_t)=\sigma_{t-1}^{-2}(x_{t-1} -\mu_{t-1})
\Longleftrightarrow x_{t-1}=\Psi_t[ar(x_t-bu_t)+\sigma_{t-1}^{2}\mu \tag{11}

となります。この結果を利用してL_t(x_{t-1},x_t)を以下のように決めます。

L_t(x_{t-1},x_t)=\frac{1}{2}\Psi_t(x_{t-1}-\Psi_t(ar(x_t-bu_t)+\sigma_{t-1}^{2}\mu))^2 \tag{12}

このように定義すれば、

det(2\pi\Psi)^{-\frac{1}{2}}exp(-L_t(x_{t-1},x_t)) \tag{13}

は変数x_{t-1}に対する確率密度関数になります。つまりこれは

\int det(2\pi\Psi)^{-\frac{1}{2}}exp(-L_t(x_{t-1},x_t))dx_{t-1}=1 \tag{14}

が成り立つということです。よってL_t(x_{t-1},x_t)はx_tに依存しないためL_tの正規化定数\etaに\int exp(-L_t(x_{t-1},x_t))dx_{t-1}を組み込むことができることがわかりました。よって\overline{bel}(x_t)は以下のように変形できます。

\overline{bel}(x_t) = \eta \cdot exp(-L_t(x_t))\int exp(-L_t(x_{t-1},x_t))dx_{t-1}
= \eta \cdot exp(-L_t(x_t)) \tag{15}

次にまだL_t(x_t)を決定していないので決定します。L_t(x_t)は以下で決定できます。

L_t(x_t) = L_t - L_t(x_{t-1},x_t)
=\frac{1}{2}r(x_t-ax_{t-1}-bu_t)^{2}+\frac{1}{2}r(x_{t-1}-\mu_t)^{2}-\frac{1}{2}\Psi_t(x_{t-1}-\Psi_t[ar(x_t-bu_t)+\sigma_{t-1}^{-2}\mu)^{2} \tag{16}

ここで\Psi_t=(a^{2}r^{-1}+\sigma_{t-1}^{-2})^{-1}を用いてx_{t-1}を消去すると

L_t(x_t) =\frac{1}{2}r^{-1}(x_t-bu_t)^{2} +\frac{1}{2}\mu_{t-1}^{2}\sigma_{t-1}^{-2}-\frac{1}{2}(a^{2}r^{-1}+\sigma_{t-1}^{-2})(ar^{-1}(x_t-bu_t)+\sigma_{t-1}^{-2}\mu_{t-1})^{2} \tag{17}

となります。L_t(x_t)はx_tの二次関数なので、\overline{bel}(x_t)は正規分布だとわかります。この分布の平均値と共分散は「L_t(x_t)が最小値の時のx_t」と「L_t(x_t)の曲率」にあたります。
（ここ、「なんで？」って思う方がいるんですが、正規分布の式を考えれば自然とわかると思います。）

信念 \overline{bel}(x_t)の平均値と共分散を求める

　まず、この分布の平均値を求めていきます。 L_t(x_t)の一次導関数は

\frac{\partial L_t(x_t)}{\partial x_{t}}=r^{-1}(x_t-bu_t)-r^{-1}a(a^{2}r^{-1}+\sigma_{t-1}^{-2})^{-1}(a r^{-1}(x_t-bu_t)+\sigma_{t-1}^{-2}\mu_{t-1}
=(r^{-1}-r^{-2}a^{2}(a^{2}r+\sigma_{t-1}^{-2})^{-1})(x_t-bu_t)-r^{-1}a(a^{2}r+\sigma_{t-1}^{-2})^{-1}\sigma_{t-1}^{-2}\mu_{t-1}
=(r+a^{2}\sigma_{t-1})^{-2}(x_t-bu_t)-r^{-1}a(a^{2}r^{-1}+\sigma_{t-1}^{-2})^{-1}\sigma_{t-1}^{-2}\mu_{t-1} \tag{18}

となります。 L_t(x_t)の最小値は\frac{\partial L_t(x_t)}{\partial x_{t}} \vert_{x_t=\overline{\mu_t}}=0の時に得られるので、この等式を\overline{\mu_t}について解くと

\overline{\mu_t}=bu_t+(r+a^{2}\sigma_{t-1}^{2})r^{-1}a(a^{2}r+\sigma_{t-1}^{-2})^{-1}\sigma_{t-1}^{-2}\mu_{t-1}
=bu_t+a(1+\sigma_{t-1}^{2}a^{2}r)(\sigma_{t-1}^{2}a^{2}r+1)^{-1}\mu_{t-1}
=bu_t+a\mu_{t-1} \tag{19}

となります。
　次に、\overline{bel}(x_t)の共分散を求めます。\frac{\partial^{2} L_t(x_t)}{\partial x_{t}^{2}}の逆数が共分散になるので、二次導関数は式(18)を微分して

\frac{\partial^{2} L_t(x_t)}{\partial x_{t}^2}=(a^{2}\sigma_{t-1}^{2}+r)^{-1} \tag{20}

となります。以上より予測更新の式は

\overline{\mu_t}=a\mu_{t-1}+bu_t \tag{19}
\overline{\sigma_t^{2}}=a^{2}\sigma_{t-1}^{2}+r \tag{20}

となります。

計測更新

計測は過去には依存しないという仮定の基、計測更新はベイズの定理から正規化定数\etaを用いて以下で表せられます。

bel(x_t)=\eta p(z_t \vert x_t,z_{1:t-1},u_{1:t}) \overline{bel}(x_t)
=\eta p(z_t \vert x_t) \overline{bel}(x_t) \tag{21}

ここで
計測確率： p(z_t \vert x_t) \sim N(z_t,c x_t,q) \tag{22}
(平均c x_{t} ,分散qの正規分布)
予測更新で求めた事前信念： \overline{bel}(x_t) \sim N(x_{t},\overline{\mu_{t}},\overline{\sigma_{t}^{2}}) \tag{23}
(平均\overline{\mu_{t}},分散\overline{\sigma_{t}^{2}}の正規分布)
です。
bel(x_t)を正規分布で表わすと

bel(x_t)=\eta p(z_t \vert x_t) \overline{bel}(x_t)
= ηexp(-J_t) \tag{24}

ここで、J_tは以下の通りです。

J_t=\frac{1}{2}q^{-1}(z_t-cx_t)^{2}+\frac{1}{2}\overline{\sigma_t^{2}}^{-1}(x_t-\overline{\mu_t})^{2} \tag{25}

J_tはx_tの二次関数なので、bel(x_t)は正規分布だとわかります。なので今回もJ_tの一次導関数を求めてbel(x_t)の平均値\mu_tを求めます。

\frac{\partial J_t}{\partial x_{t}}=-cq^{-1}(z-cx_t)+\overline{\sigma_t^{2}}^{-1}(x_t-\overline{\mu_t}) \tag{26}

\frac{\partial J_t}{\partial x_{t}} \vert_{x_t=\mu_t}=0を解くと、

cq^{-1}(z_t-c\mu_t)=\overline{\sigma_t^{2}}^{-1}(\mu_t-\overline{\mu_t})
\Longleftrightarrow \mu_t=\overline{\mu_t}+\sigma_t^{2}cq^{-1}(z_t-c\overline{\mu_t}) \tag{27}

ここでカルマンゲイン

K_t=\sigma_t^{2}cq^{-1} \tag{28}

を定義すると\mu_tは以下のようになります。

\mu_t=\overline{\mu_t}+K_t(z_t-c\overline{\mu_t}) \tag{29}

　次に、bel(x_t)の共分散を求めます。\frac{\partial^{2} J_t(x_t)}{\partial x_{t}^2}の逆数がbel(x_t)の共分散になるので、二次導関数は

\frac{\partial^{2} J_t(x_t)}{\partial x_{t}^{2}}=c^{2}q^{-1}+\overline{\sigma_t}^{-2} \tag{30}

となります。以上よりbel(x_t)の共分散\sigma_t^{2}は

\sigma_t^{2}＝（c^{2}q^{-1}+\overline{\sigma^{2}}_t^{-1})^{-1} \tag{31}

先ほど求めたカルマンゲインですが、事後信念の共分散\sigma_tを用いるのは計算上不便なので、上式より事前信念の共分散\overline{\sigma_t}を用いた式に書き換えます。

K_t=\sigma_t^{2}cq^{-1}
=（c^{2}q^{-1}+\overline{\sigma}_t^{-2})^{-1}cq^{-1}
=\overline{\sigma^{2}}_tc(c^{2}\overline{\sigma^{2}}+q)^{-1} \tag{32}

またカルマンゲインを用いて、\sigma_t^{2}を書き換えると、

\sigma_t^{2}＝（c^{2}q^{-1}+\overline{\sigma_t^{2}}_t^{-1})^{-1}
=(1-\overline{\sigma_t}c^{2}(q+c^{2}\overline{\sigma_t^{2}})^{-1})\overline{\sigma_t^{2}}
=(1-K_tc)\overline{\sigma_t}^{2} \tag{33}

となります。
以上より、観測更新の式は

K_t=\overline{\sigma_t^{2}}_tc(c^{2}\overline{\sigma_t^{2}}+q)^{-1} \tag{32}
\mu_t=\overline{\mu_t}+K_t(z_t-c\overline{\mu_t}) \tag{29}
\sigma_t^{2}=(1-K_tc)\overline{\sigma_t^{2}} \tag{33}

となります．

カルマンフィルタの式まとめ

まとめると以下のようになります。

予測更新

平均値：\overline{\mu_t}=a\mu_{t-1}+bu_t
分散： \overline{\sigma_t^{2}} = a^{2} \sigma_{t-1} ^{2}+ r

計測更新

カルマンゲイン：K_t=\overline{\sigma}^{2}_tc(c^{2}\overline{\sigma_t}^{2}+q)^{-1}
平均値：\mu_t=\overline{\mu_t}+K_t(z_t-c\overline{\mu_t})
分散：\sigma_t^{2}=(1-K_tc)\overline{\sigma_t^{2}}

参考文献

  • 訳：上田、著：S.スラン et all,"確率ロボティクス 3.2.4 カルマンフィルタの数学的導出"

関連記事

ssk0109.hatenablog.com