27.5. 센서 퓨전 코어(Sensor Fusion Core) 소스 레벨 분석

IMU 데이터를 맹목적으로 믿고 기체의 미래 상태를 점쳐내는 예측 단계(Prediction)와, 링 버퍼(Ring Buffer)를 통해 센서 간의 시간 지우개를 맞추는 지연 처리 단계를 거쳤다면, EKF 알고리즘의 대단막은 단연 관측 업데이트(Observation Update), 즉 센서 퓨전 코어(Sensor Fusion Core)다.

PX4 ECL 아키텍처에서 이 핵심 로직은 fuseVelocity(), fusePosition(), fuseBaro(), fuseMag() 등 센서 종류별로 특화된 일련의 fuseXYZ() 함수 군으로 분리되어 구현된다. 본 절에서는 이 함수들의 공통적인 수학적 파이프라인과 잔차(Innovation) 산출, 그리고 칼만 게인(Kalman Gain) 행렬 곱셈을 통한 상태 보정의 3단계 소스 레벨 메커니즘을 분석한다.

1. 예상 관측치(Predicted Measurement) 산출

센서 데이터를 융합하기 위한 첫 번째 단계는, EKF 스스로가 상상하고 있는 현재 기체의 상태 벡터를 바탕으로 “저 센서에서는 과연 어떤 값이 나와야 정상일까?” 를 계산해 내는 것이다. 이를 예상 관측치(Predicted Measurement, $h(\mathbf{x}_k)$ )라고 부른다.

예를 들어, 현재 EKF가 상상하는 속도가 $v_{NED} = (10, 0, 5)$ 이고 추정된 GPS 속도 지연 시간이 200ms라면, 링 버퍼에서 200ms 전의 기체 상태를 복원하여 GPS 센서가 읽었어야 할 예상 속도를 다음과 같이 산출한다.

// src/lib/ecl/EKF/gps_fusion.cpp 내부 (개념적 코드)

// 1. EKF 상태 기반 예상 관측치 산출
// NED 프레임 속도는 상태 벡터 4~6번 인덱스에 저장되어 있음
Vector3f vel_pred = _state.vel; // + GPS 안테나 편심 오차 등 추가 보정 연산 포함

간단한 GPS 속도는 이처럼 상태 변수 자체와 거의 일치하지만, 복잡한 지자기 센서나 광학 흐름(Optical Flow) 센서의 경우 0~3번의 쿼터니언 자세와 야코비안 회전 행렬을 총동원하여 수십 줄에 걸친 비선형 예상 관측 방정식을 연산해 내야 한다.

2. 잔차(Innovation)와 잔차 공분산(Innovation Covariance) 연산

예상 관측치(pred)가 준비되면, 실제 물리 세계의 센서가 가져온 원시 데이터(obs)를 들이밀어 그 차이를 구한다. 이 차이를 잔차(Innovation 또는 Residual) 라고 부르며, EKF가 현실 세계와 얼마나 단절되어 있는지를 보여주는 척도다.

// 2. Innovation 계산: 실제 관측치 - 예상 관측치
Vector3f innov_vel = gps_sample.vel - vel_pred;

단순히 잔차를 구했다고 해서 이것을 무작정 기체 상태에 덧셈/뺄셈하여 보정해 버리면 제어기가 폭발한다. GPS 수신기가 잠시 미쳐서 오류 데이터를 던졌을 수도 있기 때문이다. 따라서 EKF는 자신의 현재 상태 공분산 행렬( $\mathbf{P}$ )과 센서의 노이즈 분산( $\mathbf{R}$ 파라미터)을 비교하여 잔차 공분산(Innovation Covariance, $\mathbf{S}$ ) 을 계산한다.

$\mathbf{S} = \mathbf{H} \mathbf{P} \mathbf{H}^T + \mathbf{R}$

$\mathbf{H}$ : 예상 관측 함수의 야코비안 (현재 기체 상태가 관측치에 미치는 민감도)
$\mathbf{P}$ : 현재 EKF 상태 불확실성 (24x24 행렬)
$\mathbf{R}$ : GPS 속도 센서의 노이즈 (사용자 지정 파라미터 EKF2_GPS_V_NOISE)

S 값이 지나치게 크다면, EKF는 이 잔차(innov_vel)를 일시적인 글리치(Glitch)로 판단하여 융합을 아예 폐기(Gate)해 버린다. 이 혁신 게이팅(Innovation Gating) 로직이야말로 날아가는 새의 배설물이 카메라 렌즈를 가렸을 때 드론이 추락하지 않게 지켜주는 방파제다.

3. 칼만 게인(Kalman Gain) 산출 및 상태 벡터 보정

혁신 게이팅을 무사히 통과했다면, 센서 퓨전의 궁극적인 존재 이유인 칼만 게인( $\mathbf{K}$ ) 을 계산할 차례다.

$\mathbf{K} = \mathbf{P} \mathbf{H}^T \mathbf{S}^{-1}$

이 수식을 C++ 텍스트로 풀어 쓰면 다음과 같은 물리적 철학이 담겨 있다.

“내 상태 공분산( $\mathbf{P}$ )이 커서 나 자신을 못 믿겠는데, 센서 노이즈( $\mathbf{R}$ )는 작아서 센서가 확실하다고 하네? 그러면 칼만 게인 $\mathbf{K}$ 를 크게 만들어서 이번 GPS 데이터를 전적으로 반영하자!”
“내 공분산은 아주 촘촘한데 GPS 노이즈 파라미터가 엄청 크다면? $\mathbf{K}$ 를 거의 0에 가깝게 만들어서 이번 GPS 데이터는 무시하고 내 갈 길(예측)을 가자.”

픽스호크 ECL 내의 융합 코드(예: fuseVelocity())는 이 칼만 게인 벡터를 잔차에 곱하여, 24개의 상태 변수 공간 내 어떤 변수를 얼마나 더하고 빼야 할지를 나타내는 보정 벡터(Correction Vector)를 도출한다.

// 3. 칼만 게인 계산 및 24상태 벡터 업데이트 (심볼릭 자동 생성 코드 기반)
for (unsigned i = 0; i < 24; i++) {
    // 24개의 각 상태 변수에 대한 칼만 게인(K) 도출
    float K = P_H_T(i) / S_variance;

    // K * Innovation 만큼을 기존 상태 벡터에 "누적(빼기)"하여 융합
    _state.states(i) -= K * innov_vel_measurement;

    // 공분산 행렬(P) 축소: 융합으로 인해 정보가 추가되었으므로 불확실성 감소
    for (unsigned j = 0; j < 24; j++) {
        P_next(i, j) = P(i, j) - K * P_H_T(j);
    }
}

이 루프가 도는 순간 EKF의 심장부는 폭발적으로 요동친다.
GPS의 $1 m/s$ 속도 잔차 정보 하나가 들어왔을 뿐인데, 칼만 게인 배열의 크로스-커플링(Cross-coupling) 효과를 타고 들어가 기체의 쿼터니언(0~3) 자세를 비틀고, 위치(7~9)를 교정하며, 심지어 센서 칩셋 내부의 가속도계 바이어스(13~15)까지 깎아내는 전방위적인 오차 수정이 연쇄적으로 일어나는 것이다.

3. 소결: 수학적 파동의 연속

퓨전 함수가 리턴되는 순간 공분산 행렬 $\mathbf{P}$ 는 다시 줄어든다 (정보를 얻어 불확실성이 해소되었기 때문).
하지만 다음 predictState 와 predictCovariance 가 호출되면 프로세스 노이즈( $\mathbf{Q}$ )에 의해 불확실성은 다시 부풀려진다.

EKF는 하늘에 떠 있는 내내 “예측(비대해짐) $\rightarrow$ 퓨전(게이팅) $\rightarrow$ 보정(축소)” 이라는 호흡을 일초에 수백 번씩 내쉬며, 노이즈로 가득한 난기류 속에서 기체의 절대적인 진실을 쟁취해 낸다.

하지만 만약 센서가 통째로 죽어버리거나 지속적인 글리치를 뿜어낸다면 단일 EKF 인스턴스만으로는 기체를 살릴 수 없다. 다음 절에서는 이 임계 상황을 타개하고 여객기 수준의 생존성을 부여하는 다중 인스턴스(Multi-Instance) EKF 및 페일오버(Failover) 아키텍처를 분석한다.