27.3.3.2. 상태 공분산 배열 업데이트 로직 및 부동소수점 오차에 의한 비대칭성 제어 기법

EKF가 기체의 상태를 예측(predictState)하고 그 불확실성을 팽창(predictCovariance)시키는 과정을 마쳤다면, 이제는 외부 센서 정보(GPS, 비전 등)를 받아들여 불확실성을 축소하고 상태를 교정하는 상태 공분산 업데이트(State Covariance Update) 단계에 돌입한다.

이 업데이트 과정의 핵심은 칼만 게인(Kalman Gain, $\mathbf{K}$ )을 통해 거대한 $24 \times 24$ 공분산 행렬 $\mathbf{P}$ 를 수정하는 것인데, 수천 번 반복되는 부동소수점(float) 뺄셈 연산은 필연적으로 행렬의 필수 수학적 성질인 대칭성(Symmetry) 과 양정치성(Positive-Definiteness) 을 훼손하게 된다.

본 절에서는 융합 수식 자체의 수학적 전개와 함께, PX4 ECL 코드가 부동소수점 연산의 마수(Numeric Instability)로부터 EKF의 뇌(공분산)를 어떻게 보호하는지 분석한다.

1. 조셉 형식(Joseph Form) 공분산 업데이트의 함의

일반적인 문헌에 나오는 확장 칼만 필터의 공분산 업데이트(축소) 공식은 매우 짧고 간단하다.
$\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}$

$\mathbf{I}$ : $24 \times 24$ 단위 행렬 (Identity Matrix)
$\mathbf{K}_k$ : 칼만 게인 벡터/행렬
$\mathbf{H}_k$ : 관측 야코비안 벡터/행렬

하지만 이 기본 형태(Standard Form)는 치명적인 약점을 안고 있다. 행렬 안의 값들을 뺄셈( $\mathbf{I} - \mathbf{K}\mathbf{H}$ )하는 과정에서 정밀도(Precision) 손실이 누적되면, 대칭 행렬이어야 할 $\mathbf{P}$ 행렬의 수치가 어긋나면서 수학적으로 파탄이 나게 된다.

이를 극복하기 위해 PX4 ECL을 비롯한 실무형 항법 시스템은 연산량이 훨씬 많아지더라도 수학적 안정성이 압도 높은 조셉 형식(Joseph Form) 으로 공분산 필터를 업데이트하도록 내부 코딩되어 있다(혹은 그와 대등하게 안정화된 변형식 사용).

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

조셉 형태식은 좌우가 $\mathbf{A}\mathbf{P}\mathbf{A}^T$ 형태의 샌드위치 구조로 감싸져 있어, 어떠한 수치적 반올림 오차(Truncation Error)가 발생해도 결과물인 $\mathbf{P}_{k|k}$ 가 억지로 대칭성(Symmetric) 을 유지하도록 강제하는 위력을 지닌다.

2. 수치적 비대칭성 제어 (Symmetry Enforcement)

조셉 형식을 사용하거나 최적화된 심볼릭 단축식을 사용하더라도, 32비트 싱글 프리시전(float)을 주로 사용하는 STM32 마이크로컨트롤러에서는 시간이 지날수록 $P(i, j)$ 와 $P(j, i)$ 값이 미세하게 어긋나는 현상(Drift)을 피할 수 없다.

만약 $P(10, 5) = 0.00012$ 인데 $P(5, 10) = 0.00018$ 이 되어버린다면, 공분산 고유값(Eigenvalue) 계산이 뒤틀리면서 필터는 $NaN$ (Not a Number)을 내뱉고 장렬히 산화한다. 이를 막기 위해 퓨전 단계(fuse...()) 직후에는 상삼각-하삼각 요소들의 산술 평균을 강제로 집어넣어 대칭성을 복원하는 하드코딩된 패치(Patch) 로직이 반드시 수행된다.

// src/lib/ecl/EKF/common.cpp (혹은 각 퓨전 함수 내부의 후처리 로직)

// 24x24 행렬을 순회하며 비대칭성을 강제로 으깸 (Symmetry Enforcement)
for (unsigned row = 0; row < 24; row++) {
    for (unsigned col = 0; col < row; col++) {
        // [row, col] 과 [col, row] 의 평균치를 구함
        const float val = 0.5f * (P(row, col) + P(col, row));
        
        // 양쪽 대칭 셀에 동일한 평균값을 덮어씌움
        P(row, col) = val;
        P(col, row) = val;
    }
}

이 무식해 보이는 이중 루프 순회 패치가 바로 장기간 비행에도 드론이 추락하지 않게 해주는 생명줄과 같다.

3. 음수 공분산 방어 메커니즘 (Positive-Definite Protection)

비대칭성보다 더욱 치명적인 예외 상황이 있다. 바로 행렬의 대각 성분(Diagonal Elements, $P(i, i)$ )이 음수 값으로 곤두박질치는 현상이다.
$P(i, i)$ 는 $i$ 번째 상태 변수 자기 자신에 대한 분산(Variance, 시그마 제곱 $\sigma^2$ )을 의미하므로, 수학적으로 절대로 $0$ 미만의 음수가 될 수 없다. 음수 분산이라는 것은 수식 체계를 완전히 부정하는 유령 데이터다.

관측 업데이트의 $\mathbf{P}_{new} = \mathbf{P}_{old} - \mathbf{K}\mathbf{H}\mathbf{P}$ 빼기 연산 과정에서 칼만 게인이 너무 공격적으로 적용되면 대각 성분이 일시적으로 아주 미세한 음수(예: $-1e-08$ )로 계산될 수 있다.
이를 방지하기 위해 PX4 개발자들은 0이 아니라는 확신을 줄 최소 분산 마지노선(Tolerance)을 시스템 전역 상수로 박아 두었다.

// 매 업데이트 스텝의 마지막에 대각(분산) 성분의 음수화를 필터링
for (unsigned i = 0; i < 24; i++) {
    // MIN_VARIANCE_TOLERANCE (예: 1e-6f) 이하로 값이 떨어지지 않도록 무조건 꺾어올림(Clipping)
    P(i, i) = math::max(P(i, i), MIN_VARIANCE_TOLERANCE);
}

이 클리핑(Clipping) 연산을 통해 EKF 코어 시스템 내부는 항상 양정치성(Positive-Definite)을 지켜내고 붕괴의 위험으로부터 탈출한다.

4. 소결: 유한계수를 수호하는 몸부림

칼만 필터를 수학 책(Textbook)으로만 접한 학부생은 오로지 연속 행렬 연산의 아름다움만을 생각한다. 하지만 그것을 C++ 언어로 타이핑하여 임베디드 칩 안에 심어 공중에 띄우는 시스템 엔지니어는, 이 세상에 무한정밀도(Infinite Precision)를 가진 실수는 존재하지 않으며 소수점 아래 7번째 자리부터 시작되는 오차의 저주(float 표현 한계)와 사투를 벌여야 한다는 것을 뼈저리게 알고 있다.

predictCovariance의 희소 행렬 오토제너레이션이 ’속도’를 극한으로 끌어올린 칼이라면, 앞서 살펴본 대칭성 복원 및 음수 클리핑 로직은 EKF를 ’에러 수렴’의 함정에서 구출해 내는 튼튼한 방패다.
PX4 ECL이 글로벌 상용 무인기 시장을 장악한 비밀은 화려한 수학 모형에만 있는 것이 아니라, 이러한 컴퓨터 공학적 예외 상황을 악착같이 보수해 낸 엔지니어링의 승리에 있다.