カルマンフィルタは逐次ベイズフィルタの特殊系として言われますが、式を眺めていても結びつきがさっぱりイメージできなかったので数式いじりをしました。
もっとしっかりした入門は他でたくさん見かけますので、
参考文献として記事のリンクしておきます。
数式操作に関しては須山さんの本を参考にしました。
ベイズ推論による機械学習入門
- 作者: 須山敦志
- 出版社/メーカー: 講談社
- 発売日: 2017/10/21
逐次的ベイズ推定から見たカルマン・フィルタ
セオリーと違ってカルマンフィルタで得たい出力値 状態量 $ \hat{x}_{k|k} $ の位置付けから入ります。理解を簡単にするため、システムとしてD次元の酔歩 $ x_k \in \mathbb{R}^D $と
酔歩する粒子の観測誤差を含む観測値 $ y_{k} \in \mathbb{R}^D $ で考察します。
\[
\begin{array}{rll}
x_{n+1} &= x_{n} + w_n & \qquad w_n \sim \mathcal{N}(0, Q) \quad Q \in \mathbb{R}^{D \times D} \\
y_{n} &= x_{n} + v_n & \qquad v_n \sim \mathcal{N}(0, R) \quad R \in \mathbb{R}^{D \times D} \\
\end{array}
\]
観測値 $ y_n $ は真の平均値のわからない正規分布 $ \mathcal{N}(x_n, R) $ から取り出されたもので、
このn=1のデータからは、せいぜい $ \hat{x}_n = y_n $ とするのが関の山です。
しかし、うまく $ x_{n+1} - x_{n} \sim \mathcal{N}(0, Q) $ と、
観測したデータ $ Y_n = \{Y_{n-1}, y_{n}\} $ を使って、より高い精度で $ x_n $ を推定することを考えます。
カルマンフィルタの式
システムをざっくり簡略化したので、カルマンフィルタの式もザクッと簡単になります。この式は逐次ベイズ推定から求めた結果とあとから比較するために掲載しました。
- 予測のステップ
-
\[
\begin{array}{rl}
\hat{x}_{k|k-1} &= \hat{x}_{k-1|k-1} \\
P_{k|k-1} &= P_{k-1|k-1} + Q \\
\end{array}
\]
- フィルタリングのステップ
-
\[
\begin{array}{rl}
e_k &= y_k - \hat{x}_{k|k-1} \\
S_k &= R + P_{k|k-1} \\
K_k &= P_k S_k^{-1} = P_k (R + P_{k|k-1} )^{-1}\\
\hat{x}_{k|k} &= \hat{x}_{k|k-1} + K_k e_k\\
P_{k|k} &= (I - K_k) P_{k|k-1} \\
\end{array}
\]
フィルタステップ
観測データ $ Y_n $から状態量 $ x_n $ の確率密度分布 $ p(x_n|Y_n) $ を求めます。確率密度分布にはベイズの定理から下記の性質があります。赤字では $ y_n $ と $ Y_{n-1} $ の独立性(マルコフ性)を用いました。
\[
\begin{array}{rll}
p(x_n \vert Y_n) & = p(x_n | y_n, Y_{n-1}) \\
& \propto \color{red}{p(y_n | x_n, Y_{n-1})} p(x_n | Y_{n-1})\\
& = \color{red}{p(y_n | x_n)} p(x_n | Y_{n-1})\\
\end{array}
\]
ここで、事前分布として $ p(x_n | Y_{n-1}) = \mathcal{N}(x_n | \hat{x}_{n \vert n-1}, P_{n \vert n-1}) $ をカルマンフィルタでは仮定する。このとき、$ p(y_n \vert x_n) = \mathcal{N}(y_n | x_n, R) $ であるので、
\[
\begin{array}{rll}
\ln \left(p(y_n | x_n) p(x_n | Y_{n-1}) \right) &= \ln \mathcal{N}(y_n | x_n, k) + \ln \mathcal{N}(x_n | \hat{x}_{n|n-1}, P_{n|n-1}) \\
&= - \frac{1}{2} \left\{ x_n^T (\color{green}{R^{-1}+P_{n|n-1}^{-1}}) x_n - 2 x_n^T \color{blue}{(R^{-1} y_n + P_{n|n-1}^{-1} \hat{x}_{n|n-1}}) \right\} + \text{const.}
\end{array}
\]
この数式の形状により $ p(x_n \vert Y_n) $ がガウス分布であることがわかるので (須山本の手法です)、
\[
p(x_n \vert Y_n) = \mathcal{N}(x_n \vert \hat{x}_{n|n}, P_{n|n}) \\
\]
とおくと
\[
\begin{array}{rl}
\ln p(x_n \vert Y_n) &= \left( -\frac{1}{2}(x_n - \hat{x}_{n|n})^T P_{n|n}^{-1} (x_n - \hat{x}_{n|n}) - \ln | P_{n|n}| \right) + \text{const}\\
& = -\frac{1}{2} \left( x_n^T \color{green}{P_{n|n}^{-1}} x_n - 2 x_n^T \color{blue}{P_{n|n}^{-1} \hat{x}_{n|n}} \right) + \text{const.}
\end{array}
\]
ここから各パラメータの比較をします。すると
\[
\begin{array}{crl}
&P^{-1}_{n|n} &= \color{green}{R^{-1} + P_{n|n-1}^{-1}}\\
\Leftrightarrow & P_{n|n} &= \left( 1- P_{n|n-1}(P_{n|n-1}+R)^{-1} \right) P_{n|n-1} \\
&& = \left( 1- K_n \right) P_{n|n-1} \\
\end{array}
\]
と誤差共分散の更新式が求まります。さらに目当ての推定値が
\[
\begin{array}{crl}
& P_{n|n}^{-1} \hat{x}_{n|n} &= \color{blue}{ R^{-1} y_n + P_{n|n-1}^{-1} \hat{x}_{n|n-1} }\\
\Leftrightarrow & \hat{x}_{n|n} &= P_{n|n} \left( R^{-1} y_n + P_{n|n-1}^{-1} \hat{x}_{n|n-1} \right) \\
&& = \hat{x}_{n|n-1} + P_{n|n-1}(P_{n|n-1}+R)^{-1} (y_n - \hat{x}_{n|n-1}) \\
&& = \hat{x}_{n|n-1} + K_n e_n \\
\text{where} & K_n &= P_{n|n-1}(P_{n|n-1}+R)^{-1} \\
& e_n &= y_n - \hat{x}_{n|n-1}
\end{array}
\]
と求まり、$ K $が、このシステムにおける最適カルマンゲインになっているなど、カルマンフィルタの更新ステップの式になっています。
$ \hat{x}_{n|n} $は、条件付き確率密度分布 $ p(x_n \vert Y_n) $ に従う、$ x_n $ の条件付き期待値 $ \mathbb{E}(x_n|Y_n) $ ですので、
これ自体が最小分散推定になっています(証明は省略. 参考文献[4]参照)。
予測のステップ
先ほどのフィルタリングのステップでは、事前分布として$ p(x_n | Y_{n-1}) = \mathcal{N}(x_n | \hat{x}_{n \vert n-1}, P_{n \vert n-1}) $を用いていました。この事前分布のパラメータ、$ \hat{x}_{n|n-1} $と$ P_{n|n-1} $を求めます。
$ x_{n+1} - x_{n} \sim \mathcal{N}(0, Q) $ であることから、
その1ステップ前のフィルタリングステップで求めた粒子位置に関する確率密度分布 $ p(x_{n-1}|Y_{n-1}) = \mathcal{N}(x_{n-1}|\hat{x}_{n-1|n-1}, P_{n-1|n-1}) $ に対して、
正規分布の加法性から
\[
\begin{array}{rl}
\hat{x}_{n|n-1} &= \hat{x}_{n-1|n-1}\\
P_{n|n-1} &= P_{n-1|n-1} + Q\\
\end{array}
\]
となります。そのままですね。
初期値
さて、同じようにしてフィルタリングステップ→予測ステップ→フィルタリングと、ステップをどんどんさかのぼっていくと、最初の位置 $ \hat{x}_{0|0} $, そしてその位置に関する共分散 $ P_{0|0} $ に行き着きます。
これはハイパーパラメータというか、えいやと決めるものになります。
システムがもう少し複雑な場合
前節では酔歩を仮定しましたが、一般の線形システムとして、
\[
\begin{array}{rl}
x_{n+1} &= A x_n + B u_n + w_n \\
y_n &= C x_n + v_n
\end{array}
\]
について考えます。このとき、
\[
\begin{array}{rl}
p(x_{n+1}|x_{n}) &= \mathcal{N}(A x_n + B u_n, Q) \\
p(y_n|x_n) &= \mathcal{N}(C x_n, R)
\end{array}
\]
であって、他の確率分布についても正規分布の再生性をふまえて
\[
\begin{array}{rl}
x &\sim \mathcal{N}(\mu, \Sigma) \\
\Rightarrow G x &\sim \mathcal{N}(G \mu, G^T \Sigma G)
\end{array}
\]
に注意すれば、もとのカルマンフィルタの式になるのは自明ですね。
参考文献
- シンプルなモデルとイラストでカルマンフィルタを直観的に理解してみる - Qiita
- ベイズの定理からカルマンフィルタを導出する - ssk tech blog
- データ同化｜カルマンフィルタと尤度 - ari23の研究ノート
- 知能制御システム学 画像追跡 (3) ― ベイズ推定とパーティクルフィルタ ― 東北大学 大学院情報科学研究科 鏡 慎吾 准教授
- カルマンフィルタの誤差共分散の意味 - 夜間飛行
