27.1.1.3. 혁신(Innovation) 도출 및 혁신 공분산 산출 과정

27.1.1.3. 혁신(Innovation) 도출 및 혁신 공분산 산출 과정

이산 시간 EKF 알고리즘의 두 번째 단계인 보정(Update) 은 전적으로 외부 센서(GPS, 기압계, 지자기 센서, 비전 등)의 새로운 측정값이 시스템에 도착했을 때 트리거된다.

하지만 EKF는 센서가 보내준 값을 곧이곧대로 믿고 기체의 위치를 덮어쓰지(Overwrite) 않는다. 대신 자신이 물리 법칙으로 정교하게 시뮬레이션해 온 ’내부 예측값’과 센서가 가져온 ‘외부 측정값’ 사이의 불일치성(Discrepancy) 을 먼저 수학적으로 계산해 내는데, 제어 공학에서는 이 오차 격차를 혁신(Innovation) 또는 잔차(Residual) 라고 부른다.

본 절에서는 PX4 ECL의 fuse() 계열 함수 내부에서 가장 먼저 일어나는 혁신 벡터와 혁신 공분산의 행렬 도출 로직을 파헤친다.

1. 혁신(Innovation) 벡터의 도출

혁신 벡터(통상 기호 y_{k} 로 표기)는 EKF가 얼마나 ’크게 놀랐는지’를 나타내는 지표다. 내 생각과 현실의 차이를 의미한다.

y_{k} = z_{k} - h(\hat{x}_{k}^{-})

  • z_{k}: 현재 센서 하드웨어가 뱉어낸 실제 측정 데이터 벡터 (예: GPS가 쏜 위도/경도/고도)
  • h(\hat{x}_{k}^{-}): EKF가 앞선 예측 단계에서 구한 사전 상태(\hat{x}_{k}^{-})를 기반으로 ’관측 함수 h(\cdot)’를 돌려 역으로 추측해 낸 관측 기대치

1.1 ECL 소스 코드 구현체 (혁신의 계산)

PX4 ECL의 소스 코드(src/lib/ecl/EKF/ 폴더 내 vel_pos_fusion.cpp 등)를 살펴보면, 각 센서의 특성에 맞게 혁신을 구하는 로직이 철저히 분산되어 있다.

// GPS 위치 혁신(Innovation) 계산 예시 (의사코드)
// 1. 센서 관측값 (z_k)
Vector2f gps_pos_meas = _gps_sample_delayed.pos;

// 2. 동역학 예측값 (h(x_k^-))
// EKF의 상태 벡터 7번, 8번 인덱스가 NED 좌표계의 위치(North, East)를 담고 있다.
Vector2f ekf_pos_pred(_state.pos(0), _state.pos(1));

// 3. 혁신 도출 (y_k)
Vector2f pos_innov = gps_pos_meas - ekf_pos_pred; 

// 혁신값은 이후 _innov 배열에 저장되어 로그(ULog)로 실시간 스트리밍된다.
_innov(index) = pos_innov(0);

만약 드론이 예측한 궤도대로 정확히 날고 있고 센서에도 노이즈가 전혀 없다면, 이 혁신 벡터 y_{k} 는 모든 축에 대해 정확히 0이 될 것이다. (하지만 현실에서는 센서 바이어스나 바람 등의 외란으로 인해 끊임없이 0 주변을 진동하는 화이트 노이즈 패턴을 띠게 된다.)

2. 혁신 공분산(Innovation Covariance) 행렬 산출

EKF는 방금 구한 혁신 분산(오차)이 과연 “내 물리 모델이 틀려서 생긴 심각한 오차인지”, 아니면 단순히 “센서 자체가 심하게 흔들려서 발생한 노이즈인지“를 확률적으로 판단해야 한다.

이를 결정하기 위한 재판정의 엄격도 기준선이 바로 혁신 공분산(Innovation Covariance, S_{k}) 이다.

S_{k} = H_{k} P_{k}^{-} H_{k}^{T} + R_{k}

이 공식은 두 가지 상이한 불확실성 소스를 하나로 더하는 과정이다.

  1. H_{k} P_{k}^{-} H_{k}^{T}: 나의 내부 상태 예측치(P_{k}^{-}) 자체가 원래부터 가지고 있던 불확실성. 관측 야코비안 행렬(H_{k})을 아령처럼 양옆에 씌워 상태 공간(24\times24)을 센서 공간으로 투영(Projection)한 값이다.
  2. R_{k}: 센서 하드웨어 데이터시트나 파라미터 튜닝을 통해 고정된 센서 측정 노이즈 행렬 (Measurement Noise Matrix). (예: EKF2_GPS_V_NOISE 파라미터로 조절되는 GPS 속도 노이즈 분산값).

결국 S_{k} 행렬은 “현재 우리가 예상할 수 있는 총체적인 오차의 분산(한계) 폭” 을 의미한다. 만약 방금 1단계에서 구한 실제 혁신값(y_{k})이 이 S_{k} 가 허용하는 표준편차 바운더리를 심각하게 초과한다면, EKF는 이 센서 데이터가 완전히 고장(Glitch)이 났다고 판단하여 퓨전을 거부하는 안전 장치(Innovation Test Threshold)를 가동하게 된다.

1.1 EKF2 센서 거부(Rejection) 로직의 코어

이 공식의 이면에는 매우 중요한 엔지니어링 팩트가 숨어 있다. S_k 의 도출은 단순히 다음 연산을 위한 거쳐 지나가는 변수가 아니다. PX4는 상태 추정기가 무너지는 것을 막기 위해 이 S_k 를 기반으로 Chi-square Test(카이 제곱 검정) 를 수행한다.

// 혁신 테스트 (Innovation Consistency Check) 로직 예시
float test_ratio = sq(pos_innov) / (innovation_variance * gate_size);

if (test_ratio > 1.0f) {
    // 혁신이 산출된 허용 분산(S_k)보다 훨씬 크다 -> GPS가 튀었으므로 융합 거부!
    _gps_pos_innov_rejected = true;
    return; // fuse() 함수 종료, 상태 보정 건너뜀
}

2. 요약: 현실 감각의 디지털화

요약하자면, y_{k} (혁신)는 “내가 틀린 정도(오차의 양)” 를 나타내며, S_{k} (혁신 공분산)는 “내가 그 오차를 확률론적으로 얼마나 너그럽게 포용할 것인가(오차의 한계)” 를 정의해 주는 척도이다.

이 두 개의 수학적 산출물은 다음 단계에서 계산될 칼만 게인(Kalman Gain) 연산기의 핵심 분자와 분모로 그대로 투입되어, 센서 데이터를 최종적으로 기체 상태에 얼만큼 반영할지 그 운명적인 비율을 결정짓게 된다.