27.3.1. 벡터 인덱싱과 상태 변수 할당 (StateSample 구조체 분석)

수학적으로 정의된 24차원의 EKF 상태 벡터 \mathbf{x} 가 C++ 코드로 구현될 때, 가장 직관적이면서도 연산 효율성을 극대화할 수 있는 자료 구조는 무엇일까? PX4 ECL 개발진은 이를 해결하기 위해 고정 크기의 1차원 부동소수점 배열(Float Array)을 감싸는 StateSample 구조체를 고안해 냈다.

본 절에서는 src/lib/ecl/EKF/common.h 파일 내에 정의된 StateSample 구조체를 해부하고, 24개의 물리적 상태 변수들이 배열의 0번 인덱스부터 23번 인덱스까지 어떠한 순서와 묶음으로 할당(Allocation)되어 있는지 그 논리적 매핑(Mapping) 규칙을 추적한다.

1. StateSample 구조체의 탄생 배경

먼저 단순하게 생각해보면, 기체의 위치(pos), 속도(vel), 자세(quat)를 각각 독립적인 구조체 멤버 변수로 선언하는 것이 가독성 측면에서는 가장 유리해 보인다.

// 비효율적인 설계 예시 (ECL에서 사용하지 않음)
struct BadState {
    matrix::Quaternionf quat;
    matrix::Vector3f vel;
    matrix::Vector3f pos;
    // ...
};

하지만 EKF는 매 스텝마다 24 \times 24 크기의 공분산 행렬 \mathbf{P} 와 상태 천이 행렬 \mathbf{F} 를 통째로 곱하고 더해야 하는 막대한 선형 대수(Linear Algebra) 연산을 수행한다. 독립적인 멤버 변수로 쪼개져 있으면 메모리 상에 데이터가 연속적으로 존재하기 어렵고, 루프(Loop)를 돌며 한 번에 for (int i=0; i<24; i++) 처럼 상태 벡터 전체를 순회할 수 없다.

이 때문에 ECL은 크기가 24로 고정된 단일 1차원 벡터를 선언하고, 프로그래머가 인덱싱 실수를 하지 않도록 C++의 참조자(Reference) 문법을 활용하여 배열의 특정 메모리 조각(Slice)에 사람이 읽을 수 있는 이름을 붙여주었다. 이 구조체가 바로 StateSample 이다.

2. 블록 단위 벡터 인덱싱(Block Indexing) 원리

실제 src/lib/ecl/EKF/common.h에 정의된 24 상태 정의를 살펴보면, 인덱스는 난립하지 않고 철저하게 3차원 축(x, y, z)을 기준으로 한 블록(Block) 단위로 채워진다.

위 표에서 보듯, EKF2의 상태 벡터는 크게 3개의 거대한 파티션으로 나뉜다.
첫 번째 파티션(인덱스 0~9)은 뉴턴 역학이 직접 지배하는 코어 항법 상태이고,
두 번째 파티션(인덱스 10~15)은 저가형 실리콘 칩셋의 태생적 한계를 극복하기 위한 센서 오차 추정 상태이며,
세 번째 파티션(인덱스 16~23)은 드론을 둘러싼 외부 환경의 자기장과 바람을 추론하는 환경 상태이다.

3. C++ 참조자(Reference)를 통한 메모리 매핑

그렇다면 프로그래머는 저 24개의 숫자가 들어있는 states[24] 배열에 접근할 때 일일이 인덱스 번호를 외우고 있어야 할까? 전혀 그렇지 않다.

StateSample 구조체 내부를 보면, 앞선 챕터에서 배웠던 수학 라이브러리(matrix::Vector3f 등)와 C++의 메모리 참조 문법을 결합하여, 배열의 일부분을 가리키는 아바타(Avatar) 변수들을 만들어 두었다.

// src/lib/ecl/EKF/common.h 내부의 StateSample 구조체 요약
struct StateSample {
    matrix::Vector<float, 24> states; ///< 24개의 연속된 부동소수점 배열 (실제 메모리 할당)

    // --- 아래는 모두 'states' 배열의 메모리를 포인팅하는 참조자(Getter) 기능 ---
    
    // 0~3번 인덱스를 쿼터니언 객체로 취급
    const matrix::Quaternion<float>& quat_nominal{states.slice<4, 1>(0, 0)}; 
    
    // 4~6번 인덱스를 3차원 속도 벡터로 취급
    const matrix::Vector3f& vel{states.slice<3, 1>(4, 0)}; 
    
    // 7~9번 인덱스를 3차원 위치 벡터로 취급
    const matrix::Vector3f& pos{states.slice<3, 1>(7, 0)}; 

    // 10~12번 인덱스를 자이로 바이어스로 취급
    const matrix::Vector3f& delta_ang_bias{states.slice<3, 1>(10, 0)}; 

    // ... (이하 동일한 방식으로 23번 인덱스까지 매핑)
};

이러한 states.slice() 기법은 Eigen 라이브러리의 Block 연산과 동일한 철학을 갖는다. 추가적인 메모리 복사(Copy) 낭비 없이, 24 \times 1 배열 안의 특정 구역을 3 \times 1 수학 벡터처럼 연산할 수 있게 해 주는 극적인 최적화 기법이다.

4. 요약

StateSample 구조체는 EKF의 거대한 24차원 수학 공간을 C++ 시스템 메모리 공간으로 옮겨 놓기 위한 치밀한 청사진(Blueprint)이다.

개발자들은 배열 전체를 한 번에 복사하거나 순회할 때는 states 변수를 사용하고, 물리적인 방정식(예: 속도를 적분하여 위치 계산)을 코딩할 때는 가독성이 높은 vel 이나 pos 참조자를 꺼내 쓴다.

다음 하위 절들(27.3.1.1 ~ 27.3.1.8)에서는 0번부터 23번 인덱스까지 할당된 각각의 상태 변수 묶음들이 칼만 필터 예측 방정식 내에서 정확히 어떠한 물리적 스케일(Scale)과 제한 조건(Constraint)을 가지고 동작하는지 하나씩 현미경처럼 들여다본다.