27.3.2. 관성 항법 방정식(Inertial Navigation Equations)의 C++ 구현체 분석

EKF 시스템에서 기체의 미래 위치와 자세를 수백 분의 1초 단위로 점쳐내는 ‘예측(Prediction)’ 단계는, 순수한 물리 법칙인 뉴턴 역학(Newtonian Mechanics)과 강체 동역학(Rigid Body Dynamics) 방정식을 컴퓨터 코드로 시뮬레이션하는 과정이다.

이러한 물리 시뮬레이션을 통틀어 관성 항법 방정식(Inertial Navigation Equations) 이라고 부르며, PX4 ECL 생태계에서는 이 막중한 임무를 predictState() 함수가 전담한다. 본 절에서는 이 함수 내부로 진입하여, 추상적인 미분 방정식들이 어떻게 24개의 StateSample 배열 값을 변화시키는 생동감 있는 C++ 코드로 치환되었는지 분석한다.

1. 물리적 통합(Integration)의 기본 논리: 오일러-크로머(Euler-Cromer) 방법

PX4 EKF2의 predictState() 루틴은 철저한 결정론적(Deterministic) 수치 적분을 기반으로 동작한다.
자이로스코프와 가속도계에서 측정된 \Delta \theta (회전량)와 \Delta v (속도 변화량)를 입력받아 이전 상태 벡터에 더해주는 이 과정은, 계산 복잡도를 낮추기 위해 1차 수치 적분인 오일러 방법(Euler Method)을 사용한다.

하지만 단순 오일러 방법이 아닌, 최신 속도를 위치 계산에 즉각 반영하여 에너지 보존성을 높이는 오일러-크로머 변형 기법을 활용하여 적분 오차의 누적을 최소화한다.

2. predictState() 내부 구현체 분석: 3단계 물리 연쇄 반응

src/lib/ecl/EKF/ekf.cpp (혹은 관련 예측 소스 파일) 내부에 존재하는 관성 항법 코드는 크게 세 가지 수학적 스텝(Step)으로 나뉘어 순차적으로 실행된다.

2.1 Step 1. 자이로스코프 바이어스 보상 및 쿼터니언 회전 (자세 업데이트)

가장 먼저, 앞서 27.3.1.4 절에서 다루었던 자이로 바이어스를 현재 센서 입력값에서 빼내어 ’순수한 회전량’을 추출한다.
그 후, 이 회전량을 4차원 복소수 행렬 곱셈을 통해 현재 자세(0~3번 인덱스)에 누적시킨다.

// 1. 센서 입력값에서 추정된 바이어스를 빼서 순수 물리량 추출
Vector3f delta_angle = imu_sample.delta_ang - state.delta_ang_bias;

// 2. 델타 앵글 벡터를 쿼터니언 형태로 변환 후 곱셈(회전) 수행
state.quat_nominal = state.quat_nominal * Quaternionf(delta_angle);

// 3. 부동소수점 오차 누적을 방지하기 위한 크기 1.0 정규화 (Normalization)
state.quat_nominal.normalize();

이 연산이 끝난 시점에서, 기체는 \Delta t 시간 동안 회전한 새로운 ’방향(Heading)’을 바라보게 된다.

2.2 Step 2. 가속도 바이어스 보상 및 로컬 프레임 변환 (DCM 연산)

자세가 업데이트되었다면, 이제 기체를 밀어붙이는 선형 가속도(비력, Specific Force)를 계산할 차례다.
가속도계 입력값에서 바이어스(13~15번 인덱스)를 빼낸 뒤, 방금 업데이트한 쿼터니언을 방향 코사인 행렬(DCM, \mathbf{R})로 변환하여 기체 축(Body Frame) 가속도를 NED 독립 좌표계(Earth Frame)로 회전시킨다.

// 1. 가속도 입력값에서 바이어스 보상
Vector3f delta_vel = imu_sample.delta_vel - state.delta_vel_bias;

// 2. 쿼터니언 행렬을 NED 회전 행렬(DCM)로 변환
Dcmf R_to_earth(state.quat_nominal);

// 3. 기체 프레임의 가속도를 지표면 프레임으로 회전 (Vector3f * Dcmf 연산)
Vector3f delta_vel_NED = R_to_earth * delta_vel;

2.3 Step 3. 중력 보상 및 속도/위치 1차 수치 적분

NED 프레임으로 회전된 가속도(delta_vel_NED)에는 아직 지구 중심을 향하는 9.81 m/s^2 의 중력 가속도가 포함되어 있다.
Z축(Down) 가속도 성분에서 중력(상수)을 상쇄시킨 후, 4~6번(속도)과 7~9번(위치) 인덱스에 값을 누적(Add) 시킨다.

// 1. Down 축(Z축) 성분에서 중력 가속도(Gravity) 보상
// (주의: NED 좌표계이므로 Down 방향이 +이므로 중력 가속도를 더해줌)
delta_vel_NED(2) += CONSTANTS_ONE_G * imu_sample.delta_vel_dt;

// 2. 속도 변수(인덱스 4~6) 적분 업데이트
state.vel += delta_vel_NED;

// 3. 위치 변수(인덱스 7~9) 적분 업데이트 (가속도를 두 번 적분하는 효과)
// 오일러-크로머 기법: 업데이트된 최신 vel 값을 사용하여 위치 적분
state.pos += state.vel * imu_sample.delta_vel_dt;

3. 요약: 물리 엔진으로서의 EKF 코어

위 C++ 코드 라인들은 드론이 날아가는 모습을 모니터 속 3D 게임 엔진이 렌더링하는 물리학 연산과 정확히 일치한다. EKF의 예측 코어는 외부의 어떠한 도움(관측) 없이도, 오직 IMU라는 두 눈(자이로/가속도)만으로 어둠 속에서 자신의 자세, 속도, 위치를 끊임없이 상상(Predict)해 내는 자율 항법 엔진이다.

하지만 불행히도 현실의 적분기는 시간이 지날수록 오차가 누적되어 상상과 현실이 걷잡을 수 없이 벌어진다. 이 불확실성의 폭주를 막기 위해서는 상태 벡터(x)의 예측뿐만 아니라, 오차가 얼마나 커지고 있는지를 추적하는 거대한 신뢰도 행렬, 즉 상태 공분산(Covariance) \mathbf{P} 배열의 예측이 병행되어야만 한다.

다음 절에서는 이 C++ 물리 엔진의 쌍둥이이자, 컴퓨팅 파워를 가장 많이 집어삼키는 24x24 공분산 예측 소스 코드(predictCovariance)의 희소 행렬 연산 구조를 파헤쳐 본다.