カルマンフィルタ

カルマンフィルタとは線形の運動モデルと観測モデルから状態を推定するフィルタの一種である

離散化モデル

サンプリング周期で離散化した線形モデルが以下で与えられるとする。

\[ \begin{array}{lll} x(t) = Fx(t-1) + Gu(t) + w(t) \\ z(t) = Hx(t) + v(t) \end{array} \]

ここで各変数は以下の通り
\(x \in \mathbb{R}^{n \times 1}\) : 状態量
\(u \in \mathbb{R}^{r \times 1}\) : 制御量
\(z \in \mathbb{R}^{p \times 1}\) : 観測量
\(F \in \mathbb{R}^{n \times n}\) : 状態遷移行列
\(G \in \mathbb{R}^{n \times r}\) : 制御行列
\(H \in \mathbb{R}^{p \times n}\) : 観測行列

\(Q \in \mathbb{R}^{n \times n}\) : システムノイズの共分散行列
\(w \in \mathbb{R}^{n \times 1}\) : システムノイズ \(N[0, Q]\)に従う
\(R \in \mathbb{R}^{p \times p}\) : 観測ノイズの共分散行列
\(v \in \mathbb{R}^{p \times 1}\) : 観測ノイズ \(N[0, R]\)に従う