27.3.3. 공분산 예측 소스 코드 분석 (`predictCovariance`)

27.3.3. 공분산 예측 소스 코드 분석 (predictCovariance)

이전 절들에서 다루었던 상태 예측(State Prediction, predictState())은 EKF의 ’육체’에 해당한다. 센서가 이끄는 대로 기체의 현재 위치와 속도를 맹목적으로 따라가는 물리 엔진이기 때문이다.
그러나 진정한 EKF의 ’지능’이자, 칼만 필터가 단순한 저주파 통과 필터(Low-pass Filter)와 궤를 달리하게 만드는 핵심은 바로 상태 공분산 예측(State Covariance Prediction) 모듈에 있다.

본 절에서는 24개 상태 변수들의 상호 신뢰도를 엮어내는 24 \times 24 차원의 거대한 거미줄, 공분산 예측 함수 predictCovariance의 수학적 당위성과 이를 C++ 언어로 최적화해 낸 소프트웨어 공학적 기법을 분석한다.

1. 공분산 행렬(\mathbf{P})의 물리적/철학적 의미

src/lib/ecl/EKF/ekf.cpp 내부를 구동하는 공분산 행렬 \mathbf{P} 는 24 \times 24 크기의 대칭 행렬(Symmetric Matrix)로, 총 576개의 실수(float) 엘리먼트로 구성된다. 이 거대한 배열 덩어리는 기체의 물리적 상태가 아니라, “EKF가 자신이 추정하고 있는 24개의 상태 변수값을 얼마나 의심(Doubt)하고 있는가?” 에 대한 수치적 자기 평가 보고서다.

  • 대각 성분(Diagonal Elements, P_{ii}): i번째 상태 변수(예: 인덱스 0번 쿼터니언 q_0) 스스로에 대한 불확실성(분산). 이 값이 클수록 EKF는 이 변수를 신뢰하지 못한다.
  • 비대각 성분(Off-diagonal Elements, P_{ij}): i번째 변수와 j번째 변수 간의 상관관계. 예를 들어 P_{pos\_North, vel\_North} 값이 크다면, “북쪽 속도 에러가 크면 필연적으로 북쪽 위치 에러도 클 것이다“라는 물리적 연쇄 작용에 대한 통계적 확신을 의미한다.

predictCovariance 함수의 유일한 목적은 타임스텝(\Delta t)이 지날수록, 혹은 시스템 모델 내에 프로세스 노이즈(Process Noise, \mathbf{Q})가 유입될수록 이 불확실성 \mathbf{P} 행렬을 어떻게 ’확장시켜 나갈 것인가’를 계산하는 것이다.

2. 상태 전이 야코비안 행렬(\mathbf{F})과 리카티(Riccati) 미분 방정식

수학적으로, 연속 시스템 공분산(\mathbf{P})의 시간 변화율은 다음과 같은 유명한 리카티 미분 방정식(Riccati Differential Equation) 꼴로 전개된다.

\dot{\mathbf{P}} = \mathbf{F}\mathbf{P} + \mathbf{P}\mathbf{F}^T + \mathbf{Q}

  • \mathbf{F}: 상태 전이 행렬의 야코비안 (Jacobian Matrix, 24 \times 24)
  • \mathbf{P}: 현재 상태 공분산 행렬 (24 \times 24)
  • \mathbf{Q}: 프로세스 노이즈 행렬 (24 \times 24, 주로 자이로/가속도의 잡음 스펙트럼 밀도)

PX4 ECL은 이를 이산 시간(Discrete Time) 시스템으로 영리하게 근사하여, 한 타임스텝 \Delta t 동안 발생한 공분산의 변화량 \Delta \mathbf{P} 를 앞서 구한 야코비안과 테일러 급수를 통해 덧셈 연산으로 쪼개서 적분한다.

문제는 저 어마어마한 \mathbf{F} (야코비안) 행렬의 정체다. 이 행렬은 앞 절에서 다룬 복잡한 비선형 물리 방정식들(쿼터니언 행렬곱, 방향 코사인 행렬 등)을 각각의 24개 상태 변수로 편미분(Partial Derivative) 한 살인적인 상호 작용의 결과물이다.

만약 24 \times 24 행렬의 곱셈 3번(\mathbf{F}\mathbf{P}\mathbf{F}^T)을 반복문을 돌려 24^3 = 13,824 번의 곱셈 연산으로 수행한다면, 저전력 마이크로컨트롤러(MCU)는 그 틱(Tick)을 버티지 못하고 실시간 제어 타이밍을 놓쳐 드론을 추락시킬 것이다.

3. 심볼릭 연산(SymPy)을 통한 희소 행렬(Sparse Matrix) 최적화 코드 자동 생성

PX4 개발팀은 이 컴퓨팅 병목 현상을 타개하기 위해 사람이 직접 C++ 행렬 곱셈 코드를 짜는 것을 포기했다.
대신, ecl/EKF/python 디렉터리에 위치한 파이썬 기반의 수학적 심볼릭 연산 라이브러리인 SymPy 를 활용하여 미분 방정식을 자동으로 풀어내고, 0이 곱해지는 모든 불필요한 연산을 가지치기(Pruning)한 뒤 극한으로 최적화된 C++ 코드를 역으로 자동 생성(Auto-generation) 하는 방식을 채택했다.

그 결과 탄생한 것이 바로 수천 줄에 달하는 predictCovariance 파이프라인이다.
실제 생성된 코드의 단면을 살펴보면, P (기존 공분산 행렬의 복사본 연산자) 배열의 특정 요소들만을 족집게처럼 뽑아내어 곱하고 더하는 기괴하지만 가장 빠른 방식을 띠고 있다.

// 자동 생성된 희소 연산 기반의 공분산 예측 코드 (극히 일부 발췌)

// 1. 공통 부분식(Common Sub-expressions) 사전 추출 및 임시 변수 할당
const float t0 = _state.quat_nominal(0) * _state.quat_nominal(3);
const float t1 = _state.quat_nominal(1) * _state.quat_nominal(2);
const float t2 = 2.0f * (t0 - t1);
// ... 수백 개의 t 변수 선언 ...

// 2. 비선형 야코비안 행렬 연산과 P 행렬 곱셈의 융합 (0 곱셈 완전 배제)
nextP(0, 4) = P(0, 4) + dt * (t2 * P(0, 7) + t5 * P(0, 8) + t6 * P(0, 9));
nextP(0, 5) = P(0, 5) + dt * (t7 * P(0, 7) + t8 * P(0, 8) + t9 * P(0, 9));
// ... 576개의 셀(Cell)에 대한 독립적인 업데이트 수행 ...

// 3. 프로세스 노이즈 행렬(Q)의 대각 성분 직접 주입 (예: 자이로 노이즈)
nextP(0, 0) += dt * gyro_noise_param;
nextP(1, 1) += dt * gyro_noise_param;

이러한 심볼릭 자동 생성 기법이 없었다면 24상태 확장 칼만 필터(EKF2)는 Pixhawk 4(STM32F7) 같은 모바일 프로세서에서 250Hz~400Hz로 동작하는 것이 물리적으로 불가능했을 것이다.

4. 대칭성(Symmetry) 보정과 오버플로우 방어 메커니즘

predictCovariance 연산의 마지막 관문은 소프트웨어적 자기 치유(Self-healing) 로직이다.
공분산 행렬 \mathbf{P} 는 수학의 정의상 무조건 P(i, j) == P(j, i) 인 대칭 행렬이어야 하며, 대각 성분 P(i, i) 는 언제나 0보다 큰 양수(Positive-definite)여야 한다.

하지만 수천 번의 부동소수점(float) 곱셈이 난무하는 자동 생성 코드 속에서, 반올림 오차로 인해 \mathbf{P} 행렬은 미세하게 찌그러지며 대칭성을 잃게 된다. 대각 성분이 음수가 되는 순간, 칼만 필터는 수학적 특이점(Singularity)에 빠져 모든 계산 결과가 NaN (Not a Number)으로 폭발하게 된다.

이를 막기 위해 함수 마지막 부분에서는 행렬의 상삼각(Upper triangular) 성분과 하삼각(Lower triangular) 성분의 평균을 내어 강제로 덮어씌움으로써 대칭성을 복구하고, 대각 성분이 음수로 떨어지지 않도록 하한선(Lower bound) 필터를 적용한다.

// 행렬 대칭성 강제 복구 및 대각선 예외 처리 (방어적 프로그래밍)
for (unsigned row = 0; row < 24; row++) {
    for (unsigned col = 0; col < row; col++) {
        // 상/하 삼각 성분의 평균으로 대칭성 회복
        const float sym_val = 0.5f * (nextP(row, col) + nextP(col, row));
        nextP(row, col) = sym_val;
        nextP(col, row) = sym_val;
    }
    // 대각 성분이 수학적으로 파탄 났는지(음수 등) 검토하여 최소 클리핑
    nextP(row, row) = math::max(nextP(row, row), MIN_VARIANCE_TOLERANCE);
}

이 고문과도 같은 연산 파이프라인을 통과한 nextP 행렬이야말로, 다음번 GPS나 기압계 센서 데이터가 들어올 때 칼만 게인(Kalman Gain)이라는 이름의 재판관으로 변신하여 기체의 운명을 결정지을 핵심 무기가 된다.

지금까지 EKF 코어의 예측 동역학과 공분산 팽창 과정을 분석했다. 다음 절에서는 이 완벽해 보이는 물리 엔진 속에 현실 세계의 센서 데이터가 유입될 때 필연적으로 발생하는 시간축 엇갈림(Time Delay) 현상을 EKF가 어떻게 극복해 내는지 링 버퍼(Ring Buffer) 메커니즘을 통해 파헤쳐 본다.