拡張カルマンフィルタ(EKF)は、ロボットのローカリゼーションにおいて広く使用されているアルゴリズムです。ロボットのモーションの数学モデルとセンサーデータを組み合わせることで、ロボットのポジションとオリエンテーションを推定します。EKFの数学的基礎を理解することは、効果的な実装とチューニングに不可欠です。

国家の代表と予測

EKFのコアは、一般的に位置と方向を含む、ロボットの状態をベクトルとして表すものを含みます。予測ステップは、非線形運動モデルを使用して、現在の状態を前もって予測します。これは、モーションモデルのヤコブーンマトリックスを計算し、現在の推定値の周りの非線形式を線形化することを含みます。

測定の更新とリニア化

センサー測定は更新ステップで組み込まれています。測定は、多くの場合、状態の非線形関数であるため、EKFは、ジェイコブリアンを使用してこれらの機能を線形化します。このプロセスは、予想と実際のセンサーの読み込みの違いに基づいて予測された状態を調整します。

数学的な式

予測式は以下のとおりです。

予測:

x の mk+1 の = f (x の mk、イギリス)

の相反予測:[

Pk+1−=Fk Pk Fkt + Qk

f が非線形運動モデルである場合、Fk はジェイコブアン、P は共和性の行列であり、Q はプロセス騒音の共和です。

更新の式は次のとおりです。

カルマンゲイン:

Kk = Pk−Hkt (Hk Pk−Hkt + Rk)−1

ステータス更新:

xk = x mk- + Kk (zk - h(x の mk-)))

キャパオリアンスアップデート:[

Pk = (I - Kk Hk) Pk−

ここでは、hは非線形測定モデルで、hkはジェイコブアン、Rkは測定ノイズの共変量であり、zkは実際のセンサー測定です。