제7부 실전: ROS2 Humble 프로젝트

7.5 실전 ④ 상태 추정 융합 (EKF)

제7부 실전: ROS2 Humble 프로젝트 · 네 번째 프로젝트. 제어에서 추정으로 — 5.2·5.3·5.4를 ROS2 실전으로. 드리프트하는 오도메트리와 절대적이나 느린 GPS(또는 IMU)를 EKF로 융합해 로봇 위치를 추정한다. 추측항법 대비 융합의 효과를 실측으로 보이고, ROS2 표준 robot_localization을 소개.

✅ 검증됨: 2D 포즈 EKF 융합(오도메트리 예측 + GPS 보정)을 ros:humble 컨테이너에서 실행해 검증(verify/ce34_ekf_fusion.py) — 추측항법 RMS 3.12 m vs EKF 융합 RMS 0.35 m, 9.1배 개선. robot_localization 실로봇 통합은 독자 환경 몫.

이 장을 마치면 2D 포즈 EKF를 구성하고, 융합이 드리프트를 억제하는 이유를 실측으로 확인하며, robot_localization으로 실전 융합을 설정할 줄 알아야 한다.

7.5.1 문제 — 오도메트리는 흐르고, GPS는 느리다

이동로봇의 위치를 어떻게 알까? 어느 센서도 단독으로는 부족하다(5.4.1):

오도메트리(바퀴 엔코더·IMU 적분): 고속·매끄럽지만 바이어스·미끄러짐이 누적돼 시간이 갈수록 드리프트한다(절대 기준 없음).
GPS(또는 맵 기반 위치): 절대 위치를 주지만 느리고·잡음 많고·끊긴다, 자세는 없다.

EKF 융합: 단기에는 오도메트리로 매끄럽게 추측하고(예측), 가끔 오는 GPS로 드리프트를 억제한다(갱신). 5.2의 예측·갱신을 비선형(EKF, 5.3)으로.

7.5.1.1 이 장이 실제로 검증한 범위 (정직)

이 장의 제목·앞선 설명은 "오도메트리+GPS(또는 IMU)"처럼 폭넓게 적었지만, 실제로 실행·검증된 수치 예제(7.5.3, 9.1배 개선)는 오도메트리+GPS 융합뿐이다. IMU는 7.5.4의 imu0_config YAML 스니펫으로만 등장하며 실행되지 않았다 — 방위(orientation) 측정을 EKF에 섞으면 7.5.5·5.3.7이 경고하는 각도 wrapping 문제가 바로 나타나는데, 이 장은 그 처리를 코드로 검증하지 않았다. 오도메트리+IMU 융합을 실제로 확인하려면 7.5.2의 코드에 방위 측정(관측행렬 $H$에 행 추가, 혁신 $[-\pi,\pi]$ wrap)을 더해 직접 검증해야 한다 — 이 장은 그 뼈대(오도메트리+GPS 2D 포즈 EKF)만 제공한다.

7.5.2 2D 포즈 EKF

상태 $[x, y, \theta]$. 운동 모델은 비선형($\cos\theta, \sin\theta$)이라 EKF(5.3)를 쓴다.

예측(오도메트리 v, ω 입력): x̂⁻ = f(x̂, v, ω): x += v cosθ·dt, y += v sinθ·dt, θ += ω·dt

$$P^- = FPF^\top + Q, \quad F = \dfrac{\partial f}{\partial x} = \begin{bmatrix} 1 & 0 & -v\sin\theta\cdot dt \\ 0 & 1 & v\cos\theta\cdot dt \\ 0 & 0 & 1 \end{bmatrix}$$

갱신(GPS 위치 z=[x,y] 도착 시):

$$\tilde{y} = z - H\hat{x}^-, \quad H = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \end{bmatrix} \quad \text{(위치만 측정)}$$

$S = H P^- H^T + R, K = P^- H^T S^{-1}$
$\hat{x} = \hat{x}^- + K \tilde{y}, P = (I - K H) P^-$
예측은 매 스텝(고속 오도메트리), 갱신은 GPS 올 때만(저속). 측정이 없으면 $P$가 계속 커진다(드리프트 = 불확실성 증가, 5.2.3).
여기서는 위치만 측정하므로 각도 wrapping이 불필요하지만, 방위(compass) 측정을 융합하면 혁신을 $[-\pi,\pi]$로 wrap해야 한다(5.3.7).

7.5.2.1 코드 — EKF 예측·갱신 (검증됨)

7.3·7.4·7.6처럼 이 장도 위 수식을 실제 코드로 구현한다. 아래가 7.5.3의 실측 결과를 만든 핵심 루프다(전체는 verify/ce34_ekf_fusion.py — 참 궤적·바이어스·잡음 생성까지 포함):

import numpy as np

def motion(s, v, w, dt):                    # 비선형 운동모델 (예측에 씀)
    x, y, th = s
    return np.array([x + dt*v*np.cos(th), y + dt*v*np.sin(th), th + dt*w])

def F_jac(s, v, dt):                         # ∂f/∂x — motion()의 자코비안
    th = s[2]
    return np.array([[1, 0, -dt*v*np.sin(th)],
                      [0, 1,  dt*v*np.cos(th)],
                      [0, 0,  1]])

H = np.array([[1., 0, 0], [0, 1., 0]])       # GPS는 x,y 위치만 관측
Q = np.diag([0.02, 0.02, 0.01])**2           # 프로세스 잡음(오도메트리 신뢰도)
R = np.eye(2) * sig_gps**2                   # 측정 잡음(GPS 신뢰도)

x = np.array([0., 0., 0.]); P = np.eye(3) * 0.01     # 초기 추정·공분산
for k in range(N):
    # 예측 — 매 스텝, 오도메트리 v,w 입력
    Fj = F_jac(x, v_odo, dt)
    x = motion(x, v_odo, w_odo, dt)
    P = Fj @ P @ Fj.T + Q
    # 갱신 — GPS가 도착한 스텝에서만
    if gps_available(k):
        z = gps_xy(k)                         # [x,y] 측정
        S = H @ P @ H.T + R
        K = P @ H.T @ np.linalg.inv(S)
        x = x + K @ (z - H @ x)               # 혁신으로 보정
        P = (np.eye(3) - K @ H) @ P

motion·F_jac은 7.5.2의 $f$·$F$를 그대로 옮긴 것, $H$·$Q$·$R$은 위 수식과 동일하다.
루프는 매 스텝 예측하고, GPS 스텝(gps_available)에서만 갱신한다 — 7.5.1의 "단기 매끄러움(예측) + 장기 절대성(갱신)"이 코드 구조 그 자체다.

7.5.3 실행 결과 — 융합 vs 추측항법 (실측)

참 궤적(원), 바이어스+잡음 오도메트리, 20스텝마다 잡음 GPS로 60초 시뮬레이션. 추측항법(오도메트리만 적분)과 EKF 융합의 위치 오차 비교:

추측항법 위치 RMS 오차 = 3.124 m   (최종 5.126 m)   ← 바이어스 누적, 무한정 드리프트
EKF 융합  위치 RMS 오차 = 0.345 m   (최종 0.532 m)   ← GPS가 드리프트를 억제, 유계
개선 배수 = 9.1x
→ CE34_FUSION_PASS

읽기:

추측항법은 무한정 드리프트한다(바이어스가 적분돼 5 m까지 벗어남) — 절대 기준이 없으니 당연.
EKF 융합은 오차가 유계(0.3~0.5 m)다 — 가끔 오는 GPS가 누적 드리프트를 주기적으로 리셋. 단기 매끄러움(오도메트리) + 장기 절대성(GPS)을 둘 다 얻었다.
9.1배 개선 — 5.4.1의 상보 융합 원리가 수치로 확인. 이것이 칼만 융합의 존재 이유다.

7.5.4 ROS2로 — `robot_localization` (독자 환경)

직접 EKF를 작성하는 대신, 실전은 검증된 robot_localization 패키지를 쓴다(5.4.8).

빠른 오도메트리는 예측을, 느린 GPS는 navsat_transform_node를 거쳐 갱신을 담당한다 — 7.5.1·7.5.2의 예측/갱신 구조 그대로다. IMU 경로(점선)는 7.5.1.1에서 밝혔듯 이 장이 실행·검증하지 않은 부분이다.

# ekf.yaml (예시 — 어떤 입력의 어느 필드를 융합할지 지정)
ekf_filter_node:
  ros__parameters:
    frequency: 30.0
    two_d_mode: true
    odom0: /wheel/odometry      # 오도메트리: 속도 사용
    odom0_config: [false,false,false, ...,  true,true,false, ...]   # vx,vy,...
    imu0: /imu/data             # IMU: 방위·각속도 사용
    imu0_config: [...]

# (예시 흐름 — 실로봇/시뮬레이션, 독자 환경)
ros2 launch robot_localization ekf.launch.py            # ekf_node
ros2 run robot_localization navsat_transform_node       # GPS(위경도)→지역좌표

ekf_node/ukf_node: /odom·/imu·기타를 융합해 융합된 /odometry/filtered 발행.
navsat_transform_node: GPS sensor_msgs/NavSatFix(위경도)를 지역 좌표(map)로 변환해 EKF에 투입(5.4.7 프레임).
어느 센서의 어느 필드(위치·속도·자세)를 신뢰할지 *_config로 지정 — 잡음 공분산($Q,R$)은 메시지 공분산·설정으로.

⚠️ robot_localization은 보통 15차 상태(위치·자세·속도·가속)로 이 장의 3차보다 풍부하다. 원리는 같다(EKF 예측·갱신). 실로봇 연동은 GUI·하드웨어라 컨테이너 자동 검증 밖 — 이 책은 EKF 융합 원리를 검증했다.

7.5.5 정직 — 튜닝·가정·프레임

$Q, R$ 튜닝이 실무 병목(5.1.10·5.2.7): 오도메트리·GPS의 공분산을 잘못 설정하면 융합이 한 센서를 과신해 진동하거나 드리프트한다. 메시지 공분산을 정직하게 채워야.
가정(5.1.10): 가우시안·백색·독립 잡음, 정확한 운동 모델. GPS 멀티패스·바이어스 오도메트리는 이 가정을 위배한다 → 이상치 게이팅·바이어스 추정(5.3.7·5.4.6).
프레임·동기(5.4.7): map·odom·base_link 프레임, GPS 좌표변환, 타임스탬프 동기가 어긋나면 추정이 왜곡된다.
각도 wrapping(5.3.7): 방위·자세를 융합하면 혁신 wrap 필수.
단순화: 이 장 3차 EKF는 원리 데모다. 실전은 더 많은 상태·센서·예외 처리가 필요.

7.5.5.1 타임스탬프 동기 — 구체적 대응

측정이 도착 순서와 타임스탬프 순서가 다르면(네트워크 지연·센서마다 다른 주기) "받은 순서대로 갱신"은 미래로 이미 예측해 둔 상태를 과거 시점 측정으로 갱신하는 꼴이 되어 추정이 왜곡된다. 실전 대응은 크게 셋이다. 첫째, 버퍼링 후 정렬 재생 — 짧은 시간창(예: 수십~100 ms) 안의 측정을 모아 타임스탬프 순으로 정렬한 뒤 예측·갱신을 다시 재생하는 방식으로, 직접 EKF를 작성할 때 가장 단순하다. 둘째, ROS2 표준 도구 message_filters.ApproximateTimeSynchronizer로 여러 토픽을 허용 오차(slop) 안에서 짝지어 한 콜백으로 묶는 방식 — slop이 너무 크면 안 맞는 쌍을 묶고, 너무 작으면 콜백이 거의 발생하지 않는다(센서 주기에 맞춰 튜닝). 셋째, robot_localization의 ekf_node는 내부에 작은 측정 이력 버퍼를 두어, 약간 늦게 도착한 측정을 그 측정의 원래 타임스탬프 시점까지 되돌아가 재적용한 뒤 이후 상태를 다시 적분한다(무한정 지연이 아니라 설정 가능한 버퍼 길이 안에서만). 실무에서는 대부분 robot_localization의 내장 버퍼로 충분하고, 커스텀 EKF를 직접 작성할 때만 앞의 두 방식을 손으로 구현하면 된다.

7.5.6 정리 — 그리고 다음 장

오도메트리는 드리프트, GPS는 절대적이나 느리다 → EKF 융합(예측=오도메트리, 갱신=GPS)으로 단기 매끄러움 + 장기 절대성(7.5.1·7.5.2).
실측: 추측항법 RMS 3.12 m(무한 드리프트) vs EKF 융합 0.35 m(유계), 9.1배 개선 — 5.4 상보 융합의 입증(7.5.3).
실전은 robot_localization(ekf_node·navsat_transform)으로 표준화(7.5.4). 튜닝·프레임·각도 wrapping이 관건(7.5.5).

다음 7.6(선택)에서는 제약 있는 궤적 추종을 MPC(6.1)로 — 제약을 명시적으로 다루는 최적 추종으로 제7부를 마무리한다.

7.5.7 연습·생각해 볼 것

드리프트 직관. 왜 오도메트리만으로는(추측항법) 위치 오차가 무한정 커지나? GPS가 어떻게 이를 유계로 만드나? (7.5.1·7.5.3)
갱신 주기. verify/ce34_ekf_fusion.py에서 gps_every를 20→100(GPS 더 드물게)으로 바꾸면 EKF 오차가 어떻게 변하나? 예측만 하는 구간이 길어지면 $P$는? (7.5.2·7.5.3)
Q/R 튜닝. 오도메트리를 더 신뢰(작은 Q)하거나 GPS를 더 신뢰(작은 R)하면 융합이 각각 어떻게 변하나? 바이어스가 큰데 Q를 너무 작게 설정하면? (7.5.5)
방위 융합. 이 예제에 compass 방위 측정을 추가한다면, 혁신 계산에서 무엇을 조심해야 하나? 왜? (5.3.7)
robot_localization. ekf_node에서 *_config가 하는 일은? 오도메트리의 속도와 GPS의 위치를 융합하도록 설정하려면 무엇을 켜나? (7.5.4)
(손으로) verify/ce34_ekf_fusion.py로 융합을 재현하고, 바이어스·잡음·GPS 주기를 바꿔 가며 추측항법 대비 개선 배수를 관찰하라. 가능하면 robot_localization으로 실/시뮬레이션 데이터 융합까지. (검증된 스크립트)

막히면 "오도메트리=단기 매끄러움(드리프트), GPS=장기 절대(느림), EKF가 둘을 융합해 유계 오차, 실전은 robot_localization"으로 돌아가라.