##カルマンフィルタとは
「離散時間システムの出力から、システムの状態を最小二乗の意味で推定する」
##問題設定
今回はもっとも簡単な線形時不変の方程式
$ x(k+1)= A x(k) + Bu(k) + Gw(k)$
$ y(k) = Cx(k) + v(k)$
を考える。 ここで、$w(k),v(k)$はそれぞれ白色ガウスノイズであることを仮定し、その平均と共分散は
$E[w(k)] = E[(v_k)] = 0$
$E[w(k)w(k)^T] = Q, E[v(k)v(k)^T] = R$
であるとする。$w(k)$と$v(k)$の間に相関はないものとする。
初期値もガウスであるとし、その平均と分散は
$E[x(0)] = \bar x(0)$
$E[(x(0)-\bar x(0))(x(0)-\bar x(0))^T] = \Sigma_0$
であるとする。
今、時刻$k-1$までの入力${u(0),\ldots,u(k-1)}$(以後$U_0^{k-1}$と表示)と時刻$k$までの出力${y(1),\ldots,y(k)}$(以後$Y_1^{k}$と表示)がわかっているとする。
###問題
$\hat x(k) = \text{argmin} E[(x(k)-\hat x(k))^T(x(k)-\hat x(k))]$
を満たす$\hat x(k)$を求めよ。
##アイデア
上記の問題は、右辺を計算することで
$\hat x(k) = \text{argmin} E[(x(k)-\hat x(k))^T(x(k)-\hat x(k)) | Y_1^k, U_0^{k-1}]$
と書き換えられる。 これを更に変形していくと、
$\hat x(k) = E[x(k)|Y_1^k, U_0^{k-1}]$
が得られる(あとで式を追加するかも)。
ここで、$x(k)$の条件付き期待値を計算するために、その条件付き確率分布が必要なわけだが、今与えられているノイズと初期値はすべてガウスであり、ダイナミクスも線形であるため、今後時間発展の中で登場する信号はすべてガウスである。
このため、$x(k)$の条件付き確率分布の平均と共分散だけを更新していけばよいことになる。
すなわち、
$\hat x(k|k) = E[x(k)|Y_1^k, U_0^{k-1}]$
$P(k|k) = E[(x(k)-\hat x(k|k))(x(k)-\hat x(k|k))^T |Y_1^k, U_0^{k-1}] $
と表示することし、これらだけを追いかけていけば、自動的に$x(k)$の条件付き確率分布は
$p(x(k) | Y_1^k, U_0^{k-1}) \sim \mathcal N(\hat x(k|k), P(k|k))$
となり、状態推定ができるということである。
##更新式
準備として、初期値の平均と分散から、
$\hat x(0|-1) = \bar x(0)$
$P(0|-1) = \Sigma_0$
としておく。
$p(x(k) | Y_1^k, U_0^{k-1})$から$p(x(k+1) | Y_1^{k+1}, U_0^{k})$へと更新するために、大きく分けて2つのステップが必要である。
###状態推定
まず、$p(x(k) | Y_1^k, U_0^{k-1})$から$p(x(k+1) | Y_1^k, U_0^{k-1})$に更新する。
ここで
$p(x(k+1) | Y_1^k, U_0^{k-1}) \sim \mathcal N(\hat x(k+1|k), P(k+1|k))$
$\hat x(k+1|k) = E[x(k+1)|Y_1^k, U_0^{k-1}]$
$P(k+1|k) = E[(x(k+1)-\hat x(k+1|k))(x(k+1)-\hat x(k+1|k))^T |Y_1^k, U_0^{k-1}] $
である。 結論から先に言えば、システムのダイナミクスを考えることで
$\hat x(k+1|k) = Ax(k|k) + Bu(k)$
$P(k+1|k) = AP(k|k)A^T + GQG$
とすればよい(あとで式を追加)。
###フィルタリング
つぎに、観測出力$y(k+1)$の情報を用いて、$p(x(k+1) | Y_1^k, U_0^{k-1})$から$p(x(k+1) | Y_1^{k+1}, U_0^k)$へ更新する。
ここでも結論を先に言えば、
$\hat x(k+1|k+1) = \hat x(k+1|k) + K(k+1) (y(k+1) - C\hat x(k+1|k))$
$P(k+1|k+1) = (I - K(k+1)C) P(k+1|k)$
とすればよい。 ここで突如現れた$K$はカルマンゲインと呼ばれており、
$K(k+1) = P(k+1|k) C (CP(k+1|k)C^T + R)^{-1}$
と定める(あとで式を追加)。
##まとめ
説明を省いたところがもっとも大事なので、あとで追記したいと思います。
それが終わったらEKFやUKFのまとめもしてみたい。