共分散更新式
共分散更新式は次のように表されます。
\[ \boldsymbol{ P_{n,n} = \left( I - K_{n}H \right) P_{n,n-1} \left( I - K_{n}H \right)^{T} + K_{n}R_{n}K_{n}^{T} } \]
ここで、
| \( \boldsymbol{P_{n,n} } \) | ：現時点における共分散行列 |
| \( \boldsymbol{P_{n,n-1}} \) | ：前回の状態から予測された、現時点における事前共分散行列 |
| \( \boldsymbol{K_{n}} \) | ：カルマンゲイン |
| \( \boldsymbol{H} \) | ：観測行列 |
| \( \boldsymbol{R_{n}} \) | ：観測の不確かさ（観測雑音の共分散行列） |
共分散更新式の導出
共分散更新式の導出を行います。近道をせず、できるだけ詳細に行うので、多少長い説明になります。細かすぎると感じる方もいるかもしれませんが、一方で、初学者の方にとっては理解を深めることができるはずです。
もし、導出についてこだわりがなければ、次の章に進んでもかまいません、
導出を行うために、次の4つの公式を使用します。
| Eq. Num. | 公式 | 備考 |
|---|---|---|
| 1 | \( \boldsymbol{\hat{x}_{n,n} = \hat{x}_{n,n-1} + K_{n} ( z_{n} - H \hat{x}_{n,n-1} )} \) | 状態更新式 |
| 2 | \( \boldsymbol{z_{n} = Hx_{n} + v_{n}} \) | 観測方程式 |
| 3 | \( \boldsymbol{P_{n,n}} = E\left( \boldsymbol{e_{n}e_{n}^{T}} \right) = E\left( \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right) \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right)^{T} \right) \) | 推定値の共分散行列 |
| 4 | \( \boldsymbol{R_{n}} = E\left( \boldsymbol{v_{n}v_{n}^{T}} \right) \) | 観測値の共分散行列 |
現時点の共分散行列 ( \( \boldsymbol{P_{n,n}} \) ) を、カルマンゲイン \( \boldsymbol{K_{n}} \) の関数として導出します。
| 備考 | |
|---|---|
| \( \boldsymbol{\hat{x}_{n,n} = \hat{x}_{n,n-1} + K_{n} ( z_{n} - H \hat{x}_{n,n-1} )} \) | 状態更新式 |
| \( \boldsymbol{\hat{x}_{n,n} = \hat{x}_{n,n-1} + K_{n} ( Hx_{n} + v_{n} - H \hat{x}_{n,n-1} )} \) | 観測方程式を状態更新式に代入 |
| \( \boldsymbol{e_{n}} = \boldsymbol{x_{n} - \hat{x}_{n,n}} \) | 推定誤差 |
| \( \boldsymbol{e_{n}} = \boldsymbol{x_{n} - \hat{x}_{n,n-1} - K_{n} \left( Hx_{n} + v_{n} - H \hat{x}_{n,n-1} \right)} \) | \( \boldsymbol{\hat{x}_{n,n}} \) に代入 |
| \( \boldsymbol{e_{n}} = \boldsymbol{x_{n} - \hat{x}_{n,n-1} - K_{n}Hx_{n} - K_{n}v_{n} + K_{n}H \hat{x}_{n,n-1}} \) | 展開 |
| \( \boldsymbol{e_{n}} = \boldsymbol{x_{n} - \hat{x}_{n,n-1} - K_{n}H\left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n}} \) | \( (\boldsymbol{ x_{n} - \hat{x}_{n,n-1}}) \) の因数を作成 |
| \( \boldsymbol{e_{n}} = \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n}} \) | |
| \( \boldsymbol{P_{n,n}} = E\left( \boldsymbol{e_{n}e_{n}^{T}} \right) = E\left( \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right) \left( \boldsymbol{x_{n} - \hat{x}_{n,n}} \right)^{T} \right) \) | 推定値の共分散行列 |
| \( \boldsymbol{P_{n,n}} = E\left( \boldsymbol{\left( \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n} \right) \times \\ \times \left( \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n} \right)^{T}} \right) \) | \( \boldsymbol{e_{n}} \) を代入 |
| \( \boldsymbol{P_{n,n}} = E\left( \boldsymbol{\left( \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n} \right) \times \\ \times \left( \left( \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \right)^{T} - \left( K_{n}v_{n}\right) ^{T} \right)} \right) \) | 展開 |
| \( \boldsymbol{P_{n,n}} = E\left( \boldsymbol{\left( \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) - K_{n}v_{n} \right) \times \\ \times \left( \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} - \left( K_{n}v_{n}\right) ^{T} \right)} \right) \) | 行列の転置に関する性質：\( \boldsymbol{(AB)^{T} = B^{T}A^{T}} \) を使用 |
| \( \boldsymbol{P_{n,n}} = E \left( \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} - \\ - \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \left( K_{n}v_{n} \right)^{T} - \\ - K_{n}v_{n} \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} + \\ + K_{n}v_{n} \left( K_{n}v_{n} \right)^{T} } \right) \) | 展開 |
| \( \boldsymbol{P_{n,n}} = E \left( \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} }\right) - \\ - \color{red}{E \left( \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \left( K_{n}v_{n} \right)^{T} }\right)} - \\ - \color{red}{E \left( \boldsymbol{ K_{n}v_{n} \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} }\right)} + \\ + E \left( \color{blue}{\boldsymbol{ K_{n}v_{n} \left( K_{n}v_{n} \right)^{T} }}\right) \) | \( E(X \pm Y) = E(X) \pm E(Y) \) を適用 |
|
\( \color{red}{E \left( \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \left( K_{n}v_{n} \right)^{T} }\right) = 0} \) \( \color{red}{E \left( \boldsymbol{ K_{n}v_{n} \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} }\right) = 0} \) |
\( (\boldsymbol{ x_{n} - \hat{x}_{n,n-1}}) \) は真値に関する事前推定値との誤差。これは観測雑音 \( \boldsymbol{ v_{n} } \) との相関は無く、2つの独立変数の積の期待値はゼロである。 |
| \( \boldsymbol{P_{n,n}} = E \left( \boldsymbol{ \left( I - K_{n}H \right) \left( x_{n} - \hat{x}_{n,n-1} \right) \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} \left( I - K_{n}H \right)^{T} }\right) + \\ + E \left( \color{blue}{\boldsymbol{ K_{n}v_{n} v_{n}^{T} K_{n}^{T} }}\right) \) | 行列の転置に関する性質：\( \boldsymbol{(AB)^{T} = B^{T}A^{T}} \) を使用 |
| \( \boldsymbol{P_{n,n}} = \boldsymbol{ \left( I - K_{n}H \right)} \color{green}{E \left( \boldsymbol{ \left( x_{n} - \hat{x}_{n,n-1} \right) \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} }\right)} \boldsymbol{ \left( I - K_{n}H \right)^{T}} + \\ + \boldsymbol{K_{n}} \color{blue}{ E \left( \boldsymbol{ v_{n} v_{n}^{T} }\right) } \boldsymbol{ K_{n}^{T} } \) | \( E(aX) = aE(X) \) を適用 |
|
\( \color{green}{E \left( \boldsymbol{ \left( x_{n} - \hat{x}_{n,n-1} \right) \left( x_{n} - \hat{x}_{n,n-1} \right)^{T} }\right) = \boldsymbol{P_{n,n-1}}} \) \( \color{blue}{ E \left( \boldsymbol{ v_{n} v_{n}^{T} }\right) = \boldsymbol{R_{n}}} \) |
\( \color{green}{\boldsymbol{P_{n,n-1}}} \) は事前共分散行列 \( \color{blue}{\boldsymbol{R_{n}}} \) は観測の不確かさ |
| \( \boldsymbol{P_{n,n}} = \boldsymbol{ \left( I - K_{n}H \right)} \color{green}{\boldsymbol{ P_{n,n-1}} } \boldsymbol{ \left( I - K_{n}H \right)^{T}} + \boldsymbol{K_{n}} \color{blue}{ \boldsymbol{ R_{n} } } \boldsymbol{ K_{n}^{T} } \) | 共分散更新式！ |