・カルマンフィルタ
信念をガウス分布(平均と分散)で表現したベイズフィルタ。
更新式の導出は1次と2次のモーメント(1次微分と2次微分)を計算することによって行う。
動作や計測が線形でないと使えない。

・拡張カルマンフィルタ(EKF)
テイラー展開を使って動作や計測を線形化する。
動作や計測の関数は、状態→状態なので、状態数nのとき、ヤコビ行列はn×nになる。

・EKF位置推定
制御更新
 平均値:普通に変換
 共分散:GΣt-1G+VMV
  G:制御ヤコビ、Σt-1:前時刻の共分散、V:制御空間→状態空間ヤコビ、M:制御空間での動作共分散
計測更新
 K=Σ'H(HΣ'H+Q)
  K:カルマンゲイン、Σ':計測前信念の共分散、H:計測(状態→マーカー)ヤコビ、Q:計測共分散
ヤコビ
 制御ヤコビ:状態(x, y, θ)→状態(x, y, θ)
 計測ヤコビ:状態(x, y, θ)→マーカー(r, φ, s)
 制御→状態ヤコビ:制御(v, ω)→状態(x, y, θ)