27.3.1.3. NED 좌표계 기준 로컬 위치 상태 변수 할당 (인덱스 7~9)

기체의 회전(자세, 인덱스 0~3)과 순간적인 프레임 이동(속도, 인덱스 4~6)을 정의했다면, EKF 코어 항법 동역학 파티션의 마지막을 장식하는 변수는 기체가 현재 공간상 어디에 위치해 있는가(Position) 이다.

본 절에서는 StateSample 구조체의 인덱스 7~9번에 매핑되는 로컬 위치(Local Position) 벡터의 의미를 해부하고, 왜 GPS 위도/경도가 아닌 데카르트(Cartesian) 평면 기반의 로컬 NED 좌표계를 사용하는지 그 수학적 당위성을 분석한다.

1. 상태 변수 매핑: `idx: 7~9`

src/lib/ecl/EKF/common.h 파일 내에서, 속도 변수 바로 다음인 7번 인덱스부터 길이 3의 메모리 슬라이스가 pos 라는 이름의 3차원 위치 벡터 참조자로 선언되어 있다.

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

    // ... (0~6 인덱스는 quat_nominal, vel)

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

이 pos 참조자가 포인팅하는 세 개의 실수 $p_N, p_E, p_D$ 의 물리적 의미는 다음과 같다.

states[7] ( $p_N$ ): 기준점(Home)으로부터 북쪽(North)으로 떨어진 미터( $m$ ) 거리
states[8] ( $p_E$ ): 기준점으로부터 동쪽(East)으로 떨어진 미터( $m$ ) 거리
states[9] ( $p_D$ ): 기준점으로부터 아래쪽(Down)으로 떨어진 미터( $m$ ) 거리. (주의: 음수일 경우 고도가 높아짐을 의미)

2. 왜 위도/경도(Global)가 아닌 미터(Local NED)인가?

GPS 수신기는 기본적으로 WGS84 좌표계 기반의 전역(Global) 위치인 위도(Latitude), 경도(Longitude), 타원체 고도(Altitude)를 출력한다.
그렇다면 EKF 상태 벡터는 왜 이 값들을 그대로 가져다 쓰지 않고, 굳이 기준점을 잡고 미터( $m$ ) 단위로 쪼갠 로컬 NED 좌표계를 채택했을까?

2.1 지구 곡률에 의한 비선형성 및 계산 복잡도 타파

만약 상태 벡터가 위도/경도를 직접 추정한다면, 위치를 갱신할 때마다 $\Delta$ 미터(속도 $\times$ 시간)를 $\Delta$ 도(degree)로 변환하기 위해 지구 반경과 위도에 따른 호의 길이 보정 공식(Haversine formula 등)을 코사인/사인 삼각함수와 함께 매 틱(250Hz)마다 계산해야 한다.
이는 저사양 마이크로컨트롤러에서는 극심한 연산 병목을 유발한다. 반면, 일정 반경(수십 km 이내)에서는 지구 표면을 평면(Flat Earth Assumption)으로 간주하더라도 데카르트 데드 레코닝(Dead Reckoning) 오차가 무시할 수준으로 작용한다.

2.2 부동소수점 정밀도(Floating-point Precision) 한계 극복

일반적인 32비트 단정밀도 부동소수점(float)으로 위도(예: 37.1234567)를 표현할 경우, 소수점 아래 7자리 부근에서 정밀도 손실(Truncation error)이 일어난다. 이는 지표면 거리를 기준으로 약 1~2미터의 오차 단위이며, 밀리미터 단위 제어를 해야 하는 칼만 필터 예측식에서는 재앙에 가깝다.
수학적 오버플로우와 정밀도 손실을 원천적으로 막기 위해, PX4 ECL은 시동(Arming)을 건 순간이나 GPS 3D Fix가 완료된 최초의 지점을 기준점(Origin, $0,0,0$ ) 으로 박아두고 거기로부터 상대적인 미터 변위만 추정하는 방식을 사용한다.

3. 위치 추정의 역학적 원리 (속도의 적분)

코드 레벨에서 7~9번 인덱스의 위치 변수가 업데이트되는 핵심은 바로 4~6번 인덱스의 ‘속도’ 적분이다.
앞서 언급한 predictState() 함수 루틴 하단부에서, 시스템은 앞 스텝에서 산출한 NED 속도를 이용해 다음과 같이 위치를 1차 수치 적분(Euler Integration)하여 갱신한다.

// 위치 예측의 1차 수치 적분 (predictState 로직 중 일부)

// 속도를 적분하여 로컬 평면 상의 변위 계산
pos(0) += vel(0) * dt;   // North 위치 = North 속도 * dt
pos(1) += vel(1) * dt;   // East 위치 = East 속도 * dt
pos(2) += vel(2) * dt;   // Down 위치 = Down 속도 * dt (고도 변화)

이 간단한 사칙연산 식 이면에는 엄청난 수학적 연쇄 폭발이 숨어 있다.
결국 이 pos 값은 “자이로와 가속도를 두 번 적분한 맹목적인 믿음값(Prediction)” 이다. 시간이 지날수록 센서 노이즈가 누적되어 이 값은 현실 세계에서 멀어지게 되며(Drift), 이를 제자리로 돌려놓는 것이 곧이어 관측 업데이트 루틴에서 작동하는 GPS나 옵티컬 플로우(Optical Flow) 데이터와의 혁신(Innovation) 오차 보정 단계가 된다.

4. 소결

자세(0~3), 속도(4~6), 위치(7~9)까지, 우리는 총 10개의 인덱스를 소모하여 EKF 24상태 중 Core Navigation 공간을 모두 채웠다. 이 10개의 변수만 정확하게 알면 기체 제어기(Position Controller)는 드론을 원하는 웨이포인트(Waypoint)로 날릴 수 있다.

하지만 저가형 MEMS 센서들이 이 10개의 값을 완벽하게 지켜줄 리 만무하다. 필연적으로 드리프트(Drift)가 발생한다. 다음 절부터는 남은 14개의 인덱스 공간을 활용하여, 비행 내내 끈질기게 괴롭히는 자이로스코프와 가속도계의 물리적인 ’센서 구조적 결함(Bias)’을 실시간으로 추정해 내는 보조 상태 공간 할당을 분석한다. 그 첫 여정은 인덱스 10~12번의 자이로 델타 앵글 바이어스다.