これまでのまとめ

ここで先に進む前に、これまで学んできたことをまとめたいと思います。

"1次元カルマンフィルタ"で学んだように(もし忘れてしまっていたら、もう一度復習してください)、カルマンフィルタの計算は5つの式で成り立っています。

2つの予測方程式

  • 状態方程式 - 現在の推定値に基づいて、将来の状態を予測または推定します。
  • 共分散遷移式 - 予測の不確かさを求めます。

2つの更新式

  • 状態更新式 - 過去の推定値と現在の測定値に基づいて、現在の状態を推定します。
  • 共分散更新式 - 推定の不確かさを求めます。

カルマゲインの式 – 更新式を計算するために必要となります。カルマンゲインは、実際には観測値と過去の推定値に対する「重み付け」パラメータです。過去の推定の重みと、現在の状態を推定する際の観測の重みを定義します。

ここまでで、行列表記による2つの予測方程式と、メインの方程式の計算に必要ないくつかの補助方程式を学習した。

予測方程式

状態方程式

一般的な、行列表記の状態方程式は次のように表されます。

\[ \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+Gu_{n}+w_{n}} \]
ここで、
\( \boldsymbol{\hat{x}_{n+1,n}} \) :時刻 \( n + 1 \) における状態ベクトル(予測値)
\( \boldsymbol{\hat{x}_{n,n}} \) :時刻 \( n \) における状態ベクトル(推定値)
\( \boldsymbol{u_{n}} \) :制御変数 または 入力変数。システムへの観測可能な入力である。
\( \boldsymbol{w_{n}} \) :プロセス雑音または外乱。 これらは、状態に影響を及ぼすが、観測できない
\( \boldsymbol{F} \) :システム行列
\( \boldsymbol{G} \) :入力係数行列

共分散遷移式

一般的な、共分散遷移式は次のように表されます。

\[ \boldsymbol{P_{n+1,n} = FP_{n,n}F^{T} + Q} \]
ここで、
\( \boldsymbol{P_{n,n}} \) :現時点における共分散行列(推定値)
\( \boldsymbol{P_{n+1,n}} \) :次時点における共分散行列(予測値)
\( \boldsymbol{F} \) :"線形システムのモデリング"で導出した、システム行列
\( \boldsymbol{Q} \) :プロセス雑音の共分散行列

補助方程式

観測方程式

一般的な、行列表記の観測方程式は次のように表されます。

\[ \boldsymbol{z_{n} = Hx_{n} + v_{n}} \]
ここで、
\( \boldsymbol{z_{n}} \) :観測ベクトル
\( \boldsymbol{x_{n}} \) :真のシステムの状態(観測不可)
\( \boldsymbol{v_{n}} \) :ランダム雑音ベクトル
\( \boldsymbol{H} \) :観測行列

共分散方程式

プロセスノイズと測定ノイズのベクトルに対応する \( \boldsymbol{w} \) と \( \boldsymbol{v} \) の項は未知であり、通常、関心のある方程式に直接現れないという点で興味深いものです。

これらの項は方程式自身の不確かさ(または雑音)をモデル化するために使用されます。

共分散行列は次の式で表されます。

\[ E \left( \boldsymbol{ee^{T}} \right) \]

すなわち、期待値の2乗誤差です。詳細は背景を参照してください。

観測値の不確かさ

観測値の共分散行列は次の式で表されます。

\[ \boldsymbol{R_{n}} = E\left( \boldsymbol{v_{n}v_{n}^{T}} \right) \]
ここで、
\( \boldsymbol{R_{n}} \) :観測値の共分散行列
\( \boldsymbol{v_{n}} \) :観測誤差

プロセス雑音の不確かさ

プロセス雑音の共分散行列は次のように表されます。

\[ \boldsymbol{Q_{n}} = E\left( \boldsymbol{w_{n}w_{n}^{T}} \right) \]
ここで、
\( \boldsymbol{Q_{n}} \) :プロセス雑音の共分散行列
\( \boldsymbol{w_{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}} \) :誤差共分散行列
\( \boldsymbol{e_{n}} \) :推定誤差
\( \boldsymbol{x_{n}} \) :真のシステムの状態(観測不可)
\( \boldsymbol{\hat{x}_{n,n}} \) :時刻 \( n \) における状態ベクトル(推定値)