Extended Kalman Filter (EKF)
拡張カルマンフィルタ（Extended Kalman Filter）は、非線形モデルを用いた逐次状態推定の主力手法である。状態に対するガウス信念 N ( x ^ , P ) \mathcal{N}(\hat{\mathbf{x}}, P) N ( x ^ , P ) 予測（predict） （運動モデルを通じて信念を伝播する）と 更新（update） （観測によってそれを補正する）の2つのステップを交互に行う — これは、すべての密度をガウス分布で近似し、すべてのモデルを現在の推定値で線形化した、まさに逐次ベイズフィルタである。
セットアップ
システムは非線形な運動モデルと非線形な観測モデルによって記述される。
x k = f ( x k − 1 , u k ) + w k , w k ∼ N ( 0 , Q k ) \mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k, \qquad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, Q_k) x k = f ( x k − 1 , u k ) + w k , w k ∼ N ( 0 , Q k ) z k = h ( x k ) + v k , v k ∼ N ( 0 , R k ) \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k, \qquad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, R_k) z k = h ( x k ) + v k , v k ∼ N ( 0 , R k ) ここで x \mathbf{x} x u \mathbf{u} u z \mathbf{z} z Q Q Q R R R f f f h h h
F k = ∂ f ∂ x ∣ x ^ k − 1 ∣ k − 1 , H k = ∂ h ∂ x ∣ x ^ k ∣ k − 1 F_k = \left.\frac{\partial f}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k-1|k-1}}, \qquad
H_k = \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k|k-1}} F k = ∂ x ∂ f x ^ k − 1∣ k − 1 , H k = ∂ x ∂ h x ^ k ∣ k − 1 予測
x ^ k ∣ k − 1 = f ( x ^ k − 1 ∣ k − 1 , u k ) \hat{\mathbf{x}}_{k|k-1} = f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_k) x ^ k ∣ k − 1 = f ( x ^ k − 1∣ k − 1 , u k ) P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k T + Q k P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k P k ∣ k − 1 = F k P k − 1∣ k − 1 F k T + Q k 平均は完全な非線形モデルを通して伝播される。共分散のみが線形化を用いる。このステップでは不確実性が増大する（デッドレコニングによるドリフト）。
更新
y k = z k − h ( x ^ k ∣ k − 1 ) (イノベーション) \mathbf{y}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1}) \qquad \text{(イノベーション)} y k = z k − h ( x ^ k ∣ k − 1 ) ( イノベーション ) S k = H k P k ∣ k − 1 H k T + R k (イノベーション共分散) S_k = H_k P_{k|k-1} H_k^T + R_k \qquad \text{(イノベーション共分散)} S k = H k P k ∣ k − 1 H k T + R k ( イノベーション共分散 ) K k = P k ∣ k − 1 H k T S k − 1 (カルマンゲイン) K_k = P_{k|k-1} H_k^T S_k^{-1} \qquad \text{(カルマンゲイン)} K k = P k ∣ k − 1 H k T S k − 1 ( カルマンゲイン ) x ^ k ∣ k = x ^ k ∣ k − 1 + K k y k , P k ∣ k = ( I − K k H k ) P k ∣ k − 1 \hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + K_k \mathbf{y}_k, \qquad
P_{k|k} = (I - K_k H_k)\, P_{k|k-1} x ^ k ∣ k = x ^ k ∣ k − 1 + K k y k , P k ∣ k = ( I − K k H k ) P k ∣ k − 1 ゲイン K k K_k K k P P P R R R S k S_k S k ゲーティング もサポートする — マハラノビス検定 y T S − 1 y < χ 2 \mathbf{y}^T S^{-1} \mathbf{y} < \chi^2 y T S − 1 y < χ 2
EKF-SLAM
EKF-SLAM（MonoSLAMの背後にある定式化）では、状態はカメラ/ロボットの姿勢とすべてのランドマーク位置を積み重ねたものである。
x = [ x robot T m 1 T ⋯ m n T ] T , \mathbf{x} = \begin{bmatrix} \mathbf{x}_{\text{robot}}^T & \mathbf{m}_1^T & \cdots & \mathbf{m}_n^T \end{bmatrix}^T, x = [ x robot T m 1 T ⋯ m n T ] T , そしてP P P
二次スケーリング : P P P O ( n 2 ) O(n^2) O ( n 2 ) 線形化誤差 : ヤコビ行列は現在の推定値で一度評価され、誤差はP P P 不整合性 （過信）の原因である。回転の扱い : 姿勢の素朴なパラメータ化はうまく振る舞わない。実用的なフィルタは誤差状態（間接）EKF を用い、フィルタが名目状態の周りの小さな誤差を推定し、クォータニオンのキネマティクスが姿勢を扱う。
これらの限界こそが、現代の視覚SLAMがキーフレームベースの非線形最適化に移行した理由である（「なぜフィルタなのか」論争、Strasdat et al.）。一方で、フィルタリングは、その定数時間の逐次形式が生きる領域で生き残っている。視覚慣性オドメトリ（MSCKF、ROVIO、誤差状態EKF）や、GPS/RADAR/ホイールエンコーダとのオドメトリ融合である。
SLAMにおける意義
EKFはSLAMの歴史的な基盤であり（EKF-SLAMは10年間唯一の 解であった）、今も実用VIOの背骨である（MSCKF系のフィルタは多くのARヘッドセットやドローンで動いている）。最適化中心のパイプラインにおいても、EKFの概念は至る所に存在する。予測/更新の構造、外れ値棄却のためのイノベーションゲーティング、不確実性伝播のための共分散、そしてマージナライゼーション（固定ラグスマザーのマージナライゼーションステップは、代数的にはカルマン更新である）。EKFの単一線形化という仮定がいつ破綻するかを理解することは、なぜバンドル調整が精度で勝るのか、そしてなぜMSCKF系フィルタが線形化を遅延させるのかを理解する鍵である。
関連ノート