27.5.2 GPS 퓨전 (`fuseVelPosHeight`)

GPS(Global Positioning System) 또는 GNSS 수신기는 실외 환경에서 기체의 절대적인 3차원 위치(위도, 경도, 고도)와 3차원 관성 속도를 제공하는 가장 강력하고 지배적인 센서다. PX4-Autopilot의 확장 칼만 필터(EKF2)에서 이 거대한 시스템의 출력을 기체의 24차원 상태 벡터로 녹여내는 심장부가 바로 Ekf::fuseVelPosHeight() 함수다. 본 절에서는 이 함수가 어떻게 GPS의 속도 및 위치 데이터를 분해하고, 관측 모델을 구성하여 상태 추정의 척추 노릇을 수행하는지 소스 코드 레벨에서 분석한다. 본 분석은 PX4 v1.14 이상의 EKF 아키텍처를 기준으로 한다.

1. `fuseVelPosHeight()` 함수의 진입 및 전제 조건

비행 제어 컴퓨터(Pixhawk 등)가 UART 인터페이스를 통해 GPS u-blox 프로토콜(또는 RTCM 데이터가 포함된 RTK GPS) 패킷을 파싱하면, 이 데이터는 uORB 메시지인 vehicle_gps_position을 통해 EKF 인스턴스로 전달된다.

EKF의 메인 루틴은 이 데이터가 도착했을 때 무작정 융합을 시작하지 않으며, 다음과 같은 철저한 검증(Validation)과 버퍼링 단계를 먼저 거친다.

지연 링 버퍼 동기화: GPS 데이터는 수신 및 처리 과정에서 기압계나 IMU 데이터보다 훨씬 긴 지연 시간(Delay, 통상 100ms ~ 200ms)을 지닌다. fuseVelPosHeight()가 호출되기 전, EKF는 지연 링 버퍼(Ring Buffer) 시스템을 통해 GPS 데이터 타임스탬프와 일치하는 ’과거의 기체 상태’를 복원해 낸다.
센서 건강 상태 및 타임아웃 검증: 위성 수신 개수(Satellites Used), HDOP/VDOP 등의 퀄리티 메트릭이 사용자 지정 임계치(예: EKF2_REQ_SACC, EKF2_GPS_CHECK)를 통과해야만 퓨전 함수가 활성화된다.

2. 관측(Observation) 및 예상 관측(Predicted Measurement) 구성

fuseVelPosHeight() 내부로 진입하면, 가장 먼저 하는 일은 EKF가 현재 추정하고 있는 ’가상의 GPS 센서 측정치(예상 관측치)’를 만드는 것이다. 이 예상치는 실제 GPS 수신 안테나가 뿜어낼 것으로 기대되는 속도와 위치의 3차원 벡터( $6 \times 1$ )다.

2.1 오프셋(Lever Arm) 보상

이 계산이 단순하지 않은 이유는 레버 암(Lever Arm) 효과 때문이다. 기체의 무게 중심(CG, IMU가 위치한 곳)과 GPS 안테나가 부착된 돛대(Mast) 끝은 물리적으로 떨어져 있다. 기체가 빠르게 회전(Yaw, Pitch, Roll)하면 안테나 위치에는 무게 중심과는 다른 선속도가 발생한다.

// src/lib/ecl/EKF/gps_fusion.cpp 의 의사코드
// 1. 상태 변수에서 기체 중심(CG)의 속도 및 위치 획득 (NED 좌표계)
Vector3f vel_pred = _state.vel;
Vector3f pos_pred = _state.pos;

// 2. 레버 암(Lever Arm) 보상: 자이로 각속도(Rates)와 안테나 오프셋(offset)의 외적(Cross Product)
Vector3f vel_offset = cross_product(_state.rates, _gps_pos_offset_body);
Vector3f pos_offset = body_to_earth(_gps_pos_offset_body);

// 3. 최종 예상 관측치 산출
vel_pred += vel_offset;
pos_pred += pos_offset;

3. 관측 행렬( $\mathbf{H}$ )의 정의와 야코비안 계산

GPS 관측 모델은 본질적으로 선형(Linear)에 가깝다. GPS 센서가 뱉어내는 데이터 포맷 자체가 NED(North, East, Down) 좌표계이고, EKF의 상태 벡터 내 4, 5, 6번 인덱스가 NED 속도, 7, 8, 9번 인덱스가 NED 위치를 의미하기 때문이다.

$\mathbf{x} = [q_0, q_1, q_2, q_3, v_N, v_E, v_D, p_N, p_E, p_D, \cdots]^T$

따라서 속도에 대한 $\mathbf{H}$ 행렬(관측이 상태에 미치는 미분)은 속도 상태 변수 인덱스 위치에만 1이 들어가고 나머지가 0인 매우 단순한 형태를 띤다.

3.1 C++ 최적화 배열 할당

ECL 코드는 $24 \times 1$ 크기의 0을 낭비하지 않기 위해, 오직 필요한 파라미터만을 추출하여 템플릿 fuse() 함수로 전달한다.

// 북쪽(North) 속도 퓨전 시 H 행렬 요소 전달
// 상태 인덱스 4번(v_N)에 해당하는 관측 행렬 값은 1.0f 이다.
float H_VEL_N = 1.0f;

단, 레버 암 오프셋을 사용자가 입력했다면, 기체의 회전 각도(Quaternion) 상태(0~3번)에 대해서도 비선형적인 미분이 발생하므로 $\mathbf{H}$ 행렬의 0~3번 인덱스에 복잡한 야코비안 편미분 항들이 자동 생성 코드(Auto-generated Code)에 의해 추가로 삽입된다.

4. `fuse()` 템플릿 함수를 이용한 스칼라 업데이트 루프

예상 관측치와 관측 행렬 성분이 준비되면, fuseVelPosHeight()는 속도 3축과 위치 3축 총 6개의 데이터 차원에 대해 루프를 돌며 공통 로직 모듈인 fuse()를 연달아 호출한다. 이를 순차적 스칼라 업데이트(Sequential Scalar Update) 방식이라 부른다.

flowchart LR
    A[Fuse GPS V_North] --> B[Fuse GPS V_East]
    B --> C[Fuse GPS V_Down]
    C --> D[Fuse GPS Pos_North]
    D --> E[Fuse GPS Pos_East]
    E --> F[Fuse GPS Pos_Down (Height)]

각각의 fuse() 호출 내부에서는 앞 절에서 다룬 혁신 검사(Innovation Test)가 개별적으로 이루어진다.

만약 일시적인 GPS 위성 난반사가 발생해 동쪽(East) 위치 잔차가 너무 커서 게이팅(Gating)에서 거절당하더라도,
북쪽(North) 속도 잔차는 게이팅을 통과해 퓨전될 수 있는 독립적인 구조를 지닌다.

이러한 메커니즘은 전체 GPS 데이터를 통째로 버리지 않고 유효한 정보 축만을 살려 EKF의 붕괴를 지연시키는 뛰어난 아키텍처적 유연성을 보여준다.

5. 소결

PX4의 fuseVelPosHeight() 모듈은 단순히 GPS 데이터를 받아 넘기는 것이 아니다. 수십 밀리초의 시간 지연을 보상하고, 레버 암(Lever Arm)의 기하학적 궤적을 예측하며, 거대한 24차원의 행렬 곱 연산을 6번의 스칼라 나눗셈 루프로 해체하여 임베디드 칩셋의 한계를 극복하는 실시간 컴퓨팅의 정수를 담고 있다.

이러한 GPS 퓨전 파이프라인에서 레버 암 보상의 고도화 및 야코비안 행렬 내 인덱스 매핑의 상세한 수식 구조는 이어지는 하위 절에서 더욱 깊이 있게 파헤친다.

27.5.2 GPS 퓨전 (fuseVelPosHeight)

1. fuseVelPosHeight() 함수의 진입 및 전제 조건