16. 離散時間システムにおける状態推定(1)

カルマンフィルタ（Kalman Filter）とは、雑音を含む観測データから、システムの状態を推定するためのアルゴリズムである。直観的に説明すると、カルマンフィルタは以下の2つを繰り返す。
・予測：前回の状態とモデルに基づいて、次の状態を予測する。
・更新：　実際の観測値と予測を比較して、推定結果を修正する。
これを繰り返すことで、雑音に強く、より正確な「真の状態」に近づいていくことになる。

16-1. カルマンフィルタが扱うシステム

離散時間システムを式(1)で表す。$$x_{k+1} = A x_k + v_k \\ y_k = C x_k + e_k \;\;\; \cdots (1)$$ここで、\(x_k\)は\(n\)次元状態ベクトル、\(y_k\)は\(p\)次元観測ベクトルである。\(v_k\)はシステムの動作の不確実性さを表す\(n\)次元の正規白色雑音であり、その平均値と分散を\(E[v_k]=0,\quad E[v_k v_k^T] = R_v\)とする。また、\(e_k\)は伝送雑音、観測誤差などを表す\(p\)次元の正規白色雑音で、\(E[e_k]=0,\quad E[e_k e_k^T]=R_e\)の性質を有し、\(v_k\)と\(e_k\)は互いに独立とする。
さらに、式(1)の初期状態\(x_0\)の統計的性質も既知であるとし、その平均値と分散を、\(E[x_0]=m,\quad E[(x_0 - m)(x_0 - m)^T]=R_0\)で与える。\(R_v,\;R_e,\; R_0\)は正定、\(x_0\)は\(v_k,\;e_k\)と独立であると仮定する。

16-2. 推定誤差の平均値

状態推定器を式(2)とするとき、推定誤差の平均値を求めよ。$$ \hat{x}_{k+1} = A \hat{x}_k + K(y_k - C \hat{x}_k) \;\;\; \cdots (2)$$

解答例：式(1)から式(2)を引く。$$x_{k+1} - \hat{x}_{k+1} = A x_k - A \hat{x}_k + v_k -K(y_k - C \hat{x}_k)$$推定誤差を\(\epsilon_k = x_k - \hat{x}_k\)とすると、$$\epsilon_{k+1} = A \epsilon_k + v_k - K(y_k - C \hat{x}_k)$$となり、この式に式(1)の\(y_k\)を代入すると、$$ \epsilon_{k+1} = (A - KC)\epsilon_k + v_k - K e_k \;\;\; \cdots (3)$$となる。従って、推定誤差\(\epsilon_k\)は式(3)の線形確率差分方程式で与えられる。式(3)の期待値をとると、\(e_k,\;v_k\)は正規白色雑音で平均値０なので、$$E[\epsilon_{k+1}] =(A - KC)E[\epsilon_k] \;\;\; \cdots (4)$$となる。推定誤差\(\epsilon_k\)の平均値\(E[\epsilon_k]\)は、差分方程式 式(4)を満たし、その解は、$$E[\epsilon_k] = (A - KC)^k E[\epsilon_0]$$となる。従って、\(A - KC\)が安定な行列であれば、$$\lim_{k \to \infty} E[\epsilon_k] = 0 , \quad \forall \epsilon_0$$となる。これは、初期値における誤差\(\epsilon_0 = x_0 - \hat{x}_0 \neq 0\)の影響は時間経過とともに減少し、推定誤差の平均値\(E[\epsilon_k]\)は０に収束することを表している。

16-3. 推定誤差の分散

状態推定器を式(2)とするとき、推定誤差の分散を求めよ。

解答例：推定誤差の分散を式(5)で定義する。$$P_k = E\left[ (\epsilon_k - E[\epsilon_k])(\epsilon_k - E[\epsilon_k])^T \right] \;\;\; \cdots (5)$$ さらに、$$P_{k+1} = E\left[(\epsilon_{k+1} - E[\epsilon_{k+1}])(\epsilon_{k+1} - E[\epsilon_{k+1}])^T \right] $$ 式(3),式(4)を代入すると、$$P_{k+1} = E\left\{\left[ (A - KC)\epsilon_k + v_k - K e_k - (A - KC)E[\epsilon_k]\right] \\ \times \left[(A - KC)\epsilon_k + v_k - K e_k - (A - KC)E[\epsilon_k]\right]^T\right\}$$ ここで、\(v_k\)と\(e_k\)は正規白色雑音で互いに独立なので、つぎのように変形できる。$$P_{k+1} = (A - KC) E\left[(\epsilon_k - E[\epsilon_k])(\epsilon_k - E[\epsilon_k])^T\right](A - KC)^T \\ + E[v_k v_k^T] + K E[e_k e_k^T]K^T$$ この式に式(5)と\(v_k\)、\(e_k\)の分散を代入すると、$$P_{k+1} = (A - KC)P_k(A - KC)^T + R_v + K R_e K^T \;\;\; \cdots (6)$$と表せる。
式(6)の初期値\(P_0\)は、式(5)の定義式より、$$P_0 = E\left[ (\epsilon_0 - E[\epsilon_0])(\epsilon_0 - E[\epsilon_0])^T \right] \\ =E\left[(x_0 - \hat{x}_0 - E[x_0] + E[\hat{x}_0])(x_0 - \hat{x}_0 - E[x_0] + E[\hat{x}_0])^T \right]$$となる。\(E[x_0] = m\)であり、また、\(E[\hat{x}_0]\)の\(\hat{x}_0\)は状態推定器の初期値であるから確定値\(\hat{x}_0\)である。よって、$$P_0 = E\left[(x_0 - \hat{x}_0 - m + \hat{x}_0)(x_0 - \hat{x}_0 - m + \hat{x}_0)^T \right] \\ = E[(x_0 - m)(x_0 - m)^T]$$となる。つまり、\(P_0 = R_0\)である。

16-4. 評価関数を最小にする\(K\)

評価関数を式(7)で定義し、この評価関数を最小化する行列\(K\)を求めよ。$$J_k = E\left[(\xi^T \epsilon_k - E[\xi^T \epsilon_k])(\xi^T \epsilon_k - E[\xi^T \epsilon_k])^T\right] \;\;\; \cdots (7)$$

解答例：評価関数 式(7)はスカラで、以下のように変形できる。$$J_k = E\left[(\xi^T \epsilon_k - E[\xi^T \epsilon_k])(\xi^T \epsilon_k - E[\xi^T \epsilon_k])^T\right] \\ = \xi^T E \left[(\epsilon_k - E[\epsilon_k])(\epsilon_k - E[\epsilon_k])^T\right] \xi = \xi^T P_k \xi$$ \(J_k\)は推定誤差分散\(P_k\)を使った２次形式である。ここで、\(J_{k+1}\)は式(6)を使って、$$J_{k+1} = \xi^T P_{k+1} \xi = \xi^T\left\{(A - KC)P_k(A - KC)^T + R_v + K R_e K^T\right\}\xi \\ = \xi^T \left\{AP_k A^T - KCP_k A^T - AP_k C^T K^T + KCP_k C^T K^T + R_v + K R_e K^T \right \}\xi \\ = \xi^T\left\{A P_k A^T + R_v - KCP_k A^T - A P_k C^T K^T + K(R_e + CP_kC^T)K^T\right\} \xi \;\;\; \cdots (8)$$となる。ここで、初期時刻\(k=0\)では、\(P_0 = R_0\)なので、評価関数は、$$J_1 = \xi^T P_1 \xi = \xi^T\left\{A R_0 A^T + R_v - KCR_0 A^T - A R_0 C^T K^T + K(R_e + C R_0 C^T)K^T\right\} \xi$$となる。この式において、調整可能なパラメータは\(K\)だけであり、\(R_e,\; R_0\)ともに正定なので、\(K\)に関しての２次関数となっている。従って、評価関数\(J_1\)を最小化でき、最適な\(K_0\)が決定され\(P_1\)を得る。これより続けて、\(k = 1,2,3,\cdots \)として最適な\(K_1,K_2,K_3, \ldots\)を決定できる。このように\(K\)は時変ゲイン\(K_k\)となることが分かる。
式(8)を平方完成すると、$$ \xi^T P_{k+1} \xi = \xi^T\left\{A P_k A^T + R_v - A P_kC^T(R_e + CP_kC^T)^{-1} C P_k A^T\right\} \xi \\ + \xi^T \left\{K - AP_k C^T(R_e + CP_kC^T)^{-1}\right\} (R_e + CP_k C^T) \\ \times \left\{K - AP_kC^T(R_e + CP_k C^T)^{-1}\right\}^T \xi$$となる。この式の右辺第１項は、\(K\)に依存しない。第２項は、行列\(R_e + CP_k C^T\)が半正定となる。従って、第２項を０とすることで所望の\(K\)を得る。$$K_k = AP_kC^T(R_e + CP_k C^T)^{-1} \;\;\; \cdots (9)$$このように行列\(K\)は時変ゲインとなる。また、評価関数を最小にしたときの推定誤差の分散は、$$P_{k+1} = A P_k A^T + R_v - A P_kC^T(R_e + CP_kC^T)^{-1} C P_k A^T \;\;\; \cdots (10)$$となる。以上より、式(9)と式(10)によって、\(K_k\)を逐次的に決定することができる。