カルマゲイン
最後の式はカルマゲインについての式です。
行列表記のカルマゲインは次のように表されます。
\[ \boldsymbol{ K_{n} = P_{n,n-1}H^{T}\left( HP_{n,n-1}H^{T} + R_{n} \right)^{-1} } \]
ここで、
| \( \boldsymbol{K_{n} } \) | ：カルマゲイン |
| \( \boldsymbol{P_{n,n-1}} \) | ：前回の状態から予測された、現時点における事前共分散行列 |
| \( \boldsymbol{H} \) | ：観測行列 |
| \( \boldsymbol{R_{n}} \) | ：観測の不確かさ（観測雑音の共分散行列） |
カルマゲインの式の導出
ここでは、カルマゲインの式の導出を行います。もし、導出についてこだわりがなければ、次の章に進んでもかまいません、
| 備考 | |
|---|---|
| \( \boldsymbol{P_{n,n}} = \boldsymbol{ \left( I - K_{n}H \right)} \boldsymbol{ P_{n,n-1}} \color{blue}{\boldsymbol{ \left( I - K_{n}H \right)^{T}}} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \) | 共分散更新式 |
| \( \boldsymbol{P_{n,n}} = \boldsymbol{ \left( I - K_{n}H \right)} \boldsymbol{ P_{n,n-1}} \color{blue}{\boldsymbol{ \left( I - \left( K_{n}H \right)^{T}\right)}} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \) | \( \boldsymbol{I^{T} = I} \) |
| \( \boldsymbol{P_{n,n}} = \color{green}{\boldsymbol{ \left( I - K_{n}H \right)} \boldsymbol{ P_{n,n-1}}} \color{blue}{\boldsymbol{ \left( I - H^{T}K_{n}^{T}\right)}} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \) | 行列の転置に関する性質： \( \boldsymbol{(AB)^{T} = B^{T}A^{T}} \) を使用 |
| \( \boldsymbol{P_{n,n}} = \color{green}{\boldsymbol{ \left( P_{n,n-1} - K_{n}H P_{n,n-1} \right)}} \boldsymbol{ \left( I - H^{T}K_{n}^{T}\right)} + \boldsymbol{K_{n}} \boldsymbol{ R_{n} } \boldsymbol{ K_{n}^{T} } \) | |
| \( \boldsymbol{P_{n,n}} = \boldsymbol{ P_{n,n-1} - P_{n,n-1}H^{T}K_{n}^{T} - K_{n}H P_{n,n-1}} + \\ + \color{#7030A0}{\boldsymbol{K_{n}H P_{n,n-1}H^{T}K_{n}^{T} + K_{n} R_{n} K_{n}^{T} } } \) | 展開 |
| \( \boldsymbol{P_{n,n}} = \boldsymbol{ P_{n,n-1} - P_{n,n-1}H^{T}K_{n}^{T} - K_{n}H P_{n,n-1}} + \\ + \color{#7030A0}{\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } } \) | 最後の2項をくくる |
カルマンフィルタは、最適フィルタとも呼ばれます。つまり、推定誤差が最小となるようなカルマゲインが得られます。
推定誤差を最小にするために、共分散行列 \( \boldsymbol{P_{n,n}} \) の対角成分を最小にする必要があります。
正方行列の対角成分の和は、行列のトレースと呼ばれます。したがって、\( tr(\boldsymbol{P_{n,n}}) \)を最小にします。これを最小にするカルマゲインは、\( \boldsymbol{P_{n,n}} \) を \( \boldsymbol{K_{n}} \) で微分し、それをゼロとすることで得られます。