27.3.2.3. 중력 가속도 보상 및 타임스텝 기반의 속도/위치 1차 수치 적분(Euler Integration) 구현

27.3.2.3. 중력 가속도 보상 및 타임스텝 기반의 속도/위치 1차 수치 적분(Euler Integration) 구현

쿼터니언 적분을 통해 기체의 자세를 업데이트하고(인덱스 0~3), 방향 코사인 행렬(DCM)을 이용해 기체 프레임의 가속도를 지구 프레임(NED)으로 회전시켰다면, 상태 예측 함수(predictState)의 마지막 종착지는 기체가 공간상 어디로 얼마나 이동했는지 계산하는 일이다.

본 절에서는 EKF 코어 연산의 화룡점정에 해당하는 중력 보상(Gravity Compensation) 연산과, 이를 바탕으로 속도(인덱스 4~6) 및 위치(인덱스 7~9) 상태 변수를 갱신하는 1차 수치 적분(Euler Integration)의 C++ 소스 코드 구현체를 정밀 분석한다.

1. 중력 가속도 보상: 하늘을 나는 센서의 숙명

드론이 지면 에 가만히 놓여 있을 때, 가속도계의 Z축(Down 방향) 센서는 약 -9.81 m/s^2 의 맹렬한 가속도를 끊임없이 보고한다. 지구가 기체를 잡아당기는 중력(Gravity)에 맞서 지면이 기체를 수직 항력으로 밀어 올리고 있기 때문이다. 자유 낙하(Free Fall) 상태가 아닌 이상, 기체에 탑재된 가속도계 데이터에는 항상 이 지구의 중력 가속도 성분이 불순물처럼 섞여 들어간다.

따라서 기체가 순수하게 모터의 추력만으로 만들어낸 이동 가속도를 구하려면, NED 프레임으로 정렬된 가속도 벡터에서 중력 벡터를 강제로 빼내야(Compensation) 한다.

PX4 ECL의 predictState() 내부 연산 코드를 보면 다음과 같이 중력을 상쇄한다.

// src/lib/ecl/EKF/ekf.cpp 내 중력 보상 연산 로직

// 상수 선언: CONSTANTS_ONE_G = ~9.80665f (m/s^2)
// imu_sample.delta_vel_dt: 센서 샘플링 타임스텝 (초 단위, 약 0.004s)

// NED 좌표계에서 Z축은 'Down(아래)' 방향을 의미함 (+ 방향)
// 중력 벡터는 항상 아래(+Z)를 향하므로, 센서 측정값에서 중력만큼을 "빼주어야" 순수 기동 가속도가 나옴.
// 센서가 측정한 Z축 델타 속도(delta_vel_NED(2)) 부호 반전 주의:
// 센서는 지위를 향하는 수직항력을 기록하므로 보상식은 사실상 "더하기" 형태가 될 수 있음(프레임 정의에 따라).
// PX4 ECL 공식 코드 표현:
delta_vel_NED(2) += CONSTANTS_ONE_G * imu_sample.delta_vel_dt;

이 연산을 통해 delta_vel_NED 벡터는 부끄러운 중력의 껍질을 벗고, 드론이 허공에서 자신이 원하는 방향으로 밀어붙인 ‘순수 가속력(\Delta v)’ 벡터로 완벽하게 벼려진다.

2. 타임스텝 연동 1차 수치 적분 (오일러-크로머 방법)

가장 순수한 형태의 델타 속도(delta_vel_NED)를 얻어냈다면, 남은 것은 이를 누적(Accumulate)하는 덧셈 연산뿐이다. 이를 미적분학에서는 수치 적분(Numerical Integration)이라고 부른다.

PX4 EKF는 저사양 마이크로컨트롤러 환경에서의 연산 부하를 최소화하기 위해 룽게-쿠타(Runge-Kutta) 같은 고차원 적분 대신 1차 오일러 적분(Euler Integration)을 채택했다. 이때, 속도 누적 결과를 곧바로 위치 누적에 재사용하는 오일러-크로머(Euler-Cromer) 방법을 사용하여 에너지 보존 법칙 위배로 인한 발산을 억제한다.

// 속도 및 위치 상태 변수에 대한 1차 수치 적분 (Euler-Cromer Integration)

// 1. 속도 적분: 기존 속도(인덱스 4~6)에 델타 속도를 더함
// v(t) = v(t-1) + \Delta v
_state.vel += delta_vel_NED;

// 2. 위치 적분: 방금 업데이트된 "최신 속도(_state.vel)"에 타임스텝(dt)을 곱하여 기존 위치(인덱스 7~9)에 더함
// p(t) = p(t-1) + v(t) * \Delta t -> 오일러-크로머 방식의 핵심 (이전 속도 v(t-1)이 아님)
_state.pos += _state.vel * imu_sample.delta_vel_dt;

일반적인 오일러 전방 적분(Forward Euler)은 p(t) = p(t-1) + v(t-1) * dt 형태를 띠어 진동 시스템에서 예측 오차가 무한히 발산(Explode)하는 치명적인 단점이 있다. 반면 방금 업데이트한 속도 v(t) 를 위치 적분에 투입하는 오일러-크로머 방식(Semi-implicit Euler)은 심플리틱(Symplectic) 성질을 강제로 부여하여, 적분 오차가 일정 범위 내에서 주기적으로 진동하며 발산하지 않도록 만들어주는 극강의 가성비 알고리즘이다.

3. 소결: 상상의 나래를 펼치는 EKF 예측기

자이로 델타 각 추출 \rightarrow 쿼터니언 행렬 적분 \rightarrow DCM 변환 \rightarrow 가속도 비력 추출 \rightarrow 중력 상쇄 \rightarrow 오일러-크로머 위치 적분에 이르는 숨 가쁜 물리 연쇄 반응이 단 한 줄의 오차 없이 완료되었다.

이러한 predictState() 함수는 IMU 센서에 데이터가 들어올 때마다 (주로 수백 Hz) 무한 반복되며 기체의 상태 변수 (0번~23번) 배열 값들을 끝없이 미래 시간으로 외삽(Extrapolate)한다. EKF는 지금 GPS나 나침반의 도움 없이 오직 자신의 머릿속 물리 엔진만으로 기체의 위치를 맹목적으로 믿고 그려내는 중이다.

하지만 현실의 물리 센서는 노이즈와 오차가 만연하다. 이 맹목적인 믿음이 얼마나 위험한 상상인지, 컴퓨터 스스로가 자신의 오차 범위를 평가하고 두려워하는 치밀한 과정이 존재해야만 한다. 그리고 그것을 코드로 구현한 것이 바로 24x24 행렬의 지옥이라 불리는 공분산 예측(Covariance Prediction) 모듈이다.

다음 절부터는 드론의 두뇌를 극한으로 혹사시키는 predictCovariance 소스 코드 내부로 진입한다.