27.3.1.2. NED 좌표계 기준 3차원 속도 상태 변수 할당 (인덱스 4~6)

자세 역학(기체가 어디를 바라보는가)을 결정짓는 0~3번 인덱스의 쿼터니언 바로 다음 자리는, 기체가 ‘어떤 방향으로, 얼마나 빠르게 이동하는가’ 를 나타내는 선형 병진 동역학(Linear Translational Dynamics)의 핵심인 3차원 속도(Velocity) 벡터가 차지한다.

본 절에서는 StateSample 구조체 내에서 4~6번 인덱스에 매핑되는 이 3차원 속도 벡터의 물리적 의미와, 왜 기체 고정축(Body Frame)이 아닌 NED 좌표계를 기준 프레임(Reference Frame)으로 사용하는지 그 타당성을 분석한다.

1. 상태 변수 매핑: `idx: 4~6`

src/lib/ecl/EKF/common.h 파일 내 StateSample 구조체를 살펴보면, 4번 인덱스부터 시작하는 길이 3의 메모리 슬라이스가 vel 이라는 이름표(Reference)를 달고 있는 것을 확인할 수 있다.

// src/lib/ecl/EKF/common.h 내부의 속도 슬라이싱
struct StateSample {
    matrix::Vector<float, 24> states;

    // ... (0~3 인덱스는 quat_nominal)

    // 인덱스 4부터 시작하는 길이 3의 슬라이스를 3차원 벡터 참조자로 선언
    const matrix::Vector3f& vel{states.slice<3, 1>(4, 0)}; 
};

이 vel 참조자를 통해 접근되는 3개의 실수 $v_N$ , $v_E$ , $v_D$ 는 논리적으로 다음과 같이 대응된다.

states[4] ( $v_N$ ): 북쪽(North)을 향하는 속도 성분 ( $m/s$ )
states[5] ( $v_E$ ): 동쪽(East)을 향하는 속도 성분 ( $m/s$ )
states[6] ( $v_D$ ): 아래쪽(Down)을 향하는 속도 성분 ( $m/s$ , 즉 하강 속도)

2. 왜 기체 축(Body Frame)이 아닌 NED 좌표계인가?

드론에 탑재된 가속도 센서(Accelerometer)는 기체의 앞(Forward), 오른쪽(Right), 아래(Down) 축을 기준으로 한 가속도( $a_x, a_y, a_z$ )를 측정한다. 그렇다면 왜 EKF의 속도 상태 변수는 가속도 센서와 동일한 기체 축(Body Frame)을 놔두고 번거롭게 NED 절대 좌표계를 사용할까?

그 이유는 지구 중력(Gravity)의 보상과 수치 적분(Numerical Integration)의 직관성 때문이다.

2.1 중력 보상(Gravity Compensation)의 단순화

기체가 허공에 가만히 떠 있는(Hovering) 상태일 때, 가속도 센서는 기체 축 Z 방향으로 $-9.8 m/s^2$ 의 맹렬한 가속도를 출력한다 (정확히는 위로 밀어 올리는 비력, Specific Force).
만약 기체 축을 기준으로 속도를 추정하면, 기체가 기울어질 때마다 중력 벡터도 같이 기울어지므로 매 순간 기체 축의 모든 속도 성분( $v_x, v_y, v_z$ )에서 중력의 영향을 삼각함수로 쪼개어 빼주어야 하는 엄청난 수학적 난이도에 직면한다.

하지만 기준 프레임을 항상 중력이 수직으로만 작용하는 로컬 지구 좌표계(NED)로 고정하면, Z축(Down) 속도를 적분할 때만 단순히 상수 $g(9.81)$ 를 빼주면 되므로 중력 제거가 극도로 단순해진다.

2.2 GPS 및 위치 센서와의 공간적 일치

또한 기체의 궤적 이동이나 GPS 속도계가 보내오는 관측(Observation) 데이터들은 기본적으로 지구 위도/경도를 기반으로 한 NED 속도( $v_N, v_E, v_D$ ) 포맷으로 들어온다.
속도 상태 변수를 미리 NED 기준으로 두면, GPS 측정치가 들어왔을 때 복잡한 회전(Rotation) 변환 없이 EKF의 관측 업데이트(Measurement Update) 방정식인 $H$ 행렬을 단순한 단위행렬( $I$ ) 형태로 구성할 수 있어 연산 오버헤드가 급감한다.

3. 속도 적분 과정에 대한 간략한 스케치

C++ 코드 상에서 이 4~6번 인덱스의 vel 벡터가 다뤄지는 메인 파이낸스는 EKF의 상태 예측(predictState) 로직 내부다.
간단히 말하면, (1) 가속도 센서값을 0~3번의 쿼터니언을 이용해 NED 좌표계로 ’회전’시키고, (2) 중력(Down 축)을 제거한 뒤, (3) 시간 변화량 $\Delta t$ 를 곱해 이 vel 값에 ’누적(Add)’하는 방식이다.

// 속도 예측의 논리적 흐름 스케치 (predictState.cpp 중 일부 물리적 요약)

// 1. 기체 축 가속도 값(비력)을 NED 기준으로 회전 변환
Vector3f accel_NED = R_to_earth * accel_body;

// 2. 오일러-크로머(Euler-Cromer) 1차 수치 적분을 통한 속도 누적
vel(0) += accel_NED(0) * dt;                  // North 속도 갱신
vel(1) += accel_NED(1) * dt;                  // East 속도 갱신
vel(2) += (accel_NED(2) + 9.81f) * dt;        // Down 속도 갱신 (중력 가속도 보상)

이처럼 속도는 가속도라는 입력 신호를 한 번 적분(Integration)한 결과물이자, 다음에 다룰 ‘위치(Position)’ 를 산출해 내기 위한 원재료가 된다.

다음 절에서는 이 3차원 속도 벡터가 다시 한번 적분되어 도출되는, 기체의 최종 목적지이자 인덱스 7~9번 공간을 차지하는 NED 3차원 로컬 위치(Local Position) 변수에 대해 심화 탐구해보겠다.