제7부 센서·인지 파이프라인

7.6 IMU·오도메트리·robot_localization

7.6.1 이 장에서 배우는 것 / 선수 지식

로봇이 "내가 어디 있는가"를 아는 방법은 하나가 아니다. 바퀴 엔코더는 미끄러지고, IMU는 드리프트하고, GNSS는 실내에서 안 되고, 라이다 측위는 특징이 없으면 실패한다. 각 센서의 약점이 다르므로 융합하면 나아진다.

robot_localization이 ROS 생태계의 표준 답이다. EKF·UKF로 여러 오도메트리 소스를 합쳐 하나의 일관된 자세 추정을 만들고, REP 105의 프레임 구조(5.1.7)에 맞게 tf를 발행한다.

이 장은 필터 이론이 아니라 ROS에서의 구성과 함정에 집중한다. 칼만 필터 수식은 자매서 robotics 제9부(RB-195~RB-241)와 kalmann이 다룬다(집필 상태는 A.5.9).

배우는 것:

• IMU의 실제 특성 — 무엇을 잘하고 무엇을 못 하는가.
• Odometry 메시지의 두 부분 — pose와 twist가 왜 나뉘어 있는가.
• 융합 설계 — 어느 센서에서 무엇을 쓸 것인가.
• robot_localization 구성 — 설정 행렬과 프레임.
• 이중 인스턴스 패턴 — odom용과 map용.
• GNSS 통합 — navsat_transform.
• 검증과 튜닝 — 잘 되고 있는지 아는 법.

선수 지식: 7.1.5(REP 145·IMU 규약), 7.1.7(공분산), 5.1.7(REP 105 프레임), 6.5.6(diff_drive 오도메트리), 5.2(tf 함정).

정직: robot_localization의 Jazzy 지원 상태·파라미터 이름은 확인이 필요하다(확인 필요). 이 패키지는 ROS 1에서 이어져 왔고 파라미터 체계가 방대하다. 아래는 개념과 널리 쓰이는 구성이며, 정확한 파라미터는 패키지 문서가 정본이다. 이 환경에서 검증하지 않았다(목차 3).(→ 157차 정정: 이 장의 핵심 절(7.6.3·7.6.4·7.6.5)에 실행 확인이 추가됐다 — 해당 절 참고. robot_localization 3.8.3이 Jazzy apt로 설치되고, 이 절이 쓰는 파라미터 이름(world_frame·odom0_config·imu0_remove_gravitational_acceleration 등)이 패키지의 실제 템플릿(ekf.yaml·dual_ekf_navsat_example.yaml)과 전부 일치함을 확인했다. 다만 이 장 전체가 검증된 것은 아니다 — 나머지 절은 여전히 미검증이다.)

7.6.2 센서별 특성

융합 설계의 출발점은 각 센서가 무엇을 잘하는지 아는 것이다.

핵심 원칙 셋:

① IMU 가속도계로 위치를 구하지 말라. 가속도를 이중 적분하면 오차가 $t^2$로 누적되어 수 초 안에 수 미터가 틀린다. IMU는 자세(roll·pitch·yaw 변화)에 쓰는 센서다.

$$\varepsilon_{\text{position}} \approx \frac{1}{2} a_{\text{bias}} t^2$$

바이어스가 0.01 m/s²만 있어도 10초 뒤 0.5 m다.

② 자이로와 엔코더는 상보적이다. 엔코더는 병진에 강하고 회전에 약하며(미끄러짐), 자이로는 회전에 강하고 병진을 모른다. 이 둘의 융합이 이동 로봇 오도메트리의 기본 조합이다.

③ 절대와 상대를 구분하라. 엔코더·IMU는 상대(연속적이지만 드리프트), GNSS·라이다 측위는 절대(점프하지만 드리프트 없음). 이것이 REP 105가 odom과 map을 나눈 이유와 정확히 대응한다(5.1.7).

7.6.3 Odometry 메시지의 두 부분

header:
  stamp: {...}
  frame_id: "odom"              # ★ pose의 기준 프레임
child_frame_id: "base_link"     # ★ twist의 기준 프레임
pose:
  pose: {position: {...}, orientation: {...}}
  covariance: [36개]
twist:
  twist: {linear: {...}, angular: {...}}
  covariance: [36개]

두 프레임 필드가 다른 것을 가리킨다:

함정: twist가 몸체 기준이라는 것을 자주 놓친다. 로봇이 앞으로 1 m/s면 twist.linear.x = 1.0이며, 로봇이 어느 방향을 보든 그렇다. 이것을 세계 기준으로 착각하면 융합이 틀린다.

함정: child_frame_id를 비워 두는 드라이버가 있다. 그러면 소비자가 twist의 기준을 알 수 없고, robot_localization이 경고하거나 잘못 해석한다.

공분산 순서는 x, y, z, roll, pitch, yaw(7.1.7). 대각 원소 인덱스는 0, 7, 14, 21, 28, 35.

함정: 공분산을 0으로 두면 융합이 그 센서를 절대 신뢰한다(7.1.7). 메시지 규약은 정반대로 정해져 있다는 점이 함정을 키운다 — Imu.msg는 전부 0을 "공분산 모름"으로 정의한다(7.1.12.2). 규약대로 0을 넣은 발행자와, 0을 분산으로 그대로 쓰는 소비자가 만나면 아무도 틀리지 않았는데 융합 결과가 손상된다. diff_drive_controller(6.5.6) 같은 컨트롤러가 기본값을 채우는지 확인하고, 없으면 파라미터로 설정하라.

실행 확인(157차, Jazzy) — 이 함정을 ekf_node에 직접 재현했다. 정상적인 휠 오도메트리(공분산 0.01, x가 참값 $0.5\times t$ 근처를 정직하게 보고)와, "버그 있는 드라이버"를 흉내낸 소스(공분산 전부 0, x를 참값과 무관한 상수 50.0으로 고정 발행) 두 개를 동시에 odom0·odom1로 넣고 10초간 융합 결과를 관찰했다. 융합된 x는 46.85~47.19 사이에 머물렀다 — 정직한 휠 오도메트리만 있었다면 이 구간에서 나왔을 값(~0~5, 7.6.4 실행 확인의 위치 구성 기준선과 대조)과는 전혀 무관하게, 명백히 틀린 공분산-0 소스 쪽으로 완전히 지배당했다. 다만 값이 즉시·정확히 50.0으로 스냅되지는 않고 46.85→47.19로 서서히 근접했다 — 구현이 공분산 0을 문자 그대로 "무한 확실"로 처리해 특이 행렬을 만들지 않도록 내부적으로 아주 작은 하한을 두는 것으로 추정되며, 정확한 메커니즘까지는 소스로 확인하지 않았다(확인 필요). 그래도 "그 센서만 신뢰한다"는 정성적 주장은 명확히, 강하게 확인됐다. 원시 데이터는 tools/gap_harness/gap157/data/zerocov_result.json. 목차 3.

7.6.4 융합 설계 — 무엇을 쓸 것인가

robot_localization의 핵심은 "각 센서에서 어떤 상태 변수를 받아들일지"를 정하는 행렬이다.

15차원 상태:

$$\mathbf{x} = [x, y, z, \phi, \theta, \psi, \dot{x}, \dot{y}, \dot{z}, \dot{\phi}, \dot{\theta}, \dot{\psi}, \ddot{x}, \ddot{y}, \ddot{z}]$$

각 센서마다 15개 불리언으로 "이 센서에서 이 변수를 쓸 것인가"를 지정한다.

ekf_filter_node:
  ros__parameters:
    frequency: 30.0
    two_d_mode: true              # 평면 로봇이면 z·roll·pitch 고정
    publish_tf: true

    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom             # ★ odom 인스턴스

    # 휠 오도메트리 — 속도만 사용
    odom0: /diff_drive_controller/odom
    odom0_config: [false, false, false,      # x, y, z
                   false, false, false,      # roll, pitch, yaw
                   true,  true,  false,      # vx, vy, vz  ← 사용
                   false, false, true,       # vroll, vpitch, vyaw ← 사용
                   false, false, false]      # ax, ay, az
    odom0_differential: false
    odom0_relative: false
    odom0_queue_size: 10

    # IMU — 자세와 각속도
    imu0: /imu/data
    imu0_config: [false, false, false,
                  false, false, true,        # yaw ← 사용
                  false, false, false,
                  false, false, true,        # vyaw ← 사용
                  false, false, false]
    imu0_differential: false
    imu0_relative: true            # 시작 시점 기준
    imu0_remove_gravitational_acceleration: true   # ★ 중력 제거(7.1.5)
    imu0_queue_size: 10

설계 판단 — 무엇을 쓸 것인가:

견해: "위치가 아니라 속도를 쓰라"가 오도메트리 융합의 핵심 원칙이다. 휠 오도메트리의 위치는 이미 적분된 값이라 오차가 누적되어 있다. 속도를 받아 필터가 직접 적분하면, 필터가 다른 센서와 함께 오차를 관리할 수 있다.

함정: 같은 정보를 두 센서에서 받으면 이중 계산(double counting)이 된다. 휠 오도메트리의 yaw와 IMU의 yaw를 둘 다 위치 성분으로 받으면 필터가 과신한다. 회전은 IMU, 병진은 엔코더로 역할을 나누는 것이 깔끔하다.

differential과 relative:

정직: 이 두 파라미터는 혼동이 잦고 잘못 쓰면 조용히 비정상적인 결과가 나온다(확인 필요: 정확한 의미론)(→ 159차 정정: 소스와 실측으로 정확한 의미론을 규명했다 — 아래 실행 확인 참고). 문서를 확인하고, 먼저 둘 다 false로 시작해 동작을 확인한 뒤 필요할 때만 켜라.

실행 확인(159차, Jazzy) — differential·relative의 정확한 의미론을 소스(ros_filter.cpp)로 규명하고, 4가지 융합 구성 × 2가지 드리프트 모형(상수 오프셋·선형 성장)으로 정량 확인했다. 132~157차에 걸쳐 세 차례 미뤄 온 이 절의 미확인 항목을 닫는다.

• differential: true의 정체 — 위치 측정을 연속 차분해 속도 측정으로 바꿔치기한다. 소스에서 이전 측정과의 tf 델타를 시간차로 나눠 TwistWithCovarianceStamped를 만들고, x,y,z 위치 설정 플래그를 그대로 속도 슬롯 플래그로 복사해 필터에 속도 측정으로 주입한다(첫 메시지는 비교 대상이 없어 버려진다).
• relative: true의 정체 — 이 센서의 "첫 측정값"을 정확히 한 번만 저장해 그 이후 모든 측정에서 빼고, 결과를 여전히 위치 측정으로 사용한다. 즉 "센서를 0점 재조정"하는 1회성 보정이다.
• 동시에 켜면 relative가 무시된다(소스+실측 둘 다 확인) — 조용하지 않다. 파라미터 로딩 시점에 RCLCPP_ERROR_STREAM으로 "Both X_differential and X_relative were set to true. Using differential mode."를 출력하고 relative를 강제로 false로 바꾼다. 실측으로 두 값을 동시에 켠 구성이 differential만 켠 구성과 기울기(0.5000 대 0.5000)·시작값(0.91 대 0.89)이 사실상 같음을 확인해 "differential이 우선한다"는 서술을 검증했다.
• 핵심 발견 — 둘 다 "상수 오프셋"에는 강하지만 "선형으로 자라는 드리프트"에는 무력하다. 참운동은 등속(0.5m/s)으로 고정하고 두 가지 오염 모형(① 상수 오프셋 +5.0, ② 시간에 비례해 자라는 드리프트 $+0.2\times t$)을 각각 입력해 회귀 분석했다.

오염 모형	기본(F/F)	relative	differential
상수 오프셋(+5.0)	그대로 노출(시작값 6.44)	제거됨(시작값 0.94)	제거됨(시작값 0.89)
선형 드리프트(+0.2×t)	기울기 0.6991(100% 유입)	막지 못함(기울기 0.6993)	막지 못함(기울기 0.7000)

상수 오프셋은 relative(첫 값을 빼므로)와 differential(연속 차분에서 상수는 상쇄되므로) 둘 다 제거하지만, 선형 드리프트는 두 파라미터 모두 막지 못한다 — relative는 $t=0$ 시점의 값만 한 번 빼므로 그 이후에 자라는 부분은 고스란히 남고, differential은 위치를 차분해 속도로 바꾸는데 선형 드리프트 자체의 기울기(0.2)가 가짜 속도 편향으로 그대로 새어 들어오기 때문이다. 두 파라미터는 "드리프트 제거기"가 아니라 "관측 시작 시점의 고정 오프셋 제거기"다 — 이것이 정확한 의미론이다.

• 부수 발견 — ukf_node는 기동은 되지만 ekf_node와 같은 최소 설정에서 수치적으로 동등하지 않았다. 136차가 ekf_node에서 명확히 확인한 속도 전용 융합 시나리오를 ukf_node에 그대로 입력하니, vx(트위스트) 필드는 0.5005→0.5000으로 정확했지만 적분된 위치(x)의 기울기는 0.3886으로, 시간이 갈수록 국소 기울기가 더 낮아지는(초기 전환 구간이 아니라 지속적인) 지연을 보였다. 원시 로그에 원인이 그대로 있었다 — "Critical Error, NaNs were detected in the output state of the filter..."가 프로세스가 살아있는 동안(약 56초) 1257회(→ 160차 정정: 저장된 로그 파일 기준 실제로는 1333회 — 카운트 시점과 kill 시점 사이에도 프로세스가 계속 실패하며 로그가 늘었다), 사실상 첫 로그 줄부터 매 갱신 주기마다 출력됐다. process_noise_covariance·initial_estimate_covariance를 명시하지 않은 이 절의 최소 설정 그대로는 ekf_node에선 안정적이었지만 ukf_node에선 지속적으로 NaN 감지·복구 경로를 거치고 있었다 — 정확히 왜 UKF만 이 조건에서 불안정한지(시그마 포인트 공분산의 콜레스키 분해 실패가 유력한 가설이나 소스까지 확인하지 않음)는 열어 둔다(확인 필요)(→ 160차 정정: 후속 조사로 인과관계 자체가 달랐다 — 아래 실행 확인 참고. NaN은 재현이 불안정했고, 지연 현상은 NaN과 무관하게 독립적으로 존재했다).

원시 데이터·계측 소스는 tools/gap_harness/gap159/(data/regression_analysis.txt·const_*_result.json·lin_*_result.json·both_result.json·ukf_result.json, logs/ukf_node.log(NaN 1333회, 저장된 로그 파일 기준)). 목차 3.

실행 확인(160차, Jazzy) — 159차의 "NaN이 지연을 설명한다"는 인과관계를 조사했고, 처음엔 오판했다가 로그 시각을 대조해 스스로 바로잡았다. 같은 시나리오를 신선한 컨테이너에서 여러 차례 재현했는데, 10~17초 길이의 짧은 관찰 창 안에서는 다섯 번 모두 NaN이 0건이어서 한때 "NaN은 재현이 안 된다"고 결론지었다. 그런데 그중 두 번(각각 stdbuf로 로그를 안전하게 받은 경우 포함)을 정리하지 않고 그대로 살려 뒀더니, 이후 별도로 저장된 최종 로그에 NaN이 대량으로 기록돼 있었다 — 로그의 타임스탬프를 프로세스 시작 시각과 맞춰 보니 NaN은 사라진 것이 아니라 단지 30초 안팎 뒤늦게 나타났을 뿐이었다(한 번은 기동 후 33.6초, 다른 한 번은 28.8초 — 둘 다 그 이후로는 죽 NaN이 이어졌다). NaN은 재현이 불안정한 것이 아니라 "지연 발생형"이었다 — 159차(56초 동안 관찰)가 그 지연 이후 구간까지 봤기 때문에 NaN을 검출했을 뿐이다. 반면 위치 적분 지연은 t≈1초부터 이미 나타나 있다 — NaN보다 먼저 시작되므로, NaN이 지연의 원인일 수는 없다(159차의 인과관계 서술은 이 점에서는 틀렸다). 둘은 같은 근본 원인(아래)이 시간을 두고 다르게 드러난 두 증상으로 보는 편이 정확하다.

• 지연의 정체 — 고정 오프셋이 아니라 시간에 따라 매끄럽게 감소하는 국소 성장률이다. t=1~1.5초 구간 국소 기울기 0.4523에서 t=9~9.5초 구간 0.3324까지 단조 감소했다(vx는 그동안 계속 0.5000 근처를 유지). 일회성 초기 전환이 아니라 지속적으로 벌어지는 발산이며, NaN(수십 초 뒤)보다 훨씬 일찍(1초 이내) 시작된다.
• 소스로 확인한 두 가지 수치적 취약점(둘 다 EKF에는 없는, UKF 고유 구조). ① filter_base.cpp의 기본 initial_estimate_covariance는 대각원소 1e-9다 — 바로 옆 주석이 "측정을 빠르게 받아들이도록 큰 값으로 초기화한다"고 말하는 것과 코드가 실제로 하는 일(아주 작은 값을 곱함)이 서로 반대다. ② ukf.cpp의 시그마 포인트 가중치는 lambda_ = alpha²×(STATE_SIZE+kappa) - STATE_SIZE로 계산되는데, 기본값(alpha=0.001, kappa=0, STATE_SIZE=15)을 넣으면 (STATE_SIZE+lambda_) ≈ 0.000015로 극히 작아져 중심 시그마 포인트 가중치가 약 -1,000,000, 나머지 30개가 각각 약 +33,333이라는 극단적인 크기의 가중치가 나온다(합은 정확히 1.0). 이 값들로 상태를 가중평균하는 것과, 같은 0.000015를 공분산에 곱해 검증 없는 콜레스키 분해(estimate_error_covariance_.llt().matrixL(), .info() 확인 없음)를 하는 것 둘 다 부동소수점 정밀도가 시간이 지날수록 누적 손실되기 쉬운 구조로, "즉시 드러나지 않고 약 30초 뒤에 NaN으로 나타난다"는 관찰과 방향이 맞는다.
• 하지만 이 두 가설에서 곧바로 나온 "고치는 법"은 둘 다 실측으로 반증됐다. ① 공식 예제 값(process_noise_covariance·initial_estimate_covariance를 0에 가깝지 않은 값으로 명시)을 넣으니 지연이 오히려 악화됐다(기울기 0.3885→0.2302, 관찰 창 안에서는 NaN 없음 — 다만 이 구성은 지연 발생 여부를 30초 이상 지켜보지는 못했다(확인 필요)). ② 극단적 가중치를 없애는 alpha=1.0(가중치가 0/0.0333으로 정상화됨, 수식으로 확인)으로 바꾸니 지연이 사라지긴커녕 NaN이 기본 설정보다 훨씬 빨리(10초 관찰 창 안에) 나타났다 — 최종 로그 기준 692회, 마지막 샘플의 x·vx가 문자 그대로 nan이었다.
• 정직한 결론 — 정확한 내부 메커니즘은 이번에도 완전히 규명하지 못했다. 다만 ukf_node는 이 절의 최소 설정에서 ekf_node와 수치적으로 동등하지 않으며(지연은 즉시, NaN은 30초 안팎 뒤 반드시 나타난다), 가장 자연스러운 두 가지 수정 시도조차 문제를 고치지 못하고 오히려 다른 방식으로 악화시켰다는 것은 확실하게 실측으로 닫혔다(확인 필요: 정확한 발산 메커니즘). 실무 결론: 이 절의 예제를 ukf_node로 그대로 바꿔 쓰면 안 된다 — 별도의 신중한 튜닝이 필요하다.
• 부수적 방법론 교훈 — kill -9가 리다이렉션된 로그 전체를 유실시킬 수 있고(159차 "1257회"가 실제로는 1333회였던 이유), 반대로 프로세스를 정리하지 않고 두면 "지연 발생형" 실패가 있는지도 모른 채 조기에 결론 내릴 수 있다. 파일로 리다이렉션되면 표준 C 라이브러리가 줄 단위가 아니라 블록 단위로 버퍼링하는데, SIGKILL은 그 버퍼를 플러시할 기회를 주지 않는다(stdbuf -oL -eL로 해결). 동시에 이번 회차 스스로가 "짧게 관찰하고 곧바로 결론 내리는" 실수를 저질렀다가 로그 타임스탬프 대조로 자체 정정했다 — 관찰 창의 길이 자체가 결론을 뒤집을 수 있다는 것이 이번 회차의 가장 일반적인 교훈이다.

원시 데이터·소스 사본은 tools/gap_harness/gap160/(data/regression_analysis.txt·sigma_weight_analysis.txt·nan_timing_analysis.txt(NaN 지연 발생 시각 대조)·baseline2_result.json·explicit_result.json·alpha1_result.json·ekfctrl_result.json, logs/(36개), src_instrumented/(참조용 소스 3종)). 목차 3.

실행 확인(157차, Jazzy) — "위치가 아니라 속도를 쓰라"는 이 절의 핵심 원칙을 회귀 분석으로 정량 확인했다. ekf_node(robot_localization 3.8.3)에 합성 휠 오도메트리를 입력했다 — 실제 운동은 등속 직진(vx=0.5m/s)이고 twist.linear.x는 이 값을 정확히 그대로 보고하지만, pose.position.x에는 인위적으로 드리프트 항(+0.2m/s 상당, 상류 오도메트리 소스 자체의 적분 오차를 흉내)을 더해 오염시켰다. 이 같은 입력을 두 ekf_node(설정만 다름)에 각각 입력하고 융합된 pose.position.x를 10초간 기록해 선형회귀(정착 구간 1초 제외)했다. odom0_config가 위치(x, y)를 쓰는 구성 — 기울기 0.6994m/s(기댓값 0.5+0.2=0.7과 0.09% 오차) — 드리프트가 그대로 새어 들어왔다. odom0_config가 속도(vx, vyaw)를 쓰는 구성 — 기울기 0.5000m/s(기댓값 0.5와 소수점 넷째 자리까지 정확히 일치) — 드리프트가 완전히 걸러졌다. 주입한 오차의 100%가 위치 구성으로 새어 들어오고 0%가 속도 구성으로 새어 들어온 것이며, 이 장에서 가장 강조된 견해가 정량으로 확인된 것이다. 원시 데이터는 tools/gap_harness/gap157/data/position_config_result.json·velocity_config_result.json·regression_analysis.txt. 목차 3.

7.6.5 이중 인스턴스 패턴

REP 105를 따르려면 robot_localization을 두 개 실행한다(5.1.7).

핵심: 두 번째 인스턴스가 map → odom을 발행한다. map → base_link가 아니다 — 그러면 base_link의 부모가 둘이 되어 트리 제약을 위반한다(5.1.4). robot_localization이 이 계산을 자동으로 한다(5.1.7의 식).

# ekf_odom
ekf_odom:
  ros__parameters:
    world_frame: odom
    odom0: /wheel/odom
    imu0: /imu/data
    publish_tf: true            # odom → base_link

# ekf_map
ekf_map:
  ros__parameters:
    world_frame: map
    odom0: /wheel/odom
    imu0: /imu/data
    odom1: /odometry/gps        # navsat_transform 출력
    publish_tf: true            # map → odom

함정: diff_drive_controller의 enable_odom_tf를 꺼야 한다(6.5.6). 켜져 있으면 컨트롤러와 ekf_odom이 둘 다 odom → base_link를 발행해 충돌한다(5.1.4).

diff_drive_controller:
  ros__parameters:
    enable_odom_tf: false      # ★ robot_localization이 담당

견해: 단순한 시스템에서는 인스턴스 하나로 충분하다. GNSS나 전역 측위가 없다면 odom 인스턴스만 실행하고, map → odom은 AMCL·SLAM이 직접 발행한다(8.2·8.3). 이중 인스턴스는 GNSS 야외 로봇이나 여러 절대 소스를 융합할 때 값을 한다.

실행 확인(157차, Jazzy) — "ekf_map은 map → odom을 발행하고 map → base_link는 발행하지 않는다"는 이 절의 핵심 주장을 /tf 원시 메시지로 직접 확인했다. ekf_odom(world_frame: odom, 상대 센서만)과 ekf_map(world_frame: map, 상대 센서 + 절대 소스로 odom1을 추가 — navsat_transform 출력을 흉내낸 합성 절대 위치 퍼블리셔) 두 ekf_node를 동시에 기동해 publish_tf: true로 두고, 8초 동안 /tf에서 실제로 나타난 (frame_id, child_frame_id) 쌍을 전부 기록했다. 결과는 정확히 두 쌍뿐이었다 — map -> odom(161회)과 odom -> base_link(160회). map -> base_link는 단 한 번도 나타나지 않았다. ros2 run tf2_ros tf2_echo map base_link로 두 변환을 이어 붙인 조회도 문제없이 성공해(map→odom→base_link 체인을 통해 이동량이 실측 vx=0.5m/s와 일치하는 값을 반환), base_link의 부모가 정확히 하나(odom)뿐인 온전한 트리임을 확인했다. 원시 데이터는 tools/gap_harness/gap157/data/tf_pairs_result.json. 목차 3.

7.6.6 GNSS 통합

navsat_transform_node가 위경도를 지역 좌표로 바꾼다.

navsat_transform:
  ros__parameters:
    frequency: 30.0
    delay: 3.0
    magnetic_declination_radians: 0.0    # 자기 편각(지역별)
    yaw_offset: 0.0                      # IMU가 자북 기준이면 조정
    zero_altitude: true
    broadcast_utm_transform: false
    publish_filtered_gps: true
    use_odometry_yaw: false
    wait_for_datum: false

입출력:

이 출력을 ekf_map의 입력으로 넣는다.

함정: GNSS 융합에는 방위(heading) 정보가 필요하다. 위치만으로는 로봇이 어느 쪽을 보는지 모른다. IMU 자력계나 이중 안테나 GNSS, 또는 이동 중 궤적에서 추정해야 하며, 이것이 초기 정렬 문제를 만든다.

함정: 자기 편각(magnetic declination)을 설정하지 않으면 방위가 지역에 따라 수 도~수십 도 틀린다. 자력계를 쓴다면 운영 지역의 값을 넣어야 한다.

함정: 자력계는 로봇 자체의 자기장에 오염된다. 모터·배터리·금속 구조물이 영향을 주므로, 로봇에 장착한 상태로 캘리브레이션해야 한다. 그래도 신뢰하기 어려운 경우가 많아, 실외 로봇에서 이중 안테나 GNSS나 RTK를 쓰는 이유가 된다.

실행 확인(158차, Jazzy) — 157차의 합성 스탠드인이 아니라 navsat_transform_node를 실제로 기동해, 이 절의 입출력 표에서 오류 하나와 문서화되지 않은 함정 하나를 찾았고, 좌표 변환·자기 편각·delay 세 가지를 정량 확인했다.

• 오류(문서 정정) — IMU 구독 토픽은 /imu/data가 아니라 /imu다. 소스(navsat_transform.cpp)의 구독 생성 코드가 "imu"(상대 이름)를 그대로 쓴다. 이 절의 표대로 /imu/data에 IMU를 발행하면 아무 에러도 없이 조용히 아무것도 융합되지 않는다 — /odometry/gps 토픽 자체는 생기지만(ros2 topic list에 나타남) 10초 관찰 동안 메시지가 0건이었다(ros2 topic hz·echo --once 모두 무응답으로 직접 확인). ros2 topic list로 /imu(구독자 1·발행자 0)와 /imu/data(발행자 1·구독자 0)가 서로 어긋나 있는 것도 직접 봤다. IMU 발행 토픽을 /imu로만 바꾸니 즉시 해결됐다.
• 문서화되지 않은 함정 — base_link → gps_frame_id tf가 없으면 좌표 변환이 조용히 원점에 고정된다. 위 수정 후에도 /odometry/gps가 계속 (0,0,0)만 냈다 — 소스에 GAP158-DEBUG 계측을 넣어 추적하니, gpsFixCallback이 받는 원시 UTM 좌표는 정상적으로 계속 자라는데(cartesian_y가 매 GPS 메시지마다 증가) 최종 결과가 계속 0으로 나오고 있었다. 원인은 getRobotOriginWorldPose()가 base_link_frame_id_ → gps_frame_id_(GPS 메시지 자신의 frame_id) tf 조회에 실패하면 결과를 setIdentity()로 둔 채 조용히 반환하는 코드였다 — 유일한 단서는 5초에 한 번만 출력되는 ERROR_THROTTLE 로그("Could not obtain base_link -> gps_link transform. Will not remove offset...")뿐이었다. static_transform_publisher로 base_link → gps_link(GPS가 로봇 원점에 있다면 항등 변환이어도 된다)를 발행해야만 실제 좌표가 나온다 — URDF·tf 설정이 없으면, 정상적으로 보이는(올바른 타입·주기의) /odometry/gps가 사실은 영구히 원점에 고정돼 있는 상태로 조용히 운용될 수 있다.
• 좌표 변환 정확도 — 참운동을 회귀로 확인. 로봇이 정북으로 등속(1.0 m/s) 이동하는 합성 GPS(위경도, 111320m/도 근사)를 입력하니, /odometry/gps의 북(y) 성분 기울기는 0.9971m/s(기댓값 1.0과 0.29% 오차), 동(x) 성분 기울기는 0.00000003m/s(기댓값 0, 순수 정북 이동에서 동서 표류 없음)였다.
• 자기 편각 — 정확히 그 각도만큼 좌표계가 회전한다는 것을 수치로 확인. 같은 시나리오에서 magnetic_declination_radians만 0.0 → 0.1745329(10°)로 바꾸니, x 기울기 0.1731(기댓값 $\sin(10^\circ)=0.1736$)·y 기울기 0.9818(기댓값 $\cos(10^\circ)=0.9848$)로 바뀌었고, 여기서 역산한 정북 대비 회전각은 정확히 10.00° — 설정한 편각과 소수점까지 일치했다. "수 도~수십 도 틀린다"는 이 절의 서술이 정확한 숫자로 뒷받침됐다.
• delay 파라미터 — 정확히 지정한 시간만큼 지연 후 시작한다. delay: 3.0(이 절의 예시 값)으로 두니 노드 자신의 로그가 "Delaying for 3 seconds before starting..." → "Delay elapsed. Continuing."을 정확히 3.000초 간격으로 출력했고, 첫 /odometry/gps 메시지는 노드 기동 직후부터 관찰을 시작한 기준으로 t=3.065초에 도착했다(지연 3초 + 약 65ms의 정착 여유).

원시 데이터·계측 소스는 tools/gap_harness/gap158/(data/regression_analysis.txt·decl0_final_result.json·decl10_result.json·delaytest_result.json, src_instrumented/navsat_transform.instrumented.cpp, logs/navsat_node_debug.log). 목차 3.

7.6.7 검증과 튜닝

"융합이 잘 되고 있는가"를 확인하는 방법이다.

① 정지 시험:

# 로봇을 정지시키고 자세가 흐르는지 관찰
ros2 topic echo /odometry/filtered --field pose.pose.position

정지 상태에서 위치가 계속 변하면 드리프트가 있는 것이다. 원인은 IMU 바이어스·잘못된 공분산·가속도계 사용.

② 직진 시험:

10 m를 직진 → 추정 위치가 10 m인가

스케일 오차를 보정한다. 휠 반경 오차(6.5.6)가 여기서 드러난다.

③ 회전 시험:

제자리에서 360도 회전 → 시작 방위로 돌아오는가

회전 스케일과 자이로 스케일 오차를 보정한다. 휠 간격(wheel_separation) 오차도 여기서 보인다.

④ 폐루프 시험:

사각형 경로를 돌아 출발점으로 → 추정 위치가 출발점 근처인가

누적 오차를 종합적으로 본다. 실무에서 가장 유용한 시험이다.

⑤ RViz2 관찰:

견해: ④ 폐루프 시험을 정기적으로 하라. 타이어 마모·짐 무게 변화로 오도메트리 특성이 변하고, 이것이 항법 성능을 서서히 떨어뜨린다. 폐루프 오차를 기록해 두면 열화를 추적할 수 있다(6.9의 예방 정비 논리).

튜닝 순서:

견해: 융합을 켜기 전에 개별 센서가 정확해야 한다. 엉터리 입력을 필터가 고쳐 주지 않는다. "융합했는데 이상하다"의 원인 대부분이 개별 센서 문제이며, 순서를 지키면 원인 분리가 쉽다.

공분산 조정:

정직: robot_localization의 프로세스 노이즈 행렬(process_noise_covariance)은 15×15로 크고 튜닝이 어렵다(확인 필요: 기본값의 적절성). 기본값에서 시작해 문제가 되는 상태 변수의 대각 원소만 조정하는 것이 실용적이다.

7.6.8 진단

융합 노드가 조용히 악화되는 것을 감지해야 한다(6.9).

# robot_localization 진단 (제공 시 — 확인 필요)
ros2 topic echo /diagnostics | grep -A5 ekf

# 입력이 실제로 들어오는가
ros2 topic hz /wheel/odom
ros2 topic hz /imu/data
ros2 topic hz /odometry/filtered

함정: 입력 메시지가 조용히 거부되는 경우가 있다. 스탬프가 너무 오래됐거나(5.2), 공분산이 유효하지 않거나, 프레임이 안 맞으면 필터가 무시한다. 로그 레벨을 높여 확인하라(1.11.4) — "왜 IMU가 반영이 안 되지"의 답이 여기 있는 경우가 많다.

7.6.9 함정 카탈로그

7.6.10 체크포인트

• 주요 센서 일곱 가지의 강약점을 표로 말할 수 있는가.
• IMU 가속도계로 위치를 구하면 안 되는 이유를 식으로 설명할 수 있는가. 바이어스 0.01 m/s²에서 10초 뒤 오차는?
• 자이로와 엔코더가 상보적인 이유는?
• 상대 센서와 절대 센서의 구분이 REP 105와 어떻게 대응하는가.
• Odometry 메시지의 두 프레임 필드가 각각 무엇을 가리키는가. twist의 기준은?
• 융합에서 "위치가 아니라 속도를 쓰라"는 원칙의 근거는?
• 이중 계산이 무엇이며 어떻게 피하는가.
• 이중 인스턴스 패턴에서 각 인스턴스가 무엇을 발행하는가. 두 번째가 map → base_link를 발행하지 않는 이유는?
• diff_drive_controller와 함께 쓸 때 반드시 꺼야 할 설정은?
• GNSS 융합에 방위 정보가 필요한 이유와, 자력계의 두 가지 함정은?
• 검증 시험 네 가지와 각각이 검출하는 오차를 말할 수 있는가. 정기적으로 해야 할 것은?
• 튜닝 순서 여섯 단계와, "융합을 켜기 전에 개별 센서가 정확해야 한다"의 근거는?

다음 장 7.7은 이 센서들을 시간 축에서 정렬하는 문제를 다룬다 — message_filters, 하드웨어 동기, 그리고 센서 지연 예산.