状態更新式

このページは、このチュートリアルの中で最も短いページです。状態更新式については、"\( \alpha -\beta -\gamma \) フィルタ" と "1次元カルマンフィルタ"で大まかな説明を行いました。

状態更新式の行列表現は次のように表されます。

\[ \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}} \)	：時刻 \( n \) における状態ベクトル（推定値）
\( \boldsymbol{\hat{x}_{n,n-1}} \)	：時刻 \( n - 1 \) における状態ベクトル（予測値）
\( \boldsymbol{K_{n}} \)	：カルマンゲイン
\( \boldsymbol{z_{n}} \)	：観測値
\( \boldsymbol{H} \)	：観測行列

Get the book

カルマンゲイン以外の状態更新式の構成要素については、行列表記で理解していると思います。カルマンゲインについては後ほど説明します。

ここで、次元に注目する必要があります。例えば、状態ベクトルが5次元であり、その中で3つ（第 1・3・5 要素）を観測できるとします。

\[ \boldsymbol{x(n)} = \left[ \begin{matrix} x_{1}\\ x_{2}\\ x_{3}\\ x_{4}\\ x_{5}\\ \end{matrix} \right] \boldsymbol{z(n)} = \left[ \begin{matrix} z_{1}\\ z_{3}\\ z_{5}\\ \end{matrix} \right] \]

観測行列は \( 3 \times 5 \) 行列で次のように表されます。

\[ \boldsymbol{H} = \left[ \begin{matrix} 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \]

イノベーション \( \left( \boldsymbol{ z_{n} - H \hat{x}_{n,n-1}} \right) \) は次のようび表されます。

\[ \left( \boldsymbol{ z_{n} - H \hat{x}_{n,n-1}} \right) = \left[ \begin{matrix} z_{1}\\ z_{3}\\ z_{5}\\ \end{matrix} \right] - \left[ \begin{matrix} 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{1}\\ \hat{x}_{2}\\ \hat{x}_{3}\\ \hat{x}_{4}\\ \hat{x}_{5}\\ \end{matrix} \right] = \left[ \begin{matrix} (z_{1} - \hat{x}_{1})\\ (z_{3} - \hat{x}_{3})\\ (z_{5} - \hat{x}_{5})\\ \end{matrix} \right] \]

カルマンゲインは \( 5 \times 3 \) 行列になります。

状態更新式の次元

以下の表に、状態更新式の変数の次元をまとめます。