カルマンフィルターの方程式群の導出をおさらいします。
日本語の教科書で入手しやすいのは、足立先生、片山先生によるものですね。これらはベイズ推定の枠組みでカルマンフィルターを導出しています。ここでは、別の導出をおさらいします。元となっているのはKalman Filtering: Theory and Practice with MATLAB です。
カルマンゲインの導出
$\hat{x}$は$x$の推定値、添字$k(+)$, $k(-)$はそれぞれ$k$番目ステップの事後、事前推定を表す。 \[ \hat{x}_{k(+)} = K^1_{k} \hat{x}_{k(-)} +\bar{K}_k z_k \] 直交原理から \[ E\left\langle \left[ x_k – \hat{x}_{k(+)} \right] z_{i}^\mathrm{T}\right\rangle = 0, \,\,\,\, i = 1, 2, \ldots, k-1 \] \[ E\left\langle \left[ x_k – \hat{x}_{k(+)} \right] z_{k}^\mathrm{T}\right\rangle = 0 \] \[ x_k = \Phi_{k-1} x_{k-1} – w_{k-1} \] \[ E\left\langle \left[ \Phi_{k-1} x_{k-1} -w_{k-1} – K^1_{k} \hat{x}_{k(-)} – \bar{K}_k z_k \right] z_{i}^\mathrm{T}\right\rangle = 0, \,\,\,\, i = 1, 2, \ldots, k-1 \] と書き直していますが、以下でも良いように思えます: \[ E\left\langle \left[ x_{k} – K^1_{k} \hat{x}_{k(-)} – \bar{K}_k z_k \right] z_{i}^\mathrm{T}\right\rangle = 0, \,\,\,\, i = 1, 2, \ldots, k-1 \] \[ z_k = H_k x_k +v_k \] \[ E\left\langle \left[ \Phi_{k-1} x_{k-1} -w_{k-1} – K^1_{k} \hat{x}_{k(-)} – \bar{K}_k H_k x_k – \bar{K}_k v_k \right] z_{i}^\mathrm{T}\right\rangle = 0, \,\,\,\, i = 1, 2, \ldots, k-1 \] これも \[ E\left\langle \left[ x_{k} – K^1_{k} \hat{x}_{k(-)} – \bar{K}_k H_k x_k – \bar{K}_k v_k \right] z_{i}^\mathrm{T}\right\rangle = 0, \,\,\,\, i = 1, 2, \ldots, k-1 \] と、そのままで良いと思います。 \[ E\left\langle w_k z_{i}^\mathrm{T}\right\rangle = 0, \,\,\,\, i = 1, 2, \ldots, k \] \[ E\left\langle v_k z_{i}^\mathrm{T}\right\rangle = 0, \,\,\,\, i = 1, 2, \ldots, k-1 \] \[ \Phi_{k-1} E \left\langle x_{k-1} z_{i}^\mathrm{T} \right\rangle – K^1_{k} E \left\langle \hat{x}_{k(-)} z_{i}^\mathrm{T} \right\rangle – \bar{K}_k H_k E \left\langle x_k z_{i}^\mathrm{T} \right\rangle – \bar{K}_k E \left\langle v_k z_{i}^\mathrm{T} \right\rangle = 0, \,\,\,\, i = 1, 2, \ldots, k-1 \] \[ E \left\langle x_{k} z_{i}^\mathrm{T} \right\rangle – K^1_{k} E \left\langle \left( \hat{x}_{k(-)} – x_k \right) z_{i}^\mathrm{T} \right\rangle – K^1_{k} E \left\langle x_k z_{i}^\mathrm{T} \right\rangle – \bar{K}_k H_k E \left\langle x_k z_{i}^\mathrm{T} \right\rangle = 0, \,\,\,\, i = 1, 2, \ldots, k-1 \] \[ E \left\langle x_{k} z_{i}^\mathrm{T} \right\rangle – K^1_{k} E \left\langle x_k z_{i}^\mathrm{T} \right\rangle – \bar{K}_k H_k E \left\langle x_k z_{i}^\mathrm{T} \right\rangle = 0, \,\,\,\, i = 1, 2, \ldots, k-1 \] \[ \left( I – K^1_{k} – \bar{K}_k H_k \right) E\left\langle x_k z_{i}^\mathrm{T} \right\rangle = 0, \,\,\,\, i = 1, 2, \ldots, k-1 \]これから
\[ K^1_{k} = I – \bar{K}_k H_k \]が得られます。
推定誤差の表記を以下のように定義します:
\[ \tilde{x}_{k(+)} \triangleq \hat{x}_{k(+)} – x_k \] \[ \tilde{x}_{k(-)} \triangleq \hat{x}_{k(-)} – x_k \] \[ \tilde{z}_{k(-)} \triangleq \hat{z}_{k(-)} – x_k = H_k \hat{x}_{k(-)} – z_k \]予測値の直交原理から
\[ E\left\langle \left[ x_k – \hat{x}_{k(+)} \right] \hat{z}_{k(-)}^\mathrm{T}\right\rangle = 0 \ldots, k-1 \]観測値の直交原理を引き算すると
\[ E\left\langle \left[ x_k – \hat{x}_{k(+)} \right] \tilde{z}_{k(-)}^\mathrm{T}\right\rangle = 0 \]となります。これを書き直します。
\[ E\left\langle \left[ x_{k} – K^1_{k} \hat{x}_{k(-)} – \bar{K}_k z_k \right] \left[ H_k \hat{x}_{k(-)} – z_{k} \right]^\mathrm{T} \right\rangle = 0 \] \[ E\left\langle \left[ x_{k} – \left(I – \bar{K}_k H_k \right)_{k} \hat{x}_{k(-)} – \bar{K}_k \left(H_k x_k + v_k \right) \right] \left[ H_k \hat{x}_{k(-)} – H_k x_{k} – v_k \right]^\mathrm{T} \right\rangle = 0 \] \[ E\left\langle \left[ x_{k} – \hat{x}_{k(-)} + \bar{K}_k H_k \hat{x}_{k(-)} – \bar{K}_k H_k x_k – \bar{K}_k v_k \right] \left[ H_k \hat{x}_{k(-)} – H_k x_{k} – v_k \right]^\mathrm{T} \right\rangle = 0 \] \[ E\left\langle \left[ – \tilde{x}_{k(-)} + \bar{K}_k H_k \tilde{x}_{k(-)} – \bar{K}_k v_k \right] \left[ H_k \tilde{x}_{k(-)} – v_{k} \right]^\mathrm{T} \right\rangle = 0 \] \[ E\left\langle \tilde{x}_{k(-)} v_k^\mathrm{T} \right\rangle = 0 \] を用いると \[ E\left\langle \left[ – \tilde{x}_{k(-)} \tilde{x}_{k(-)}^\mathrm{T} H_k^\mathrm{T} + \bar{K}_k H_k \tilde{x}_{k(-)}\tilde{x}_{k(-)}^\mathrm{T} H_k^\mathrm{T} + \bar{K}_k v_k v_k^\mathrm{T} \right] \right\rangle = 0 \] になります。 \[ P_{k(-)} =E\left\langle \tilde{x}_{k(-)} \tilde{x}_{k(-)}^\mathrm{T} \right\rangle \] \[ R_{k} =E\left\langle v_k v_{k}^\mathrm{T} \right\rangle \] を使って \[ – P_{k(-)} H_k^\mathrm{T} + \bar{K}_k H_k P_{k(-)} H_k^\mathrm{T} + \bar{K}_k R_k = 0. \] これから \[ \bar{K}_k \left(H_k P_{k(-)} H_k^\mathrm{T} + R_k \right) = P_{k(-)} H_k^\mathrm{T} \] カルマンゲインは最終的に \[ \bar{K}_k = P_{k(-)} H_k^\mathrm{T} \left(H_k P_{k(-)} H_k^\mathrm{T} + R_k \right)^{-1} \] になります。誤差共分散行列の更新式の導出
誤差共分散行列の定義は以下の通りです:
\[ P_{k(+)} =E\left\langle \tilde{x}_{k(+)} \tilde{x}_{k(+)}^\mathrm{T} \right\rangle. \]推定誤差を以下のように書き直します:
\[ \hat{x}_{k(+)} = \left(I -\bar{K}_k H_k \right)\hat{x}_{k(-)} +\bar{K}_k z_k = \hat{x}_{k(-)} + \bar{K}_k \left(z_k – H_k \hat{x}_{k(-)} \right) \] \[ \hat{x}_{k(+)} -x_k = \hat{x}_{k(-)} -x_k + \bar{K}_k H_k x_k + \bar{K}_k v_k – \bar{K}_k H_k \hat{x}_{k(-)} \] \[ \tilde{x}_{k(+)} = \tilde{x}_{k(-)} – \bar{K}_k H_k \tilde{x}_{k(-)} + \bar{K}_k v_k = \left( I – \bar{K}_k H_k \right) \tilde{x}_{k(-)} + \bar{K}_k v_k \]上記の推定誤差を使って、誤差共分散行列の定義に代入します:
\begin{align*} P_{k(+)} = & E\left\langle \tilde{x}_{k(+)} \tilde{x}_{k(+)}^\mathrm{T} \right\rangle \\ = & E\left\langle \left[ \left( I – \bar{K}_k H_k \right) \tilde{x}_{k(-)} + \bar{K}_k v_k \right] \left[ \left( I – \bar{K}_k H_k \right) \tilde{x}_{k(-)} + \bar{K}_k v_k \right] ^\mathrm{T} \right\rangle \\ = & E\left\langle \left[ \left( I – \bar{K}_k H_k \right) \tilde{x}_{k(-)} + \bar{K}_k v_k \right] \left[ \tilde{x}_{k(-)}^\mathrm{T} \left( I – \bar{K}_k H_k \right)^\mathrm{T} + v_k^\mathrm{T} \bar{K}_k^\mathrm{T} \right] \right\rangle \\ = & E\left\langle \left[ \left( I – \bar{K}_k H_k \right) \tilde{x}_{k(-)} + \bar{K}_k v_k \right] \left[ \tilde{x}_{k(-)}^\mathrm{T} \left( I – \bar{K}_k H_k \right)^\mathrm{T} + v_k^\mathrm{T} \bar{K}_k^\mathrm{T} \right] \right\rangle \\ = & \left( I – \bar{K}_k H_k \right) P_{k(-)} \left( I – \bar{K}_k H_k \right)^\mathrm{T} + \bar{K}_k v_k \bar{K}_k^\mathrm{T} \\ = & P_{k(-)} – P_{k(-)} H_k^\mathrm{T} \bar{K}_k^\mathrm{T} – \bar{K}_k H_k P_{k(-)} + \bar{K}_k H_k P_{k(-)} H_k^\mathrm{T} \bar{K}_k^\mathrm{T} + \bar{K}_k R_k \bar{K}_k^\mathrm{T} \\ = & P_{k(-)} – P_{k(-)} H_k^\mathrm{T} \bar{K}_k^\mathrm{T} – \bar{K}_k H_k P_{k(-)} + \bar{K}_k\left( H_k P_{k(-)} H_k^\mathrm{T} + \bar{K}_k R_k \right) \bar{K}_k^\mathrm{T} \\ = & P_{k(-)} – P_{k(-)} H_k^\mathrm{T} \bar{K}_k^\mathrm{T} – \bar{K}_k H_k P_{k(-)} + P_{k(-)} H_k\mathrm{T} \bar{K}_k^\mathrm{T} \\ = & P_{k(-)} – \bar{K}_k H_k P_{k(-)} \\ = & \left(I – \bar{K}_k H_k \right)P_{k(-)} \\ \end{align*}誤差共分散行列の時間更新
事前推定誤差を以下のように書き直します:
\begin{align*} \tilde{x}_{k(-)} = & \hat{x}_{k(-)} – x_k \\ =& \Phi_{k-1} \hat{x}_{k-1(+)} – x_k \\ =& \Phi_{k-1} \left( \hat{x}_{k-1(+)} – x_{k-1} \right) + \Phi_{k-1} x_{k-1} – x_k \\ =& \Phi_{k-1} \left( \hat{x}_{k-1(+)} – x_{k-1} \right) – w_{k-1} \\ =& \Phi_{k-1} \tilde{x}_{k-1(+)} – w_{k-1} \\ \end{align*}上記の事前推定誤差を使って、誤差共分散行列の定義に代入します:
\begin{align*} P_{k(-) } = & E\left\langle \tilde{x}_{k(-)} \tilde{x}_{k(-)}^\mathrm{T} \right\rangle \\ = & E\left\langle \left[\Phi_{k-1} \tilde{x}_{k-1(+)} -w_{k-1} \right] \left[\Phi_{k-1} \tilde{x}_{k-1(+)} -w_{k-1}\right]^\mathrm{T} \right\rangle \\ =& \Phi_{k-1} P_{k-1(+)} \Phi_{k-1}^\mathrm{T} + E\left\langle w_{k-1} w_{k-1}^\mathrm{T}\right\rangle \\ =& \Phi_{k-1} P_{k-1(+)} \Phi_{k-1}^\mathrm{T} + Q_{k-1} \end{align*}
