27.3 24상태 EKF 모델 동역학 및 소스 코드 매핑
PX4의 상태 추정 및 제어 라이브러리(Estimation and Control Library, ECL) 내 ekf2 모듈은 수학적으로 24개의 고유한 상태 변수(State Variables)를 가진 확장 칼만 필터(Extended Kalman Filter, EKF) 모델을 기반으로 동작한다. 이 24개의 변수들은 기체의 현재 3차원 위치, 속도, 자세뿐만 아니라, 저가형 MEMS 측정 센서들이 태생적으로 가지고 있는 오차(바이어스)와 주변 환경의 외란(바람, 자기장 왜곡)까지도 실시간으로 추정해 내는 핵심 동력이다.
본 장에서는 이 24상태 EKF 모델이 어떻게 정의되어 있으며, 이 추상적인 수학 항들이 실제 PX4 C++ 소스 코드 레벨에서 어떠한 구조체 배열 인덱스로 매핑(Mapping)되어 관리되는지 심층적으로 해부한다. 이 추적 과정을 거치고 나면, 단순한 ’필터 연산’을 넘어 동역학 방정식이 코드로 직접 시뮬레이션되는 관성 항법(Inertial Navigation)의 본질을 이해할 수 있다.
1. 24상태(24-State) 벡터의 수학적 조감도
수학 파트에서 다루었던 EKF의 상태 벡터 \mathbf{x} 는 24개의 실수(Float) 성분으로 이루어진 24 \times 1 열벡터(Column Vector)이다. PX4 ECL은 이 벡터를 논리적인 물리 그룹으로 나누어 관리하며, 그 구성은 다음과 같다.
- 자세 (Attitude): 4차원 쿼터니언 (Quaternion)
- 속도 (Velocity): 3차원 NED (North-East-Down) 속도
- 위치 (Position): 3차원 NED 위치
- 자이로 바이어스 (Gyro Bias): 3차원 각속도 센서 오프셋
- 가속도 바이어스 (Accel Bias): 3차원 가속도 센서 오프셋
- 지구 자기장 (Earth Magnetic Field): 3차원 절대 지자기장 벡터
- 기체 왜곡 자기장 (Body Magnetic Field): 3차원 기체 고정축 지자기 측정 왜곡(Hard-iron) 벡터
- 바람 속도 (Wind Velocity): 2차원 수평(North, East) 풍속 벡터
총합 4 + 3 + 3 + 3 + 3 + 3 + 3 + 2 = 24 개의 상태가 도출된다.
이 24개의 변수는 기체가 하늘에 떠 있는 매 순간순간, 고도 수백 미터 상공의 기압 변화, 태양풍에 의한 자기장 뒤틀림, 계절풍의 방향 변화를 수학적 확률로 추론해 내는 생명체의 감각 기관과도 같다.
2. 상태 전이 모델(State Transition Model)의 본질
EKF 모델 동역학(Dynamics) 코드를 이해하기 위해서는 근본적인 ’예측(Prediction)’의 철학을 짚고 넘어가야 한다.
k 번째 타임스텝의 상태 \mathbf{x}_k 는 이전 타임스텝 \mathbf{x}_{k-1} 와 시스템의 비선형 동적 모델 함수 f(), 그리고 입력 함수(제어 입력 혹은 자이로/가속도 적분값) \mathbf{u}_k 에 의해 다음 식으로 전파(Propagate)된다.
\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k
여기서 프로세스 노이즈 \mathbf{w}_k 를 제외한 결정론적 예측 파트는 관성 항법 시스템(INS, Inertial Navigation System) 의 편미분 방정식을 그대로 따른다. 즉, 현재 속도에 시간 변화량(\Delta t)을 곱해 위치를 더하고, 자이로스코프의 델타 각(\Delta \theta) 행렬로 쿼터니언을 회전시키는 작업이다.
이때, 단순 모델과 고도화된 모델(PX4 EKF2)의 차이를 가르는 핵심은 상태 백터 안에 ’센서 바이어스’나 ‘바람’ 모델을 포함시켰는지 여부다.
바람 속도나 센서 바이어스는 물리적으로 매초 극심하게 변하는 값이 아니므로, 동적 모델 f() 내에서 이들은 흔히 ’상수(Random Walk)’로 모델링되어 예측식에서는 이전 값을 그대로 가져가고 다음 측정 업데이트(Update) 단계에서 공분산에 의해 오차가 보정되는 메커니즘을 띤다.
3. C++ 소스 코드의 StateSample과 Covariance 행렬
PX4의 소스 코드 트리 src/lib/ecl/EKF/ 폴더 내부로 들어가면, 이 24개의 수학적 기호들은 고정된 크기의 부동소수점 배열과 구조체 안에 단단히 결속되어 있다.
이 24차원의 상태를 담아두는 버퍼 구조체가 바로 StateSample 이며, 오차 공분산(Error Covariance)을 저장하는 행렬은 크기가 자그마치 24 \times 24 에 달하는 거대한 정사각 행렬(Square Matrix) 구조를 띈다. EKF가 매 컴퓨팅 틱(Tick)마다 가장 많은 CPU 사이클을 쏟아붓는 작업이 바로 이 24 \times 24 행렬의 야코비안(Jacobian) 희소 행렬 연산(Sparse Matrix Multiplication)이다.
이어지는 하위 장들에서는 이 StateSample 구조체가 정확히 배열의 인덱스 몇 번지에 어떤 물리량을 매핑하고 있는지 하나하나 추적하고, 이를 바탕으로 EKF의 예측(Prediction) 함수(predictState, predictCovariance) 내부로 들어가 C++로 구체화된 관성 항법 방정식의 실체를 파헤쳐 본다.