27.5.1.1. 퓨전 템플릿 함수(`fuse`) 호출 패턴: 관측 행렬 구성부터 업데이트까지의 파이프라인

PX4의 확장 칼만 필터(EKF2) 아키텍처 내부, 특히 src/lib/ecl/EKF 디렉토리의 핵심을 들여다보면, 센서 퓨전 과정이 난잡한 스파게티 코드가 아니라 매우 정형화된 템플릿 함수 패턴을 따르고 있음을 알 수 있다. 이 패턴의 중심에는 fuse()라는 이름의 템플릿(Template) 혹은 래퍼(Wrapper) 함수가 존재한다. 본 절에서는 센서 데이터가 수신된 직후부터 실제 EKF 상태(State)가 업데이트되기까지 펌웨어 내부에서 어떠한 C++ 호출 파이프라인을 거치는지, 그리고 코드 구조가 수학의 관측 행렬 구성을 어떻게 지시하는지 상세히 분석한다.

1. 개별 센서 퓨전 함수의 엔트리 포인트 (Entry Point)

각 센서 모듈(예: GPS, 기압계 등)은 자신만의 고유한 퓨전 진입점 함수(Entry Point Function)를 가진다. 예를 들어 GPS의 경우에는 fuseVelPosHeight(), 기압계의 경우에는 fuseBaro(), 지자기 센서의 경우 fuseMag()가 호출된다.

이 진입점 함수들의 역할은 센서의 물리적 특성을 수학적으로 모델링하여 **관측 행렬의 0이 아닌 요소(Non-zero elements of H matrix)**와 **예상 관측치(Predicted Measurement)**를 계산하는 것이다.

// src/lib/ecl/EKF/gps_fusion.cpp 의 의사코드 예시
void Ekf::fuseVelPosHeight()
{
    // 1. 센서 버퍼에서 지연이 보상된 데이터 가져오기
    const float dt = _dt_ekf_avg;
    Vector3f gps_vel = _gps_sample_delayed.vel;

    // 2. 상태 벡터(State Vector) 기반 예상 관측치(Predicted Measurement) 연산
    Vector3f vel_pred = _state.vel; // 속도 예상치 설정

    // 3. 야코비안 행렬(H)의 비영 원소 연산 (희소 행렬 최적화)
    // GPS 측정치는 상태 4~6번(속도)에 1:1로 매핑되므로 H 요소는 1 또는 간단한 오프셋이 됨
    float H_VEL[3] = {1.0f, 1.0f, 1.0f}; 

    // 4. 개별 축(X, Y, Z)에 대하여 템플릿 함수 fuse() 반복 호출
    for (uint8_t i = 0; i < 3; i++) {
        fuse(gps_vel(i), vel_pred(i), H_VEL[i], _gps_vel_vnc, i); 
    }
}

위의 예시에서 볼 수 있듯이, 진입점 함수는 거대한 24차원의 행렬을 직접 다루지 않고, 자신이 영향을 미치는 상태 인덱스와 그 값만을 선별하여 하위 계층인 fuse() 계열 함수로 넘긴다.

2. 템플릿 함수 `fuse()`의 구조와 파이프라인

PX4 ECL에서 각 센서 모듈이 호출하는 추상화된 fuse() 함수의 내부 파이프라인은 수학 교과서의 EKF 관측 업데이트(Observation Update) 공식을 C++의 절차적 패턴으로 고스란히 옮겨 놓은 것이다.

sequenceDiagram
    participant Sensor as Entry Function (e.g. fuseBaro)
    participant Fuse as Common fuse() logic
    participant Test as Innovation Test
    participant State as State Vector & Covariance

    Sensor->>Fuse: 1. 관측값, 예상값, H 성분, 노이즈 분산(R) 전달
    activate Fuse
    Fuse->>Fuse: 2. 잔차(Innovation) 계산: y = z - h(x)
    Fuse->>Fuse: 3. 잔차 공분산(S) 계산: S = H*P*H^T + R
    Fuse->>Test: 4. 혁신 검사 (Innovation Gating)
    activate Test
    Test-->>Fuse: 통과(Passed) / 거부(Rejected) 반환
    deactivate Test
    alt 혁신 검사 통과 시
        Fuse->>State: 5. 칼만 게인(K) 계산: K = P*H^T / S
        Fuse->>State: 6. 상태 벡터(x) 업데이트: x = x + K*y
        Fuse->>State: 7. 공분산 행렬(P) 축소 연산: P = P - K*H*P
    else 혁신 검사 거부 시
        Fuse->>Sensor: 업데이트 취소 및 에러 상태(Telemetry) 보고
    end
    deactivate Fuse

이 패턴의 강력함은 코드 중복의 제거와 안전성의 일괄 통제에 있다. fuse() 함수는 모든 센서에 대해 동일한 메모리 안전장치와 부동소수점 오차 방어 로직(예: 0으로 나누기 방지(Divide-by-zero Protection), NaN 감지)을 강제한다.

2.1 스칼라 업데이트(Scalar Update) 기법 적용

수학적으로 EKF의 업데이트 단계는 측정이 벡터형인 경우 행렬 곱셈과 다차원 역행렬 연산을 수반한다. 하지만 PX4의 fuse() 계열 함수들은 다차원 행렬을 다루지 않는다. 입력되는 관측 데이터를 모두 스칼라 단위(요소별)로 분리하여 여러 번의 1차원 업데이트 루틴으로 쪼개서 실행한다. 이를 통해 가장 컴퓨팅 연산 배수가 큰 S_inverse(잔차 공분산의 역행렬) 연산을 단순한 정수형 나눗셈 1개로 변환함으로써, 임베디드 하드웨어의 CPU 로드를 획기적으로 낮추는 “스칼라 업데이트(Scalar Update)” 기법을 완성한다.

3. 공분산 대각 결합(Diagonal Covariance Symmetrization)

fuse() 루틴의 마지막을 장식하는 것은 공분산 행렬( $\mathbf{P}$ )의 재계산이다. 퓨전이 성공하면 필터는 정보를 얻은 것이므로 불확실성이 감소하며 $\mathbf{P}$ 가 작아져야 한다.

$\mathbf{P}_{k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}$

PX4는 이 과정인 제너럴 조셉 폼(Joseph Form) 연산을 그대로 수행하는 대신, 최적화된 심볼릭 스칼라 전개 방식을 사용하여 루프를 돈다. 이 연산 루틴이 끝난 직후, 공통 라이브러리는 강제로 $\mathbf{P}$ 행렬의 수치적 특성을 복원하는 함수를 호출한다. 비대칭 오차가 발생할 소지가 조금이라도 감지되면 P(i, j) = P(j, i) = 0.5f * (P(i, j) + P(j, i)) 코드를 실행하여 행렬의 양의 정부호(Positive Definite) 특성을 임의로 강제한다.

4. 소결

PX4의 센서 퓨전 파이프라인에서 하위 모듈들은 오직 관측 모델에 기반한 복잡한 기하학적 야코비안 연산에만 집중한다. 이렇게 준비된 행렬 퍼즐 조각들을 fuse()라는 단일화된 엔진에 던져 넣으면, 이후의 필터링 수학 연산부터 게이팅, 상태 업데이트, 메모리 보정은 템플릿 계층이 알아서 처리하는 우아한 패턴을 지니고 있다.

다음 절에서는 이 파이프라인 로직에서 센서 퓨전의 ‘문지기’ 역할을 하는, 혁신 검사(Innovation Test)의 통계적 기법과 코드 구조를 단독으로 조명한다.

27.5.1.1. 퓨전 템플릿 함수(fuse) 호출 패턴: 관측 행렬 구성부터 업데이트까지의 파이프라인