27.3.1.1. 쿼터니언 기반 자세 상태 변수 할당 (인덱스 0~3)

관성 항법 시스템(INS)의 모든 연산, 즉 센서 데이터의 좌표 변환부터 속도의 적분까지 모든 과정의 가장 역학적 토대가 되는 정보는 바로 ’기체가 어느 방향을 보고 있는가’이다.

이 때문에 EKF 24상태 벡터(states[24])에서 가장 영광스러운 인덱스 0번부터 3번까지의 맨 앞자리 4칸 공간은 자세 쿼터니언(Attitude Quaternion) 에게 할당된다. 본 절에서는 왜 오일러 각(Euler Angle)이 아닌 쿼터니언이 상태 벡터의 가장 중요한 첫 단추로 채택되었는지, 그리고 이 4개의 변수가 C++ 구조체 내에서 어떻게 매핑되는지 분석한다.

1. 짐벌 락(Gimbal Lock) 회피와 쿼터니언의 당위성

학습자나 일선 조종사들에게 가장 친숙한 3차원 회전 표현 방식은 앞뒤(Pitch), 좌우(Roll), 제자리 회전(Yaw)으로 표현되는 3차원 오일러 각이다.

만약 EKF의 상태 벡터가 오일러 각을 사용했다면 인덱스는 3칸(\phi, \theta, \psi)만 차지하여 연산량이 줄어들었을 것이다. 하지만 PX4 시스템은 아크로바틱(Acrobatic) 비행이나 급격한 회전 시에 수직(Pitch \pm 90^\circ)을 바라볼 때 두 개의 회전축이 겹쳐버리는 치명적인 수학적 특이점(Singularity), 즉 짐벌 락(Gimbal Lock) 현상을 원천 차단해야만 했다.

이를 완벽하게 극복하기 위한 해답이 바로 4차원 복소수 체계인 해밀턴 쿼터니언(Hamiltonian Quaternion: q_0, q_1, q_2, q_3)의 도입이다. 쿼터니언은 특이점이 존재하지 않으며 반올림 오차에 대한 보정(정규화, Normalization)이 연산적으로 훨씬 저렴하다는 강력한 이점을 갖는다.

2. 상태 벡터 내 쿼터니언 변수 매핑 (idx: 0~3)

StateSample 구조체 코드를 보면 24개의 실수 배열 중 최상단 4개의 실수를 잘라내어 쿼터니언 객체로 바인딩(Binding)하는 것을 볼 수 있다.

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

    // 인덱스 0, 길이 4의 슬라이스(Slice)를 쿼터니언 참조자로 선언
    const matrix::Quaternion<float>& quat_nominal{states.slice<4, 1>(0, 0)}; 
};

이 4개의 숫자는 논리적으로 NED(North-East-Down) 지구 고정 프레임(Earth Frame)에서 FRD(Forward-Right-Down) 기체 프레임(Body Frame)으로의 방향 코사인 행렬(Direction Cosine Matrix, DCM)을 정의하는 파라미터 역할을 한다.

PX4 문맥에서 각 쿼터니언 요소의 수학적 위치는 다음과 같다.

• states[0] (q_0): 스칼라부 (Scalar part), 투영된 회전량(Cosine)과 연관
• states[1] (q_1): 기체의 X축(Forward-Roll) 방향 벡터 성분
• states[2] (q_2): 기체의 Y축(Right-Pitch) 방향 벡터 성분
• states[3] (q_3): 기체의 Z축(Down-Yaw) 방향 벡터 성분

3. 메모리 참조의 수학적 효용성

states.slice<4, 1>(0, 0) 로 정의된 quat_nominal 참조 변수가 강력한 이유는, 방대한 24개의 플로트 덩어리에서 단지 앞쪽 4개의 주소 포인터만을 따와 이들을 matrix::Quaternion 객체의 멤버 함수들로 마음껏 주무를 수 있다는 점이다.

예를 들어 EKF 예측 연산에서 자이로스코프 데이터를 읽어들여 쿼터니언을 업데이트한 뒤 길이가 1.0이 되도록 정규화(Normalize)를 수행해야 할 때, 프로그래머는 다음과 같이 직관적이고 객체 지향적인(Object-Oriented) 코드를 작성할 수 있다.

// EKF 예측 로직 내에서의 쿼터니언 활용 예시 (의사코드)

// 1. 자이로 적분값을 이용한 복소 행렬 회전 수행
quat_nominal = quat_nominal * gyro_delta_angle_quaternion;

// 2. 부동소수점 오차 누적 방지를 위한 4차원 거리 정규화 (크기를 1로 만듦)
quat_nominal.normalize(); 

컴파일러는 위 코드를 해석할 때, 무거운 복사 연산 없이 그저 거대한 C 배열 states의 0~3 번지 메모리에 직접 접근하여 제곱근 스케일링을 수행하는 고성능 어셈블리어를 찍어내게 된다.

이제 가장 기본이 되는 기체의 ’자세(자전)’를 할당했으니, 다음 절에서는 공간을 뚫고 지나가는 선형적인 힘 표현을 위해 배열의 4~6번에 매핑되는 NED 속도(Velocity) 변수의 구조를 파헤쳐보겠다.