Extended Kalman Filter (EKF)

拡張カルマンフィルター（EKF）は、ドローンの姿勢や、距離と方位を報告するレーダーで観測した目標の位置など、運動や計測が非線形であるシステムの状態を推定する再帰アルゴリズムです。標準的なカルマンフィルター（KF）が正確なのは、ガウスノイズを伴う線形モデルに限られます。EKFは各ステップで現在の推定値の周りをヤコビ行列で線形化し、非線形モデルに対応したうえで、よく知られた予測・更新方程式を適用します。NASAエイムズ研究センターのGerald Smith、Stanley Schmidt、Leonard McGeeが1960年代初頭に月周回飛行体の搭載航法向けに開発した技術で、アポロの航法コンピューターにもつながりました。現在も、航法システム、GPS受信機、ドローン、ロボットにおける非線形状態推定の事実上の標準として使われています。

EKFの仕組み#

カルマンフィルターと同様に、EKFは状態推定値xと、不確実性を表す共分散Pを保持します。一方、運動モデルfと観測モデルhには、任意の微分可能関数を使用できます。各ステップは2つのフェーズで構成されます。

1. 予測：推定値を非線形運動モデルx = f(x)に通します。フィルターは推定値でヤコビ行列F = ∂f/∂xを評価し、プロセスノイズQを使って不確実性を増加させます：P = F P Fᵀ + Q。
2. 更新：観測値zが得られます。フィルターは予測状態で観測ヤコビ行列H = ∂h/∂xを評価し、残差y = z − h(x)とゲインK = P Hᵀ (H P Hᵀ + R)⁻¹を計算します。ここでRは観測ノイズです。その後、x = x + K yとP = (I − K H) Pを使って推定値を補正します。

状態と予測観測値には真の非線形関数を適用し、共分散とゲインにのみ線形化したヤコビ行列を使います。fとhが線形の場合、ヤコビ行列はモデル行列に等しくなり、EKFはカルマンフィルターと完全に一致します。不確実性の広がりの範囲内でモデルがほぼ線形であれば近似は良好ですが、その範囲内で大きく曲がる場合は精度が低下します。

距離と方位を測定するレーダーは典型例です。x座標とy座標上を直線移動する目標はそれらの座標では線形ですが、距離√(x² + y²)と方位atan2(y, x)は線形ではありません。このEKFはそのような目標を追跡し、レーダーでは直接測定できない速度を推定します。

import numpy as np

rng = np.random.default_rng(0)
F = np.eye(4) + np.eye(4, k=2)  # constant velocity, state = [x, y, vx, vy], 1 s per step
Q, R = 1e-4 * np.eye(4), np.diag([1.0**2, 0.02**2])  # process noise; radar noise of 1 m and 0.02 rad
s = np.array([20.0, 50.0, 1.0, -0.5])  # true state
x, P = np.array([20.0, 50.0, 0.0, 0.0]), np.diag([1.0, 1.0, 4.0, 4.0])  # estimate starts with unknown velocity

raw_err, ekf_err = [], []
for _ in range(60):
    s = F @ s
    z = np.array([np.hypot(s[0], s[1]), np.arctan2(s[1], s[0])]) + rng.normal(0, [1.0, 0.02])  # range, bearing
    x, P = F @ x, F @ P @ F.T + Q  # predict
    r = np.hypot(x[0], x[1])
    H = np.array([[x[0] / r, x[1] / r, 0, 0], [-x[1] / r**2, x[0] / r**2, 0, 0]])  # Jacobian of h(x)
    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
    x = x + K @ (z - np.array([r, np.arctan2(x[1], x[0])]))  # update with the nonlinear residual
    P = (np.eye(4) - K @ H) @ P
    raw_err.append(np.hypot(z[0] * np.cos(z[1]) - s[0], z[0] * np.sin(z[1]) - s[1]))
    ekf_err.append(np.hypot(x[0] - s[0], x[1] - s[1]))

print(f"Mean position error: raw radar {np.mean(raw_err):.2f} m, EKF {np.mean(ekf_err):.2f} m")
print(f"Estimated velocity: ({x[2]:.2f}, {x[3]:.2f}) m/s, true (1.00, -0.50)")

各距離と方位の生データをx座標とy座標に変換すると、60ステップでの平均位置誤差は1.43 mになります。一方、EKFの推定誤差は0.57 mで、フィルターは速度を(1.01, −0.50) m/sと推定します。

制限事項とバリエーション#

EKFは最適な推定器ではありません。初期推定が大きく外れていたり、モデルが強く非線形であったりすると、線形化誤差によってフィルターが過信した後に発散することがあるため、エンジニアは安全のためにプロセスノイズを大きめに設定することがよくあります。ヤコビ行列は手作業または自動微分で導出する必要があり、微分の誤りは検出されない障害の一般的な原因です。方位や進行方向などの角度の残差も、−πからπの範囲に収める必要があります。こうした問題に対処する複数のバリエーションがあります。反復EKFは更新中に観測モデルを再線形化し、誤差状態EKFは基準状態の周りの小さな補正量を推定します。これは、慣性航法でクォータニオンを用いて3D姿勢を追跡する際によく使われる方法です。

EKFと他のフィルターの比較#

どのフィルターを選ぶかは、モデルの非線形性の程度と不確実性の形状によって異なります。

• カルマンフィルター: 両方のモデルが線形であれば、最適で計算コストも低くなります。ByteTrackなどのバウンディングボックストラッカーは、ボックス座標を直接測定するため、線形フィルターで十分です。
• 無香料カルマンフィルター（UKF）：線形化する代わりに、少数の決定論的なシグマ点を非線形関数に通します。ヤコビ行列が不要で、強く曲がるモデルでは通常、EKFより高精度です。
• パーティクルフィルター：多数の重み付きランダムサンプルで確率分布を表現するため、ロボットが2つの廊下のどちらにいるか判断できない場合など、多峰性分布を追跡できます。ただし、計算コストは大幅に高くなります。
• 隠れマルコフモデル（HMM）：同じ予測・更新の再帰処理を、有限個の離散状態に適用します。

実際のアプリケーション#

• ドローンと航空機：PX4オートパイロットのEKF2推定器は24状態のEKFを実行し、IMU、GPS、磁力計、気圧計などのセンサーデータを統合して、姿勢、速度、位置、センサーのバイアス、地球の磁場、風を推定します。
• ロボティクス：ROSのrobot_localizationパッケージは、車輪オドメトリ、IMU、GPSを統合するEKFおよびUKFノードを提供します。また、EKF-SLAMは同時自己位置推定とマッピングの古典的な手法です。マルチステート制約カルマンフィルター（MSCKF）などのEKFベースの視覚慣性オドメトリは、画像特徴とIMUの読み取り値からカメラの姿勢を追跡します。
• 自動車のセンサーフュージョン：運転支援システムと自動運転車システムは、レーダーの距離と方位をカメラやLiDARの検出結果と統合して、周囲の車両の位置と速度を追跡します。
• カメラベースの位置推定：単眼カメラで測定できるのは、物体の方向だけです。検出した物体を地図上に配置するパイプラインでは、Ultralytics YOLO26の検出結果をEKFの観測値として使用します。各ボックスの中心はピクセル位置であり、カメラモデルを通じた物体の3D位置の非線形投影です。フィルターはこれをレーダーの距離、またはカメラプラットフォームのGPSやIMUのデータと統合します。