27.5.2.1. 관측 행렬 내 속도/위치 인덱스 매핑 및 단일 스칼라 업데이트 기법 적용

앞 절(27.5.2)에서 설명한 바와 같이, GPS 센서는 $6 \times 1$ 크기의 3차원 속도 및 위치 벡터를 EKF에 공급한다. 하지만 PX4의 EKF2 엔진은 이 6개의 데이터를 수백 건의 곱셈이 수반되는 $24 \times 24$ 행렬 연산으로 한 번에 묶어서 융합하지 않는다. 임베디드 환경(NuttX RTOS)의 제약 속에서 비행 제어 루프의 실시간성(Real-time)을 철저히 보장하기 위해, PX4는 관측 행렬( $\mathbf{H}$ )의 상태 인덱스 축소 매핑(Mapping) 과 단일 스칼라 업데이트(Single Scalar Update) 기법이라는 최적화된 수학적 파이프라인을 가동한다. 본 절에서는 이 최적화 로직의 소스 코드 구현 구조를 깊이 있게 해부한다.

1. 관측 행렬( $\mathbf{H}$ ) 내 상태 인덱스 매핑(Mapping)

$\mathbf{H}$ 행렬(야코비안 행렬)은 현재의 상태 벡터 $\mathbf{x}$ (24차원)가 센서의 측정값에 얼마나 기여하는지를 나타내는 민감도(Sensitivity) 행렬이다. GPS 데이터는 NED(North, East, Down) 프레임 기준이므로 EKF의 상태 차원 중 다음과 같이 직접적으로 매핑된다.

EKF 상태 벡터 인덱스	물리적 의미	매핑되는 GPS 데이터
4 ~ 6	NED 속도 ( $v_N, v_E, v_D$ )	GPS 3축 속도
7 ~ 9	NED 위치 ( $p_N, p_E, p_D$ )	GPS 3축 위치 (경도/위도/고도를 NED로 환산)

1.1 희소 행렬(Sparse Matrix) 최적화

전체 24개의 인덱스 중 GPS 속도 측정 시 의미 있는 값을 갖는 인덱스는 오직 4번, 5번, 6번뿐이다 (안테나 레버 암 효과가 없는 기본 상태를 가정할 때). PX4 코드는 크기가 24인 배열 6개를 만들고 나머지 21개 요소에 0.0f를 채우는 비효율적인 메모리 할당을 전면 배제한다.

대신, 상태 벡터의 어느 인덱스(Index) 를 조작할 것인지만을 지정하는 정수(Integer) 배열과 해당 인덱스의 비영(Non-zero) 요소 값만을 저장하는 배열을 분리하여 관리한다.

// src/lib/ecl/EKF/gps_fusion.cpp (개념적 코드 예시)

// 속도 측정에 대응하는 상태 벡터 인덱스 (4, 5, 6)
const uint8_t state_index_vel[3] = {4, 5, 6};

// H 행렬 내에서 비영 요소의 값 (단순 속도 매핑이므로 1.0)
const float H_vel[3] = {1.0f, 1.0f, 1.0f};

// 위치 측정에 대응하는 상태 벡터 인덱스 (7, 8, 9)
const uint8_t state_index_pos[3] = {7, 8, 9};

const float H_pos[3] = {1.0f, 1.0f, 1.0f};

2. 단일 스칼라 업데이트(Single Scalar Update) 기법

GPS로부터 6차원 데이터 벡터가 들어왔을 때, 이를 한 번에 $\mathbf{y}$ 벡터로 구성하여 EKF 공식을 적용하면 어떻게 될까? 가장 치명적인 문제는 혁신 공분산 행렬 $\mathbf{S}$ 가 $6 \times 6$ 크기를 갖게 되고, 칼만 게인 $\mathbf{K}$ 를 구하기 위해 이 $6 \times 6$ 행렬의 역행렬(Inverse)을 구해야 한다는 점이다. 행렬 역산 알고리즘(예: 가우스 조던 소거법이나 LU 분해)은 $O(n^3)$ 의 시간 복잡도를 요구하므로, 픽스호크의 CPU 연산량(Clock Cycles)을 급격히 낭비하게 만든다.

이를 타개하기 위해 PX4는 수학적으로 엄밀하게 동등한 결과를 도출하는 축차적 스칼라 업데이트(Sequential Scalar Update) 혹은 단일 요소 융합(Single-element Fusion) 기법을 채택하였다.

2.1 C++ `for` 루프를 통한 6회 분할 융합

6차원의 측정 모델은 센서 관측 간의 상호 상관성(Cross-correlation)이 없거나 무시할 수 있다고 가정할 경우, 각각 1차원인 6개의 독립적인 관측으로 쪼개서 순차 적용할 수 있다.

// 6개의 데이터 차원을 분리하여 6번의 스칼라 융합을 수행
for (uint8_t axis = 0; axis < 3; axis++) {
    // 1~3회차: 속도 X, Y, Z 순차 융합
    fuse(gps_vel(axis), predicted_vel(axis), H_vel[axis], state_index_vel[axis], ...);
}

for (uint8_t axis = 0; axis < 3; axis++) {
    // 4~6회차: 위치 X, Y, Z 순차 융합
    fuse(gps_pos(axis), predicted_pos(axis), H_pos[axis], state_index_pos[axis], ...);
}

이 방식의 마법은 하위 fuse() 함수 내부에서 극대화된다. $\mathbf{S}$ 가 더 이상 행렬이 아닌 스칼라(1x1 단일 부동소수점 값) 가 되기 때문이다.

$\mathbf{K} = \mathbf{P} \mathbf{H}^T \mathbf{S}^{-1} \quad \xrightarrow{\text{스칼라 업데이트 시}} \quad \mathbf{K} = \frac{\mathbf{P} \mathbf{H}^T}{S_{\text{scalar}}}$

거대한 반복문을 요구하던 행렬 역산 로직이, 단 한 번의 C++ 나눗셈 연산자(/)로 대체되는 순간이다.

3. 순차적 업데이트의 수학적 파급력 (Cross-Coupling)

단일 스칼라 업데이트 기법을 볼 때 흔히 착각하는 점은 “GPS 북쪽 속도를 융합했으니, EKF 내부의 북쪽 속도 상태 변수(인덱스 4번)만 변경될 것“이라는 생각이다. 하지만 현실은 전혀 다르다.

북쪽 속도 관측치 스칼라 하나를 fuse() 함수에 던져 넣더라도, 내부에서는 $\mathbf{P}$ 행렬 전체( $24 \times 24$ )와의 크로스-커플링(Cross-Coupling)을 기반으로 전체 24차원 모두에 대한 칼만 게인 열 벡터( $24 \times 1$ )가 생성된다.

즉, 단 하나의 GPS 스칼라 속도 데이터가 오차를 교정한 파동은, $\mathbf{P}$ 행렬을 타고 전파되어 기체의 쿼터니언 자세 오차(0~3번 인덱스)를 비틀어 교정하고, 가속도계 센서 바이어스(13~15번 인덱스)까지 미세하게 깎아낸다.

그리고 이렇게 갱신된 새로운 $24 \times 24$ $\mathbf{P}$ 행렬과 $\mathbf{x}$ 벡터를 바탕으로, for 루프의 다음 바퀴인 동쪽(East) 속도 융합이 이전 회차의 연산 결과를 상속받아 연쇄적으로 수행되는 정교한 폭포수 모델이 완성된다.

4. 소결

PX4 EKF2의 관측 인덱스 매핑과 스칼라 업데이트 아키텍처는 행렬 기반의 수학 공식을 CPU 친화적인 코드로 극단적으로 최적화한 모범 사례다. 공간 복잡도를 제거하는 희소 행렬 매핑과, $O(n^3)$ 시간 복잡도를 나눗셈 하나로 축소한 이 테크닉 덕분에 구형 STM32F4 기반의 픽스호크 보드에서도 400Hz의 필터 주기 방어가 가능한 것이다.

다음 절에서는 지금까지 단순하게 1:1 매핑된다고 가정했던 기체 무게 중심과 GPS 센서 안테나 간의 물리적 거리, 즉 레버 암(Lever Arm) 보상 수식이 C++ 코드 상에서 자이로스코프의 각속도와 얽혀 어떻게 야코비안 행렬( $\mathbf{H}$ ) 구조를 복잡하게 비틀어 놓는지 상세히 살펴본다.