개발 중인 ROS 프로젝트에 IMU의 마그네틱필드 센서 드리프트를 보정하고자 공부했던 ekf 입니다.
GPS와 RTK로 센서퓨전으로 사용하기 위해 써봤지만, 제가 원하는 시나리오와 로케이션에서는 IMU의 마그네틱필드를 ekf만으로 보정하기에는 무리가 있었기에 현재는 GPS fix 상태에서 가상의 3m~4m의 두 점을 기준으로 방위각을 측정하여 초기 IMU의 진북 오류를 보정하는 방식으로 사용하고 있습니다.
EKF(Extended Kalman Filter)는 재귀적인 필터링 알고리즘으로, 상태 추정 문제에 적용됩니다. EKF는 비선형 동적 모델과 비선형 측정 모델을 가정하고, 선형 근사를 사용하여 이를 선형화하여 추정을 수행합니다.
EKF의 주요 수학적 공식은 다음과 같습니다.
1. 예측 단계 (Predict):
- 예측된 상태 벡터 (사후 예측): x̂ₖ⁻ = f(x̂ₖ₋₁, uₖ₋₁)
- 여기서 x̂ₖ⁻는 이전 상태의 예측값입니다.
- f는 비선형 동적 모델을 선형화한 함수입니다.
- uₖ₋₁는 외부 입력(제어 입력)입니다.
- 예측된 오차 공분산 행렬 (사후 예측 오차 공분산): Pₖ⁻ = FₖPₖ₋₁Fₖᵀ + Qₖ
- 여기서 Pₖ⁻는 이전 오차 공분산 행렬의 예측값입니다.
- Fₖ는 동적 모델을 선형화한 야코비안 행렬입니다.
- Qₖ는 프로세스 노이즈 공분산 행렬로, 시스템의 모델 오차를 나타냅니다.
2. 측정 단계 (Update):
- 예측된 측정값: ẑₖ = h(x̂ₖ⁻)
- 여기서 ẑₖ는 예측된 측정값입니다.
- h는 비선형 측정 모델을 선형화한 함수입니다.
- 측정 잔차: yₖ = zₖ - ẑₖ
- 여기서 yₖ는 측정 잔차(예측값과 실제 측정값의 차이)입니다.
- 측정 잔차 공분산 행렬: Sₖ = HₖPₖ⁻Hₖᵀ + Rₖ
- 여기서 Hₖ는 측정 모델을 선형화한 야코비안 행렬입니다.
- Rₖ는 측정 노이즈 공분산 행렬로, 측정값의 오차를 나타냅니다.
- 최적 게인 행렬: Kₖ = Pₖ⁻HₖᵀSₖ⁻¹
- 여기서 Kₖ는 최적 게인(최적 가중치) 행렬입니다.
- 상태 추정치: x
̂ₖ = x̂ₖ⁻ + Kₖyₖ
- 여기서 x̂ₖ는 측정값을 사용한 상태 추정값입니다.
- 오차 공분산 행렬: Pₖ = (I - KₖHₖ)Pₖ⁻
- 여기서 Pₖ는 오차 공분산 행렬로, 상태 추정치의 오차를 나타냅니다.
여기서 주목해야 할 점은, EKF가 비선형 동적 모델과 비선형 측정 모델을 선형화하여 사용한다는 것입니다. 선형화는 야코비안 행렬인 Fₖ와 Hₖ를 사용하여 수행됩니다. 따라서 선형화 과정에서 근사 오차가 발생할 수 있습니다. 그러나 EKF는 이러한 근사 오차를 최소화하여 비선형 시스템에 대한 상대적으로 정확한 추정을 제공합니다.