Background

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

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

制御工学のオブザーバから入る方法
ベイズの定理（ベイズフィルタ）から導く方法

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

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

カルマンフィルタの基礎

作者:足立修一,丸田一郎
発売日: 2012/10/10
メディア: 単行本（ソフトカバー）

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

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

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

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

確率ロボティクス (Mynavi Advanced Library)

作者:Sebastian Thrun,Wolfram Burgard,Dieter Fox
発売日: 2015/04/02
メディア: Kindle版

以下のベイズの定理と，

$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}$