Extended Kalman Filter (EKF)

확장 칼만 필터(EKF)는 드론의 자세나 거리와 방위를 측정하는 레이더로 관측한 표적 위치처럼, 움직임이나 측정이 비선형인 시스템의 상태를 추정하는 재귀 알고리즘입니다. 표준 칼만 필터(KF)는 가우시안 잡음을 가진 선형 모델에 대해서만 정확합니다. EKF는 각 단계에서 Jacobian 행렬의 1차 도함수를 사용해 현재 추정값 주변에서 비선형 모델을 선형화한 다음, 익숙한 예측 및 업데이트 방정식을 적용합니다. NASA Ames의 Gerald Smith, Stanley Schmidt, Leonard McGee는 1960년대 초 달 궤도 비행체의 탑재 항법을 위해 EKF를 개발했습니다. 이 연구는 Apollo 항법 컴퓨터 개발에 기여했으며, EKF는 항법 시스템, GPS 수신기, 드론, 로봇의 비선형 상태 추정을 위한 사실상의 표준으로 여전히 사용됩니다.

EKF의 작동 방식#

칼만 필터와 마찬가지로 EKF는 상태 추정값 x과 불확실성을 나타내는 공분산 P을 유지하지만, 운동 모델 f과 측정 모델 h은 미분 가능한 함수라면 무엇이든 사용할 수 있습니다. 각 단계는 두 가지 단계로 구성됩니다:

1. 예측: 추정값이 비선형 운동 모델 x = f(x)을 통과합니다. 필터는 추정값에서 Jacobian F = ∂f/∂x을 계산하고 프로세스 잡음 Q을 사용해 불확실성을 증가시킵니다: P = F P Fᵀ + Q.
2. 업데이트: 측정값 z이 들어옵니다. 필터는 예측된 상태에서 측정 Jacobian H = ∂h/∂x을 계산하고, 잔차 y = z − h(x)와 이득 K = P Hᵀ (H P Hᵀ + R)⁻¹을 계산합니다. 여기서 R는 측정 잡음이며, x = x + K y와 P = (I − K H) P을 사용해 추정값을 보정합니다.

상태와 예측 측정값에는 실제 비선형 함수가 적용되며, 공분산과 이득에만 선형화된 Jacobian을 사용합니다. f과 h이 선형이면 Jacobian은 모델 행렬과 같아지고 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는 최적 추정기가 아닙니다. 초기 추정값이 크게 벗어나 있거나 모델이 강한 비선형성을 보이면 선형화 오차로 인해 필터가 불확실성을 실제보다 낮게 추정한 뒤 발산할 수 있으므로, 엔지니어는 안전을 위해 프로세스 잡음을 높이는 경우가 많습니다. Jacobian은 수작업이나 자동 미분으로 유도해야 하며, 잘못된 도함수는 감지되지 않는 오류의 흔한 원인입니다. 방위나 방향과 같은 각도 잔차도 −π에서 π 범위로 래핑해야 합니다. 여러 변형이 이러한 문제를 다룹니다. 반복 EKF는 업데이트 중 측정 모델을 다시 선형화하며, 오차 상태 EKF는 기준 상태 주변의 작은 보정값을 추정합니다. 후자는 관성 항법에서 쿼터니언을 사용해 3D 자세를 추적하는 일반적인 방법입니다.

EKF와 다른 필터 비교#

필터 선택은 모델의 비선형성 정도와 불확실성 분포의 형태에 따라 달라집니다:

• 칼만 필터: 두 모델이 모두 선형일 때 최적이며 계산 비용이 낮습니다. ByteTrack과 같은 바운딩 박스 추적기는 박스 좌표를 직접 측정하므로 선형 필터로 충분합니다.
• 무향 칼만 필터(UKF): 비선형 함수를 선형화하는 대신 소수의 결정론적 시그마 점을 통과시키며, Jacobian이 필요 없고 모델의 곡률이 큰 경우 대체로 EKF보다 정확합니다.
• 파티클 필터: 다수의 가중치가 적용된 무작위 샘플로 확률 분포를 표현하므로, 어느 쪽 복도에 있는지 확신할 수 없는 로봇처럼 여러 모드를 가진 분포를 추적할 수 있지만 연산 비용이 훨씬 높습니다.
• 은닉 마르코프 모델(HMM): 유한한 이산 상태 집합에 동일한 예측 및 업데이트 재귀를 적용합니다.

실제 적용 사례#

• 드론 및 항공기: PX4 오토파일럿의 EKF2 추정기는 24개 상태의 EKF를 실행해 IMU, GPS, 자력계, 기압계 및 기타 센서 데이터를 융합하고 자세, 속도, 위치, 센서 바이어스, 지구 자기장, 바람을 추정합니다.
• 로봇공학: ROS robot_localization 패키지는 휠 오도메트리, IMU, GPS를 융합하는 EKF 및 UKF 노드를 제공하며, EKF-SLAM은 동시적 위치 추정 및 지도 작성의 대표적인 접근 방식입니다. 다중 상태 제약 칼만 필터(MSCKF)와 같은 EKF 기반 시각 관성 오도메트리는 이미지 feature와 IMU 측정값으로 카메라 자세를 추적합니다.
• 자동차 센서 융합: 운전자 보조 및 자율주행차 시스템은 레이더의 거리 및 방위 측정값을 카메라 및 lidar 탐지 결과와 융합해 주변 차량의 위치와 속도를 추적합니다.
• 카메라 기반 위치 추정: 단일 카메라는 물체의 방향만 측정합니다. 탐지된 물체를 지도에 표시하는 파이프라인은 EKF 측정값으로 Ultralytics YOLO26 탐지 결과를 사용합니다. 각 박스 중심은 픽셀 위치이며, 카메라 모델을 통해 물체의 3D 위치를 비선형 투영한 값입니다. 필터는 이 값을 레이더 거리 측정값이나 카메라 플랫폼의 GPS 및 IMU 데이터와 융합합니다.