粗大メモ置き場

個人用,たまーに来訪者を意識する雑記メモ

ベイズの定理を用いてカルマンゲインを導出する(行列版)

Background

カルマンフィルタにはたくさんの導出がありますが, 自分的には大まかに以下の2つのパターンを把握していれば実用上は十分だと考えています。

(なお超初心者の場合はとりあえずコードを書いて動かして見た方が理解しやすいと思います。)

  1. 制御工学のオブザーバから入る方法
  2. ベイズの定理(ベイズフィルタ)から導く方法

オブザーバから導出するパターン

前者の例として,足立先生の「カルマンフィルタの基礎」をおすすめします。

カルマンフィルタの基礎

カルマンフィルタの基礎

ネタバレすると,証明は線形の(濾波型)オブザーバを設計し,ガウス分布で表される誤差が線形に伝搬する性質を用いて推定後の共分散を最小化するオブザーバゲインKを求める(平方完成)となっています。

似たアプローチとして超平面に垂線をおろす図形的なアプローチが合った気がしますが忘れてしまいました… 思い出したら書きます。

ベイズの定理から導くパターン

こちらは「確率ロボティクス」の書籍を見ると詳しくわかると思います。

以下のベイズの定理と,


p(x|z) = \dfrac{p(z|x)p(x)}{p(z)}

ガウス分布


f(x)=\frac{1}{(\sqrt{2 \pi})^{n} \sqrt{|\Sigma|}} \exp \left(-\frac{1}{2}(x-\mu)^{\mathrm{T}} \Sigma^{-1}(x-\mu)\right)

の式から導出できます。

ssk0109.hatenablog.com

ベイズの定理から多変数カルマンフィルタのゲインを導出

確率ロボティクスに倣って導出します。 基本的な流れは先程のブログとほぼ同じです。書きやすいように表記を少しいじっていますが,恐らく意味を大きく損なわないと思います。

前提

とりあえずよく見る線形状態方程式を仮定します。


x_{n+1}=Ax_{n}+Bu_{n}+\omega \\
y_{n} = Cx_{n}+\nu

 \omega,\nuはそれぞれシステムノイズと観測ノイズでその分散は, R,Qとします。

予測

予測にはベイズの定理を使いません。

nステップでの推定値の分布の平均を \mu_n 分散 P_nとすると, 予測値の分布の平均 \hat \mu_nと分散 \hat P_nは,


\hat P_n = P_{n|n-1} = A P_{n-1} A^\top + R\\ 
\hat \mu_n = \mu_{n|n-1} = A \mu_{n-1} + B u_n

となります。

計測更新

ベイズの定理から予測における推定値 \hat xとセンサ出力 yの分布からセンサ情報を考慮したときの推定値の分布 p(x|y)がわかります。


p(x|y) = \dfrac{p(y|\hat x)p(\hat x)}{p(y)} = \eta p(y|\hat x)p(\hat x)



ここで, \hat xは平均 \hat \mu_nと分散 \hat P_nガウス分布なので,


p(\hat x)=\lambda_1 \exp \left(-\frac{1}{2}(x_n-\hat \mu_n)^\top \hat P_n^{-1}(x_n-\hat \mu_n)\right)

また,観測の分布は


p(y|\hat x)=\lambda_2 \exp \left(-\frac{1}{2}(C x_n- z_t)^\top Q^{-1}(C x_n- z_t)\right)

と表せます。



したがって,ベイズの定理を用いてこれらをかけ合わせた場合の推定値の確率分布は


p(x|y) = \eta \lambda_1 \lambda_2 \exp \left(-\frac{1}{2}(C x_n- y_n)^\top Q^{-1}(C x_n- y_n) - \frac{1}{2}(x_n-\hat \mu_n)^\top \hat P_n^{-1}(x_n-\hat \mu_n) \right)

となります。

このexpの肩が極小値になる時がガウス分布の極大,すなわち分布の平均値,つまり最尤値になるので,


J = \frac{1}{2} \left( (C x_n- y_n)^\top Q^{-1}(C x_n- y_n) + (x_n-\hat \mu_n)^\top \hat P_n^{-1}(x_n-\hat \mu_n) \right)

が極大となる x_nを求めます。


一回微分して=0の条件から,


\dfrac{\partial J}{\partial x_n} \times 2 = C^\top (Q^{-1}+ Q^{-1} {}^\top) (C x_n- y_n) + (\hat P_n^{-1}+ \hat P_n^{-1} {}^\top ) (x_n - \hat \mu_n) = 0

よって共分散行列は対称行列より


C^\top Q^{-1}(C x_n- y_n) + \hat P_n^{-1}(x_n-\hat \mu_n) = 0

したがって推定値の平均は,


x_n = (C^\top Q^{-1}C +\hat P_n^{-1})^{-1} (C^\top Q^{-1}y_n+\hat P_n^{-1}\hat \mu_n)



また,分散はJの二階微分から明らかで,


P_n =  C^\top Q^{-1}C +\hat P_n^{-1}

となります。

式変形によるカルマンゲインの導出

先程の展開からカルマンゲイン K


x_n = \hat x_n + K (y_n - C  \hat x_n)

を満たすので,係数比較から以下のように導けます。


K =  (C^\top Q^{-1}C +\hat P_n^{-1})^{-1} C^\top Q^{-1}



見慣れた形と違いますね?この式に右から (C\hat P_n C^\top +Q)(C\hat P_n C^\top +Q)^{-1}をかけて整理することでよく見るカルマンゲインの式が得られます。


K =  (C^\top Q^{-1}C +\hat P_n^{-1})^{-1} C^\top Q^{-1}(C\hat P_n C^\top +Q)(C\hat P_n C^\top +Q)^{-1} \\
= (C^\top Q^{-1}C +\hat P_n^{-1})^{-1}(C^\top Q^{-1}C\hat P_n C^\top + C^top) (C\hat P_n C^\top +Q)^{-1}\\
= (C^\top Q^{-1}C +\hat P_n^{-1})^{-1}(C^\top Q^{-1}C + \hat P_n^{-1}) \hat P_n C^\top(C\hat P_n C^\top +Q)^{-1}\\
= \hat P_n C^\top(C\hat P_n C^\top +Q)^{-1}

長くなりましたが,これが確率ロボティクスの教科書に倣ったベイズの定理からのカルマンゲインの導出になります。

教科書のほうが丁寧なのでそちらを参照していただければと思いますが,研修中や電車の中など時間のあるときに自分で1から計算することで理解がより深まるのではないかと思います。

余談

はてなTex記法

はてなブログTex記法を書こうとすると通常の書き方から大きく変えることが多かったため,回避策として以下のブログを参考にしました。

7shi.hateblo.jp