27.5.1 센서 퓨전 공통 로직

PX4-Autopilot의 확장 칼만 필터(Extended Kalman Filter, EKF) 구현체인 ECL(Estimation and Control Library) 아키텍처에서 센서 데이터를 융합하는 과정은 고도로 모듈화된 파이프라인(Pipeline)을 따른다. 비전 센서, GPS, 기압계 등 센서의 물리적 특성이나 측정 단위(Unit)는 서로 완전히 다르지만, 상태 추정(State Estimation)이라는 목적 아래 수학적으로 통합되기 위해 동일한 추상화 로직을 거치게 된다. 본 절에서는 센서의 종류와 무관하게 모든 관측 모델이 공통적으로 상속하거나 호출하는 센서 퓨전 공통 로직을 분석한다. 참고로 본 분석은 PX4 펌웨어 버전 v1.14 이상의 최신 ECL/EKF2 구조를 기준으로 한다.

1. 센서 데이터 처리 파이프라인의 개요

PX4 ECL 아키텍처는 코드의 재사용성을 높이고 필터의 연산 복잡도를 낮추기 위해 센서별 퓨전 로직을 공통된 단계로 추상화하였다. 이 공통 로직은 크게 데이터 동기화 및 버퍼링, 예상 관측치(Predicted Measurement) 계산, 잔차(Innovation) 및 잔차 공분산(Innovation Covariance) 연산, 그리고 최종적인 상태 보정(State Correction) 이라는 4단계로 구성된다.

flowchart TD
    A[센서 원시 데이터 수신] --> B(관측 버퍼 저장 및 지연 오차 보상)
    B --> C{혁신 검사: Innovation Test}
    C -->|통과| D(예상 관측치 및 야코비안 행렬 계산)
    C -->|실패| E[데이터 기각: Gated/Rejected]
    D --> F(칼만 게인 계산)
    F --> G[24차원 상태 벡터 및 공분산 업데이트]

이러한 공통 파이프라인은 ecl/EKF/ekf.cpp 내의 주 제어 루프를 통해 호출되며, 개별 센서 퓨전 함수(예: fuseGps(), fuseBaro())의 뼈대를 형성한다.

2. 관측 모델(Observation Model)과 야코비안(Jacobian) 연산

센서 퓨전의 수학적 핵심은 EKF의 비선형 관측 방정식 $h(\mathbf{x}_k)$ 을 정의하고, 이를 선형화한 야코비안 행렬 $\mathbf{H}_k$ 를 구하는 것이다. PX4-Autopilot은 이 연산을 효율적으로 수행하기 위해 내부적으로 심볼릭 연산 기반의 자동 생성 코드(Auto-generated Code)를 사용한다.

2.1 관측 행렬( $\mathbf{H}$ )의 블록 구조 구성

$24 \times 1$ 차원의 상태 벡터를 사용하는 PX4 EKF2 모델에서 이상적인 $\mathbf{H}$ 행렬 크기는 (관측 차원) $\times 24$ 가 된다. 하지만 이를 그대로 행렬 연산할 경우 임베디드 플랫폼(예: STM32 기반의 Pixhawk 하드웨어)의 부족한 부동소수점 연산 능력을 급격히 저하시킨다. 따라서 코드 레벨에서는 다음과 같은 희소 행렬(Sparse Matrix) 연산 기법이 적용되어 있다.

상태 인덱스 분리: 현재 관측되는 센서 데이터가 영향을 미치는 특정 상태 변수들(예: GPS의 경우 벡터 중 속도 및 위치 인덱스 번호인 4~9번)만을 추출하여 소규모 배열 연산으로 대체한다.
0이 아닌 값만 연산: $\mathbf{H}$ 행렬 내의 대다수를 차지하는 0에 대해서는 초기 연산 자체를 생략한다. 코드 상에서는 H_LOS 등 특정 센서 타입에 매핑되는 작은 변수 배열만을 취급한다.

3. 공통 잔차 및 공분산 계산 구조

공통 로직에서 가장 중요한 단계는 혁신 검사(Innovation Test, 흔히 Innovation Gating이라고도 함)를 수행하기 전, 임시로 혁신 공분산 행렬( $\mathbf{S}$ )을 계산하는 것이다. 이 과정은 $S = H P H^T + R$ 로 표현되는 표준 EKF 수식에 근거한다.

// PX4 관측 공분산 계산의 의사 코드(Pseudo Code)
float innovation = measured_value - predicted_value;
float innovation_variance = H * P * transpose(H) + R_variance; // P는 상태 공분산, R은 센서 노이즈

3.1 센서 노이즈 파라미터( $\mathbf{R}$ ) 동적 반영

여기서 $\mathbf{R}$ 에 해당하는 센서 측정 노이즈(Measurement Noise) 값은 고정된 상수가 아니다. QGroundControl(QGC) 등에서 사용자가 파라미터(예: EKF2_BARO_NOISE, EKF2_GPS_V_NOISE)를 조정함에 따라 동적으로 할당되며, 비행 중 센서의 건강 상태(Health Status)에 따라 가변적으로 페널티가 부여되기도 한다. 공통 로직 모듈은 각 센서 퓨전 함수가 innovation_variance를 계산할 때 반드시 최신 상태의 파라미터 구조체를 룩업(Lookup)하도록 강제한다.

4. 융합의 실행과 칼만 게인(Kalman Gain) 적용

혁신 검사를 통과한 유효한 데이터는 최종적으로 시퀀셜 업데이트(Sequential Update) 방식으로 상태 벡터에 반영된다. 즉, 3차원 관측 데이터 블록 전체를 한 번에 역행렬 연산하여 융합하는 것이 아니라, 스칼라(Scalar) 단위로 분할하여 1차원 업데이트 로직을 3번 반복하는 방식으로 다변수 센서 퓨전을 수행한다.

이는 임베디드 펌웨어에서 행렬 역연산 연산(Matrix Inversion) 알고리즘으로 인한 메모리 접근 및 CPU 사이클 낭비를 제거하는 PX4의 전통적인 최적화 철학이다. 대표적으로, $\mathbf{S}$ 행렬의 역행렬을 구하는 과정은 단순히 배열의 요소들을 나누기 연산(/)으로 대체하여 스칼라 나눗셈으로 구현된다. 결과적으로 다음과 같은 상태 방정식 업데이트가 공통 루틴 내에서 고속으로 이루어진다.

$\mathbf{x}_{k} = \mathbf{x}_{k|k-1} + \mathbf{K}_k \mathbf{y}_k$

(여기서 $\mathbf{y}_k$ 는 Innovation(잔차), $\mathbf{K}_k$ 는 Kalman Gain 행렬이다.)

4.1 상태 공분산 대칭화 방어 로직

공통 업데이트 루틴의 대단원은 공분산 행렬 $\mathbf{P}$ 의 축소 연산이다. 반복적인 곱셈과 덧셈 연산에서 부동소수점 오차(Floating-point Round-off Error)가 누적되어 공분산 행렬의 양의 정부호(Positive Definite) 특성이 깨지고 비대칭이 되는 것을 막기 위해, PX4 공통 로직은 매 업데이트마다 대각 성분을 기준으로 행렬을 강제로 대칭화(Symmetrization)하고 특이점(Singularity) 발생을 방지하는 정규화(Normalization) 코드를 실행한다.

이러한 고도로 추상화되고 최적화된 공통 로직을 기반으로, 하위의 GPS, 기압계, 지자계 센서 퓨전 모듈들(예: fuseVelPosHeight, fuseMag)은 각자에게 맞는 $\mathbf{H}$ 행렬 원소만을 대입하는 것으로 복잡한 EKF 업데이트를 안전하게 완료할 수 있다. 이어지는 하위 절들에서는 개별 센서들이 이 템플릿 환경 위에서 어떤 물리적 특성을 가지고 관측 방정식을 변형하는지 심층적으로 살펴본다.