カルマゲイン

最後の式はカルマゲインについての式です。

行列表記のカルマゲインは次のように表されます。

\[ \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}} \) で微分し、それをゼロとすることで得られます。

備考
\( tr\left( \boldsymbol{P_{n,n}} \right) = tr\left( \boldsymbol{P_{n,n-1}}\right) - \\ - \color{#FF8C00}{tr\left( \boldsymbol{ P_{n,n-1}H^{T}K_{n}^{T} }\right) - tr\left( \boldsymbol{ K_{n}H P_{n,n-1} }\right)} + \\ + tr\left(\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } \right) \) 共分散更新式のトレース
\( tr\left( \boldsymbol{P_{n,n}} \right) = tr\left( \boldsymbol{P_{n,n-1}}\right) - \color{#FF8C00}{ 2tr\left( \boldsymbol{ K_{n}H P_{n,n-1} }\right)} + \\ + tr\left(\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } \right) \) 行列のトレースは、その行列の転置行列のトレースと等しい
\( \frac{d\left( tr\left( \boldsymbol{P_{n,n}} \right) \right)}{d\boldsymbol{K_{n}}} = \color{blue}{\frac{d\left( tr\left( \boldsymbol{P_{n,n-1}}\right) \right)}{d\boldsymbol{K_{n}}}} - \color{green}{ \frac{d\left( 2tr\left( \boldsymbol{ K_{n}H P_{n,n-1} }\right) \right) }{d\boldsymbol{K_{n}}} } + \\ + \color{#7030A0}{\frac{ d\left( tr\left(\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) K_{n}^{T} } \right) \right) }{d\boldsymbol{K_{n}}}} = 0 \) \( \boldsymbol{P_{n,n}} \) のトレースを \( \boldsymbol{K_{n}} \) について微分
\( \frac{d\left( tr\left( \boldsymbol{P_{n,n}} \right) \right)}{d\boldsymbol{K_{n}}} = \color{blue}{0} - \color{green}{ 2 \left( \boldsymbol{ H P_{n,n-1} }\right)^{T} } + \\ + \color{#7030A0}{\boldsymbol{2K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) } } = 0 \) \( \color{green}{\frac{d}{d\boldsymbol{A}} \left( tr\left( \boldsymbol{AB} \right) \right) = \boldsymbol{B}^{T} } \\ \color{#7030A0}{\frac{d}{d\boldsymbol{A}} \left( tr\left( \boldsymbol{ABA^{T}} \right) \right) = 2\boldsymbol{AB} } \)
証明はここを参照
\( \color{green}{ \left( \boldsymbol{ H P_{n,n-1} }\right)^{T} } = \color{#7030A0}{\boldsymbol{K_{n} \left( H P_{n,n-1}H^{T} + R_{n} \right) } } \)
\( \boldsymbol{K_{n}} = \left( \boldsymbol{ H P_{n,n-1} }\right)^{T} \boldsymbol{\left( H P_{n,n-1}H^{T} + R_{n} \right)^{-1} } \)
\( \boldsymbol{K_{n}} = \boldsymbol{ P_{n,n-1}^{T} H^{T} } \boldsymbol{\left( H P_{n,n-1}H^{T} + R_{n} \right)^{-1} } \) 行列の転置に関する性質: \( \boldsymbol{(AB)^{T} = B^{T}A^{T}} \) を使用
\( \boldsymbol{K_{n}} = \boldsymbol{ P_{n,n-1} H^{T} } \boldsymbol{\left( H P_{n,n-1}H^{T} + R_{n} \right)^{-1} } \) 共分散行列は対称行列: \( \boldsymbol{ P_{n,n-1}^{T}} = \boldsymbol{ P_{n,n-1}} \)