目的

カルマンフィルタの備忘録を残す。

カルマンゲインが、分散が小さいほうの状態に重みをつけることで、カルマンフィルタは真の状態に近い状態を推定している。

カルマンフィルタの概要

状態空間モデルが与えられたとする。

\begin{eqnarray}
x(i)&=&ax(i-1)\\
y(i)&=&x(i)+\varepsilon(i), \ \ \varepsilon(i)\sim N(0,\sigma(i))
\end{eqnarray}

カルマンフィルタは、真の状態$x(i)$と推定値$\hat{x}(i)$の分散$V[\hat{x}(i)-x(i)]$が最小となるようにすることで、カルマンフィルタは$\hat{x}(i)$を推定する。

カルマンフィルタによる推定値$\hat{x}(i)$は、状態空間モデルと推定値$\hat{x}(i-1)$から算出される$\hat{x}_M(i)$と、観測値$\hat{x}_O(i)$および、カルマンゲイン$K_i$から構成される。

\hat{x}(i) = (1-K_i)\hat{x}_{M}(i) + K_i \hat{x}_{O}(i)

このとき、分散$V[\hat{x}(i)-x(i)]$は、

V[\hat{x}(i)-x(i)]=(1-K_i)^2 V[\hat{x}_M(i)]+K_i^2 V[\hat{x}_O(i)]+V[x(i)]

分散$V[\hat{x}(i)-x(i)]$が最小となるカルマンゲイン$K_i$は、分散$V[\hat{x}(i)-x(i)]$をカルマンゲイン$K_i$で微分すれば求まる。

K_i=\dfrac{V[\hat{x}_M(i)]}{V[\hat{x}_M(i)]+V[\hat{x}_O(i)]}

カルマンゲインが、分散が小さいほうの状態に重みをつけることで、カルマンフィルタは真の状態に近い状態を推定している。