カルマゲイン
最後の式はカルマゲインについての式です。
行列表記のカルマゲインは次のように表されます。
| \( \boldsymbol{K_{n} } \) | ：カルマゲイン |
| \( \boldsymbol{P_{n,n-1}} \) | ：前回の状態から予測された、現時点における事前共分散行列 |
| \( \boldsymbol{H} \) | ：観測行列 |
| \( \boldsymbol{R_{n}} \) | ：観測の不確かさ（観測雑音の共分散行列） |
カルマンゲインの直感的な説明
まずは1次元の場合の状態更新式を見てみましょう。
\[ \hat{x}_{n,n} = \hat{x}_{n,n-1} + {k}_{n}({z}_{n} - \hat{x}_{n,n-1}) \]
この式は、更新された状態推定値 \( \hat{x}_{n,n} \) が、事前推定値 \( \hat{x}_{n,n-1} \) をイノベーション（観測値 \( {z}_{n} \) と事前推定値の差）を使って調整することで得られることを示しています。カルマンゲイン \( {k}_{n} \) は、観測値と事前推定値のどちらをどれだけ信頼するかを決定します。
多変数ケースへの拡張
1次元の場合、システムの状態 \( x_{n} \) と観測値 \( z_{n} \) は同じドメインにあります。例えば、\( x_{n} \) がビルの実際の高さを表しているなら、\( z_{n} \) もまたその高さの直接的な測定値です。
多変数の場合、事情は少し異なります。観測ベクトル \( \boldsymbol{{z}}_{n} \) とシステムの状態ベクトル \( \boldsymbol{x}_{n} \) は、しばしば異なるドメインに存在します。そのため、イノベーションを計算するためにこれらを直接引き算することはできません。代わりに、観測行列 \( \boldsymbol{H} \) を使用して、システムの状態を観測ドメインに射影する必要があります。
\[ \text{innovation} = (\boldsymbol{{z}}_{n} - \boldsymbol{H}\boldsymbol{\hat{x}}_{n,n-1}) \]
観測方程式の章で説明したように、観測行列 \( \boldsymbol{H} \) はシステムの状態ベクトル \( \boldsymbol{{x}}_{n} \) を状態ドメインから観測ドメインへとマッピングします。
例えば、垂直に発射されたロケットの状態が、高度 \( h_{n} \) と垂直速度 \( v_{h_{n}} \) で定義されるとします。システムの状態ベクトルは以下のようになります。
\[ x_n = \begin{bmatrix} h_n \\ v_{h_n} \end{bmatrix} \]
ここで、高度のみが測定されると仮定します。この場合、観測値 \( \boldsymbol{z}_{n} \) は次のようにシステムの状態と関連付けられます。
\[ \boldsymbol{z}_{n} = \boldsymbol{H} \boldsymbol{x}_{n} + v_n = \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} h_n \\ v_{h_n} \end{bmatrix} + v_n \]
ここで：
- \( \boldsymbol{H} = \begin{bmatrix} 1 & 0 \end{bmatrix} \)
- \( v_{n} \) は観測ノイズです。
ここで、状態更新式の章で議論したような、多次元の場合の状態更新式を詳しく見てみましょう。
\[ \boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n}(\boldsymbol{{z}}_{n} - \boldsymbol{H}\boldsymbol{\hat{x}}_{n,n-1}) \]
システムの状態 \( \boldsymbol{\hat{x}}_{n,n-1} \) と観測値 \( \boldsymbol{z}_{n} \) は異なるドメインにある可能性があるため、観測行列 \( \boldsymbol{H} \) を使用して状態推定値を観測ドメインに射影します。
1次元のカルマンフィルタの章で導出した、1次元の場合のカルマンゲインを確認してみましょう。
\[ k_{n} = \frac{p_{n,n-1}}{p_{n,n-1} + r_{n}} \]
ここで：
- \( p_{n,n-1} \) は事前推定値 \( \hat{x}_{n,n-1} \) の分散
- \( r_{n} \) は観測ノイズの分散
この式は、カルマンゲイン \( k_{n} \) が事前推定値の不確かさと観測値の不確かさのバランスを取ることを示しています。事前推定値がより不確かな場合（つまり、\( p_{n,n-1} \) が大きい場合）、ゲインは観測値をより重視するように働きます。逆に、観測値がノイズを多く含む場合（つまり、\( r_{n} \) が大きい場合）、ゲインは事前推定値を支持します。
フィルタを最適にするために、予測された（事前）推定値 \( \hat{x}_{n,n-1} \) と観測値 \( \hat{z}_{n} \) に基づいて計算される、現在の（事後）推定値 \( \hat{x}_{n,n} \) の分散 \( p_{n,n} \) を最小化することを目指します。カルマンゲイン \( k_{n} \) はこの事後分散を最小化します。詳細な導出は1次元のカルマンフィルタの章で提供されています。
1次元のカルマンゲインを次のように書き直してみましょう。
\[ k_{n} = p_{n,n-1}(p_{n,n-1} + r_{n})^{-1} \]
多変数の場合、事前推定値の不確かさ（共分散）行列 \( \boldsymbol{P}_{n,n-1} \) と観測ノイズの不確かさ（共分散）行列 \( \boldsymbol{R}_{n} \) を扱います。\( \boldsymbol{P}_{n,n-1} \) を観測ノイズ共分散 \( \boldsymbol{R}_{n} \) と組み合わせる前に、観測ドメインに射影する必要があります。この射影は観測行列 \( \boldsymbol{H} \) を使用して行われます。
\( \boldsymbol{P}_{n,n-1} \) は共分散行列であり、分散は二乗の項であるため、次の変換を使用して射影する必要があります。
\[ \boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T} \]
(本の第7.5章の証明を参照してください。)
この項は、1次元の場合の \( p_{n,n-1} \) に類似した、予測された観測の不確かさを表します。
事前推定値の不確かさを観測ドメインに射影した後、多変数の場合のカルマンゲイン*は次の形をとります。
\[ \color{green}{\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}}(\color{green}{\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}} + \color{blue}{\boldsymbol{R}_{n}})^{-1} \]
(* これはカルマンゲインの最終形ではありません)
カルマンゲイン*にイノベーションを掛けると次のようになります。
\[ \color{green}{\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}}(\color{green}{\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}} + \color{blue}{\boldsymbol{R}_{n}})^{-1}(\boldsymbol{{z}}_{n} - \boldsymbol{H}\boldsymbol{\hat{x}}_{n,n-1}) \]
上の式は観測ドメインにありますが、状態更新式では、状態ドメインにあるシステムの状態推定値 \( \boldsymbol{\hat{x}}_{n,n-1} \) を更新する必要があります。
\[ \boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \boldsymbol{K}_{n}(\boldsymbol{{z}}_{n} - \boldsymbol{H}\boldsymbol{\hat{x}}_{n,n-1}) \]
カルマンゲイン*の式から先頭の \( \boldsymbol{H} \) を取り除くことで、結果を状態ドメインに戻します。カルマンゲインにイノベーションを掛けた更新された式は次のようになります。
\[ \color{green}{\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}}(\color{green}{\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}} + \color{blue}{\boldsymbol{R}_{n}})^{-1}(\boldsymbol{{z}}_{n} - \boldsymbol{H}\boldsymbol{\hat{x}}_{n,n-1}) \]
これで、状態更新式を完全な多変数の形で表現できます。
\[ \boldsymbol{\hat{x}}_{n,n} = \boldsymbol{\hat{x}}_{n,n-1} + \color{red}{\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T} + \boldsymbol{R}_{n})^{-1}}(\boldsymbol{{z}}_{n} - \boldsymbol{H}\boldsymbol{\hat{x}}_{n,n-1}) \]
対応するカルマンゲインの最終形は次の通りです。
\[ \color{red}{\boldsymbol{K}_{n} = \boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T}(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^{T} + \boldsymbol{R}_{n})^{-1}} \]
この形は、イノベーション項が観測ドメインから状態ドメインへと適切にマッピングされることを保証し、システムの状態推定値の一貫性のある最適な更新を可能にします。
このセクションでは、多変数カルマンゲインの直感的な理解を提供しました。次のセクションでは、その形式的な導出を示します。
カルマゲインの式の導出
ここでは、カルマゲインの式の導出を行います。もし、導出についてこだわりがなければ、次の章に進んでもかまいません、
| 備考 | |
|---|---|
| \( \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}} \) で微分し、それをゼロとすることで得られます。