제5부 상태 추정: 칼만필터

5.4 센서 융합 (IMU+GPS+Compass)

제5부 상태 추정: 칼만필터 · 마무리. 어느 센서도 단독으로는 부족하다 — IMU는 빠르나 드리프트하고, GPS는 절대적이나 느리고, 지자기는 방위를 주나 외란에 약하다. 상보적인 센서를 융합해 자세·위치를 추정한다. 직관적인 상보 필터에서 시작해, 자세 추정의 표준 ESKF(오차상태 칼만)까지. 그리고 ROS2 robot_localization(7.5)으로 가는 다리.

⚠️ 범위 정직 경고: 관성항법(INS)·자세추정은 그 자체로 두꺼운 책 한 권인 큰 분야다. 이 장은 개념·직관과 진입점을 주는 입문이며, 완전한 쿼터니언 운동학·좌표변환 유도는 전문 문헌의 몫이다. 부호·프레임 규약은 구현마다 달라 반드시 해당 라이브러리·문헌으로 검증하라.

이 장을 마치면 상보적 센서를 융합해 자세·위치를 추정하고, 상보 필터와 ESKF의 발상을 구분하며, 실전 융합의 함정(프레임·동기·각도 wrapping)을 인지해야 한다.

5.4.1 왜 융합 — 완벽한 센서는 없다

항법에 쓰는 센서들은 서로의 약점을 보완한다(상보적, complementary).

핵심 그림: IMU = 단기 진실(빠르나 흐른다), GPS·Compass = 장기 진실(느리나 절대적). 융합은 단기에는 IMU, 장기에는 절대 센서를 신뢰해 둘의 장점만 취한다. 이것이 모든 센서 융합의 직관이다.

5.4.2 상보 필터 — 저비용 융합

칼만 전에, 같은 직관의 가장 단순한 구현을 보자. 1축 기울기를 자이로(고속·드리프트)와 가속도계(절대·잡음)로 추정:

# complementary.py — 자이로 + 가속도 상보 필터 (1축 기울기)
alpha, dt = 0.98, 0.01           # alpha≈1 (자이로를 단기 신뢰)
theta = 0.0
def update(gyro_rate, accel_angle):       # gyro_rate[rad/s], accel_angle[rad]
    global theta
    theta = alpha*(theta + gyro_rate*dt) + (1 - alpha)*accel_angle
    return theta

• alpha·(theta + gyro·dt): 자이로를 적분해 고주파(빠른 변화)를 따른다 — 고역통과.
• (1−alpha)·accel_angle: 가속도계의 저주파(평균 기울기)로 드리프트를 억제한다 — 저역통과.
• alpha(시정수)가 둘의 교차주파수를 정한다(2.3의 주파수 관점!).

상보 필터는 칼만의 특수·정적 버전이다 — alpha를 손으로 고정한 것. 칼만은 이 가중(alpha↔$K$)을 불확실성에 따라 자동·최적으로 정한다(5.2.5). 직관을 얻기에는 상보 필터가 가장 적합하고, 성능·다센서에는 칼만.

5.4.3 항법 상태와 자세 표현

본격 융합의 상태(전형적 9~15차원):

• 위치 $p$, 속도 $v$ (각 3차원)
• 자세(attitude) — 방향. 표현이 까다롭다(아래)
• IMU 바이어스 — 자이로 바이어스 b_g, 가속도 바이어스 b_a (드리프트의 원인, 추정해서 빼야 한다)

자세 표현 3가지:

• 오일러각(roll·pitch·yaw): 직관적이나 짐벌락(gimbal lock) — 특정 자세에서 자유도가 붕괴. 항법에는 부적합.
• 회전행렬(3×3): 특이점 없으나 9개 원소·6개 제약으로 표현 비용이 크다.
• 쿼터니언(quaternion, 4원소): 특이점 없고 콤팩트해 표준. 단 단위 노름 제약(‖q‖=1)이 있어 3자유도를 4수로 표현 → 별도 처리 없이 칼만에 넣으면 공분산이 특이해진다(다음 절).

5.4.4 왜 ESKF — 자세는 다양체에 속한다

문제: 자세(회전)는 벡터공간이 아니라 곡면(다양체 SO(3)) 위에 정의된다. 쿼터니언은 4수지만 자유도는 3(+단위 제약)이라, 쿼터니언을 직접 칼만 상태로 두면 공분산 행렬이 특이(singular)해진다(제약 방향에는 불확실성이 없으므로).

오차상태 칼만필터(ESKF, Error-State KF)의 해법:

• 공칭 상태(nominal): 자세를 쿼터니언으로 그대로 유지한다(제약 만족).
• 오차 상태(error state): 자세 오차를 작은 회전 = 3차원 벡터(접공간, tangent space)로 둔다. 3자유도 = 3수라 최소 표현, 공분산이 특이하지 않다.

이점:

• 특이점 없음·최소 표현(자세 오차 3벡터).
• 선형화가 항상 0 근처에서 일어난다(오차가 작으므로) → EKF 자코비안 근사가 훨씬 정확(5.3.2의 약점 완화).
• 짐벌락·노름 제약 문제 회피.

그래서 드론·INS의 자세추정은 거의 ESKF(또는 MEKF, 곱셈형 EKF)를 쓴다. "쿼터니언은 공칭으로, 오차는 3벡터로 칼만" — 이 한 줄이 ESKF의 핵심.

5.4.5 ESKF 구조 (개념)

흐름:

1. 공칭 전파: IMU를 고속으로 적분해 $p,v,q$의 큰 궤적을 만든다(잡음 무시).
2. 오차 예측: 오차상태의 공분산 $P$를 IMU 잡음 모델로 전파(KF 예측, 5.2.3).
3. 측정 갱신: GPS(위치)·Compass(방위)가 오면, 측정과 공칭의 차이로 오차상태를 추정(KF 갱신).
4. 주입·리셋: 추정된 오차를 공칭에 더해 보정하고(특히 자세는 작은 회전을 쿼터니언에 곱함), 오차상태를 다시 0으로 리셋.

칼만의 예측·갱신(5.2)을 오차상태에 적용한 것일 뿐 — 틀은 같다.

5.4.6 IMU 메커니제이션과 바이어스

공칭 전파(스트랩다운 적분)의 골자:

• 자이로 → 자세: 각속도(− 바이어스)를 적분해 쿼터니언을 갱신.
• 가속도 → 속도 → 위치: 측정 비력에서 중력을 빼고(자세로 회전시켜 항법 좌표계로) 적분해 속도·위치.
• 바이어스 추정: 자이로·가속도 바이어스는 천천히 변하며 드리프트의 주요 원인 → 상태에 넣어 추정하고 빼야 한다. GPS·Compass가 이 바이어스를 간접적으로 관측 가능하게 만든다(가관측, 4.2).

바이어스를 추정 안 하면 IMU 적분이 빠르게 발산한다 — 융합의 핵심 가치가 바이어스를 절대 센서로 보정해 주는 것이다.

5.4.7 실전 — 프레임·동기·이상치

센서 융합 실패의 대부분은 알고리즘이 아니라 기하·타이밍에서 온다.

• 좌표 프레임: body(센서)·ENU/NED(지역 항법)·ECEF(지구중심). GPS는 위경도→지역 좌표 변환 필요. 프레임·부호 규약 혼동이 1번 버그.
• 시간 동기: IMU(고속)·GPS(저속)·Compass(중속)의 타임스탬프를 맞춰야 한다. 동기 어긋남은 곧 추정 오차.
• 중력·레버암: 중력 정확히 제거, 센서들이 한 점에 없으면(레버암) 보정.
• 이상치(outlier): GPS 멀티패스·점프는 카이제곱 게이팅(혁신 $S$ 기반)으로 걸러야(5.3.7 일관성).
• 지자기 보정: 하드/소프트아이언 캘리브레이션, 자기 외란 회피.

5.4.7.1 GPS 끊김 대응 — 추측항법·재획득 게이팅

5.4.1의 표는 GPS의 약점으로 "끊김"을 적었고 5.4.9는 이를 "실전에서 계속 대응해야 하는 대상"이라 인정만 하고 넘어갔다 — 여기서 최소한의 구체적 대응을 정리한다. 끊김 중에는 필터를 멈추지 않고 예측만 수행한다(추측항법): GPS 갱신이 안 들어오면 5.2.3의 예측 단계($\hat{x}^-=f(\hat{x},u), P^-=FPF^T+Q$)만 IMU·오도메트리로 계속 반복하고, 갱신이 없으니 $P$는 자연히 커지게 둔다 — 이것은 버그가 아니라 5.2.8에서 이미 다룬 "측정 없이 예측만 오래 하면 $P$가 계속 커진다"가 정상적으로 일어나는 것이며, 커진 $P$가 "지금 얼마나 신뢰할 수 없는지"를 정직하게 반영한다. 재획득 직후 첫 몇 번의 측정은 평소보다 엄격히 게이트한다 — 끊긴 동안 추측항법 드리프트가 쌓였을 수도 있고, 막 다시 포착된 GPS 자체가 초기에는(콜드/웜스타트, 멀티패스 잔재) 부정확할 수도 있어서다. 평소의 카이제곱 임계값(5.3.7)을 그대로 쓰면 드리프트된 필터가 정상 GPS를 이상치로 오인하거나, 반대로 여전히 품질이 낮은 GPS를 받아들여 튈 위험이 있다 — 재획득 후 $N$번(또는 $T$초) 동안은 임계값을 좁히거나 $R$을 일시적으로 키워 서서히 신뢰를 회복시키고, 연속으로 게이트를 통과하면 평소 $R$로 복귀한다. "마지막 정상 수신 이후 경과 시간"을 카운터로 둔다: 이 값이 임계값(환경·요구정밀도에 따라 정함)을 넘으면 추측항법만으로 유지한 시간이 너무 길어 $P$가 실용적으로 못 쓸 만큼 커졌다는 뜻이므로, 계속 추측항법만 수행하기보다 결함으로 플래그해 상위 시스템에 알려야 한다 — 이 감지·대응 구조는 7.9(안전과 결함 처리)의 "정상→저하→안전" 패턴과 같다.

5.4.8 ROS2로 — robot_localization (7.5 예고)

제7부에서 직접 쓸 표준 패키지:

• robot_localization: ekf_node·ukf_node가 오도메트리+IMU+GPS를 융합(EKF/UKF). 여러 입력의 어느 필드를 쓸지 설정으로 지정.
• navsat_transform_node: GPS(위경도)를 지역 좌표(map/odom)로 변환해 융합에 투입.
• 표준 tf(좌표변환)·메시지(/odom,/imu,/gps/fix)로 묶인다.

즉 ESKF의 원리를 알고, 실전은 검증된 패키지를 올바른 프레임·설정으로 쓰는 것이 권장 경로다(7.5에서 실습).

5.4.9 정직 — 이 장은 입문, INS는 거대하다

• 이 장은 진입점이다: 완전한 쿼터니언 운동학·ESKF 자코비안·좌표변환은 더 깊은 문헌이 필요하다. 여기 식들은 개념이며 구현 전 검증 필수.
• 규약이 걸림돌이 된다: 프레임(ENU vs NED)·쿼터니언 순서(wxyz vs xyzw)·회전 방향 등 규약 불일치가 가장 흔한 실패. 라이브러리 문서를 그대로 따르라.
• 바이어스·외란은 지속적이다: IMU 바이어스 드리프트, 자기 외란, GPS 끊김은 실전에서 계속 대응해야 하는 대상.
• 검증된 패키지를 써라: 직접 ESKF를 작성하기보다 robot_localization·mavros·INS 라이브러리를 올바르게 설정하는 것이 실용적이며 안전하다(직접 구현은 학습용으로).

5.4.10 정리 — 제5부 마무리

• 센서 융합은 상보적 센서(IMU 단기·GPS/Compass 장기)를 합쳐 자세·위치를 추정한다(5.4.1). 직관은 상보 필터(고역 IMU + 저역 절대), 최적·자동 가중은 칼만(5.4.2).
• 자세는 다양체에 살아 쿼터니언을 직접 필터하면 특이 → ESKF가 공칭(쿼터니언)·오차(3벡터)를 분리해 푼다(5.4.4·5.4.5). 바이어스 추정이 드리프트를 억제하는 핵심 요인(5.4.6).
• 실전 성패는 프레임·시간동기·이상치·규약(5.4.7·5.4.9). ROS2 robot_localization이 표준 도구(5.4.8, 실습 7.5).

이로써 제5부(상태 추정: 칼만필터)를 마친다. 확률(5.1)·KF(5.2)·EKF/UKF(5.3)·센서융합(5.4)으로, 잡음 속에서 상태를 추정하는 길을 완성했다 — 제어(제2·4부)와 추정(제5부)이 LQG로 만난다. 다음 제6부(6.1~)는 심화 주제 — MPC·비선형·강인·적응 제어로, 표준 기법의 한계를 넘는 도구들을 개념·진입점 위주로 다룬다.

5.4.11 연습·생각해 볼 것

1. 상보성 매칭. IMU·GPS·Compass 각각이 주는 것과 못 주는 것을 정리하고, 왜 셋을 합쳐야 위치·속도·자세를 다 얻는지 설명하라. (5.4.1)
2. 상보 필터. complementary.py에서 alpha를 0.98→0.6으로 낮추면 자이로·가속도 신뢰가 어떻게 바뀌나? 진동이 심한 환경에서는 어느 쪽이 좋은가? alpha와 교차주파수(2.3)의 관계는? (5.4.2)
3. 자세 표현. 오일러각의 짐벌락이 왜 문제이며, 쿼터니언이 왜 표준인지, 쿼터니언의 단점(제약)은 무엇인지 설명하라. (5.4.3)
4. ESKF 필요성. 쿼터니언(4수, 3자유도)을 그대로 칼만 상태로 넣으면 공분산에 무슨 문제가 생기나? ESKF가 오차를 3벡터로 두어 이를 어떻게 피하나? (5.4.4)
5. 바이어스. IMU 자이로 바이어스를 추정하지 않으면 자세 적분에 무슨 일이? GPS·Compass가 어떻게 이 바이어스를 간접적으로 관측해 주나(가관측성, 4.2)? (5.4.6)
6. 실전 버그. 두 팀이 같은 융합 코드를 썼는데 한 팀은 발산했다. 알고리즘이 아니라면 어디서 문제가 생겼을 가능성이 큰가(세 가지)? (5.4.7·5.4.9)
7. (손으로) complementary.py로 합성 자이로(드리프트 추가)+가속도(잡음 추가) 데이터를 만들어 기울기를 추정하라. 자이로만 적분한 것과 비교해 드리프트가 억제되는지, alpha를 바꿔 가며 관찰하라. (여유가 되면 7.5에서 robot_localization으로 실로봇 데이터에 적용.) (검증 필요)

막히면 "단기는 IMU·장기는 절대센서, 가중은 칼만이 자동, 자세는 ESKF(공칭 쿼터니언+오차 3벡터), 실전은 프레임·동기가 걸림돌"로 돌아가라.