27.1.1.5. 보정 단계: 사후 상태 업데이트 및 공분산 업데이트 로직

혁신(오차, $y_k$ )과 칼만 게인( $K_k$ )이 모두 준비되었다면, 이제 EKF 알고리즘의 마지막 종착지인 보정(Correction 또는 Update) 절차만이 남았다. 이 단계에서는 앞서 IMU로만 맹목적으로 추측해 놓았던 24차원의 방대한 기체 예측 상태( $\hat{x}_k^-$ )와 그 불확실성 공분산 행렬( $P_k^-$ )을 현실 세계의 궤도 위로 단호하게 끌어내리는 사후(A Posteriori) 확정 연산이 이루어진다.

PX4 ECL 펌웨어에서는 이 과정이 fuse() 센서 융합 함수 루틴의 가장 마지막 라인들에서 일괄적으로 전개된다.

1. 사후 상태 벡터 보정 (A Posteriori State Update)

EKF가 기체의 최종 상태를 확정 짓는 공식은 매우 단순하면서도 철학적이다.
“기존의 내 생각(예측)에다가, 내가 틀린 만큼(혁신)을 조언자의 신뢰도(칼만 게인)를 곱해서 더해주겠다.”

$\hat{x}_{k}^{+} = \hat{x}_{k}^{-} + K_{k} y_{k}$
(단, $y_{k} = z_{k} - h(\hat{x}_{k}^{-})$ )

$\hat{x}_{k}^{-}$ : 예측 단계(predictStates())에서 적분해 놓은 사전 상태 24차원 벡터
$K_{k}$ : 방금 전 단계에서 도출한 칼만 게인 행렬 (어떤 상태 변수 번호에 오차를 얼만큼 할당할지 결정하는 비율)
$y_{k}$ : 계산된 혁신 오차 스칼라 (또는 벡터)
$\hat{x}_{k}^{+}$ : 센서의 조언을 듣고 최종 확정된 사후 상태 벡터

1.1 ECL 소스 코드 레벨의 구현 특징

만약 GPS가 들어와서 위치(Position) 오차가 발견되었다고 가정하자. 직관적으로는 상태 벡터의 ‘위치’ 성분(인덱스 7, 8 등)만 수정하면 끝날 것 같지만, 확장 칼만 필터의 진정한 위력은 여기서 발휘된다.

GPS 위치 오차라 할지라도, 칼만 게인 배열 $K_k$ 에는 위치뿐만 아니라 속도, 자이로 바이어스, 자세 각도 등에 대한 보상 비율이 전부 미세하게 계산되어 들어 있다. 따라서 fuse() 루틴 내의 갱신 for 문을 거치면, 단 하나의 GPS 위치 오차 데이터가 쿼터니언과 풍속 상태 추정치까지 24차원 전역에 걸쳐 도미노처럼 교정(Correction)을 일으킨다.

// 24차원 상태 벡터 일괄 업데이트 로직 (의사코드)
for (unsigned i = 0; i < 24; i++) {
    // 델타 상태(오차 보상값) = 게인 * 혁신값
    _state_delta(i) += K_gain(i) * innovation; 
}

// 이후 _state_delta 배열의 값들을 본래의 _state 벡터에 더해주면서(또는 쿼터니언 곱셈) 최종 업데이트를 마무리한다.

2. 사후 오차 공분산 업데이트 (A Posteriori Covariance Update)

상태 벡터를 GPS의 도움을 받아 현실로 교정했으므로, 앞서 IMU 적분 단계에서 눈덩이처럼 부풀려 놓았던 불확실성( $P_{k}^{-}$ ) 풍선 바람을 다시 빼주어야 한다. 이 수축(Shrink) 과정은 다음과 같은 공식으로 전개된다.

$P_{k}^{+} = (I - K_{k} H_{k}) P_{k}^{-}$

$I$ : 단위 행렬 (항등 행렬)
$K_{k} H_{k}$ : 센서의 관측 모델( $H$ )이 기체의 상태 추정에 관여한 절대적인 지분율
$P_{k}^{+}$ : 다음 프레임 연산을 위해 저장해 두는 사후 오차 공분산 행렬

식을 직관적으로 해석해 보자. 괄호 안의 $(I - K_{k} H_{k})$ 는 $1$ 에서 관여한 지분율을 뺀 값이다. 센서 측정값을 강력하게 신뢰하여 칼만 게인 $K$ 가 컸다면, 이 뺄셈의 결과값(오차의 보존율)은 $0$ 에 가깝게 대폭 줄어들 것이다.

즉, “뛰어난 센서를 통해 내 위치를 방금 확신했으니, 나의 불확실성 행렬 $P$ 의 덩치를 지금 당장 무자비하게 깎아내리겠다“는 선언과 같다.

1.1 조셉 형태(Joseph Form)의 대칭성 강제 보존

위 기본 공식은 계산은 빠르지만, 컴퓨터의 부동소수점 오차(Floating-point error)가 누적되다 보면 공분산 행렬 $P$ 가 대칭 행렬(Symmetric Matrix)의 성질을 잃어버리고 마이너스 분산을 띄며 필터가 붕괴(Divergence)되는 현상이 발생할 수 있다.

따라서 PX4 ECL 등 고도로 안정적인 상용 펌웨어에서는 연산량이 조금 더 많더라도 무조건 양의 정부호(Positive Definite)와 대칭성을 보존해 주는 안전한 변형 공식인 조셉 폼(Joseph Form) 을 채택하거나, 계산 직후 매턴마다 대칭화(Symmetrization) 강제 코드를 삽입하여 수치 해석적 붕괴를 원천 차단한다.

// 강제 대칭화 루틴 예시 (부동소수점 오차 방어)
for (unsigned row = 0; row < 24; row++) {
    for (unsigned col = 0; col < row; col++) {
        float p = 0.5f * (P(row, col) + P(col, row));
        P(row, col) = p;
        P(col, row) = p;
    }
}

2. 요약: $\Delta t$ 사이클의 종료와 새로운 시작

사후 상태 벡터 $\hat{x}_k^+$ 와 사후 오차 공분산 행렬 $P_k^+$ 가 무사히 도출되었다.
이로써 이산 시간 EKF의 길고 길었던 하나의 프레임(Timestep) 5단계 여정이 모두 끝났다. 방금 산출된 이 ’최상’의 사후 결과물(+ 기호)들은 다음 $0.004$ 초 뒤(250Hz 기준)에 다시 맞이할 루프의 ’사전 예측(- 기호)’을 위한 훌륭한 과거( $k-1$ ) 밑거름 데이터로 메모리에 저장된 채 생명을 다한다.

이 쉼 없는 예측-보정의 무한 궤도(Infinite Loop) 덕분에, 조종기의 피치 스틱을 미는 찰나의 순간에도 기체는 자신이 지구 한가운데 어디에 떠 있는지 한 치의 흔들림 없이 인지할 수 있는 것이다.