🔄 Day 24: EKF로 비선형 상태 추정 실습

안녕하세요! ✨
앞서 Day 23에서는 확장 칼만필터(EKF)의 원리를 정리했습니다.
오늘은 EKF를 실제로 구현해, 비선형 시스템 상태 추정이 어떻게 이루어지는지 실습해보겠습니다.

EKF로 비선형 상태 추정 실습

---

🔍 EKF 실습 시나리오

• 차량이 2D 평면에서 움직이고 있다고 가정
• 상태 벡터: [x,y,v][x, y, v][x,y,v] (위치 x, 위치 y, 속도 v)
• 센서:
  • GPS → 위치(x, y) 제공 (노이즈 있음)
  • IMU → 가속도 제공 (노이즈 있음)

👉 EKF로 두 센서 데이터를 융합해 차량의 상태를 추정합니다.

---

⚙️ Python EKF 구현 예제

python

 

 

import numpy as np

# 초기 상태 [x, y, v]
x = np.array([0.0, 0.0, 1.0])
P = np.eye(3)  # 초기 공분산
dt = 1.0       # 샘플링 시간
Q = np.eye(3) * 0.01  # 프로세스 잡음
R = np.eye(2) * 5.0   # 측정 잡음

def f(x):
    # 비선형 상태 전이 함수
    x_new = np.zeros_like(x)
    x_new[0] = x[0] + x[2] * np.cos(np.pi/6) * dt
    x_new[1] = x[1] + x[2] * np.sin(np.pi/6) * dt
    x_new[2] = x[2]
    return x_new

def F_jacobian(x):
    # f(x)의 Jacobian
    F = np.eye(3)
    F[0,2] = np.cos(np.pi/6) * dt
    F[1,2] = np.sin(np.pi/6) * dt
    return F

def h(x):
    # 비선형 관측 함수 (GPS 위치만 제공)
    return np.array([x[0], x[1]])

def H_jacobian(x):
    # h(x)의 Jacobian
    H = np.zeros((2,3))
    H[0,0] = 1
    H[1,1] = 1
    return H

# 가상의 GPS 측정값 (노이즈 포함)
measurements = [np.array([i + np.random.normal(0,2), i + np.random.normal(0,2)]) for i in range(20)]

for z in measurements:
    # 1. 예측 단계
    x_pred = f(x)
    F = F_jacobian(x)
    P_pred = F @ P @ F.T + Q

    # 2. 업데이트 단계
    H = H_jacobian(x_pred)
    y = z - h(x_pred)  # 혁신(Residual)
    S = H @ P_pred @ H.T + R
    K = P_pred @ H.T @ np.linalg.inv(S)
    x = x_pred + K @ y
    P = (np.eye(3) - K @ H) @ P_pred

    print("추정 상태:", x)

---

📊 결과 해석

• GPS 관측값은 노이즈 때문에 불안정
• EKF 추정값은 점점 안정되어 실제 차량 궤적에 근접
• Jacobian을 이용한 선형 근사 덕분에 비선형 시스템에서도 칼만필터 적용 가능

---

🚘 자율주행 응용

• GPS + IMU 융합: 실시간 위치 추정 안정화
• Radar + Camera: 객체 위치 및 속도 추정
• SLAM: 로봇/자율주행차의 지도 작성 및 자기 위치 인식

---

📌 정리

• EKF는 비선형 상태 추정에 활용되는 칼만필터 확장판
• 핵심: 비선형 함수 f(x), h(x)를 Jacobian으로 선형 근사
• Python으로 간단한 EKF를 구현하면 원리를 쉽게 이해할 수 있음

반응형

---

🚀 다음 글 예고

다음 글에서는 [Day 25: 센서 융합 (LiDAR + IMU) 실습] 을 다룹니다.
ROS2 환경에서 LiDAR와 IMU 데이터를 융합해 실제 주행 데이터를 다뤄보겠습니다.

---

✅ 오늘 글이 도움이 되셨다면 좋아요와 공유 부탁드립니다.