27.3.2.2. 가속도계 비력(Specific force)의 로컬 프레임 변환을 위한 방향 코사인 행렬(DCM) 연산 최적화

27.3.2.2. 가속도계 비력(Specific force)의 로컬 프레임 변환을 위한 방향 코사인 행렬(DCM) 연산 최적화

상태 예측 함수(predictState)의 첫 번째 관문에서 기체의 자세(Quaternion) 적분이 무사히 완료되었다면, 두 번째 관문은 기체가 어느 방향으로 얼마나 세게 가속하고 있는지를 판단하는 속도(Velocity) 누적의 전처리 단계다.

이를 위해서는 기체 몸체에 부착된 가속도계가 측정한 3차원 가속도 데이터를 지구 기준의 NED 프레임으로 돌려놓는 험난한 좌표 변환 과정이 필수적이다. 본 절에서는 _state.quat_nominal 을 이용해 방향 코사인 행렬(Direction Cosine Matrix, DCM)을 생성하고 기체 프레임의 비력을 지구 프레임으로 변환하는 C++ 연산 최적화 과정을 해부한다.

1. 비력(Specific Force)의 정의와 바이어스 상쇄

로터가 고속으로 회전하며 기체를 위로 밀어 올릴 때, 가속도계 센서는 단순히 기계적인 진동이나 물리적인 밀림만을 측정하는 것이 아니다. 센서의 X, Y, Z축은 중력장 속에서 버티고 있는 힘 자체를 함께 읽어 들이는데, 관성 항법에서는 이 순수한 센서 측정 좌표계의 관성력을 비력(Specific Force) 이라고 부른다.

EKF는 이 원시 비력(또는 델타 벨로시티, \Delta v)을 받아들인 직후, 가장 먼저 소프트웨어적으로 추정해 낸 자기가장(Self-bias)을 빼내어 순수한 기동 가속도만을 추출한다.

// 1. 센서가 측정한 $\Delta v$ 에서 EKF가 추정한 가속도 바이어스(인덱스 13~15번) 상쇄
// corrected_delta_vel = 측정치 - 바이어스 추정치
matrix::Vector3f corrected_delta_vel = imu_sample.delta_vel - _state.delta_vel_bias;

2. 방향 코사인 행렬(DCM) 변환과 행렬 곱셈 연산

기체 축(Body Frame, FRD) 기준으로 측정된 corrected_delta_vel 은 그 자체로는 아무 쓸모가 없다. 기체가 롤(Roll) 방향으로 90도 기울어져 있다면, 기체 Z축(Down) 센서가 읽어들인 값은 사실 지구 기준으로는 Y축(East)으로 이동하는 가속력이기 때문이다.

따라서 기체 프레임의 벡터를 지구 프레임(NED) 벡터로 돌려놓으려면 3 \times 3 크기의 회전 변환 행렬인 방향 코사인 행렬(DCM, \mathbf{R}_{body}^{ned}) 을 곱해주어야 한다. predictState() 내부에서는 0~3번 인덱스에 저장된 최신 쿼터니언을 참조하여 이 DCM 행렬을 즉각적으로 생성해 낸다.

// 2. 최신 자세 쿼터니언(인덱스 0~3번)을 기반으로 DCM 3x3 회전 행렬 인스턴스화
matrix::Dcmf R_to_earth(_state.quat_nominal);

// 3. 기체 프레임(Body frame)의 델타 벨로시티를 지구 프레임(Earth frame)으로 회전 변환
// 수식: v_earth = R * v_body
matrix::Vector3f delta_vel_NED = R_to_earth * corrected_delta_vel;

이 연산이 통과하면, delta_vel_NED 의 세 성분은 비로소 드론이 북쪽, 동쪽, 지면 방향으로 얼마나 가속되었는지를 나타내는 지구 기준의 절대 물리량으로 재탄생한다.

3. matrix::Dcmf 캐스팅과 C++ 연산 최적화의 미학

여기서 주목해야 할 점은 matrix::Dcmf R_to_earth(_state.quat_nominal) 라인의 성능 최적화(Optimization) 철학이다.
쿼터니언 q = [q_0, q_1, q_2, q_3]^T 를 3 \times 3 DCM 행렬 원소로 변환하는 수학 공식은 다음과 같다.

\mathbf{R} = \begin{bmatrix} q_0^2 + q_1^2 - q_2^2 - q_3^2 & 2(q_1q_2 - q_0q_3) & 2(q_1q_3 + q_0q_2) \\ 2(q_1q_2 + q_0q_3) & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2(q_2q_3 - q_0q_1) \\ 2(q_1q_3 - q_0q_2) & 2(q_2q_3 + q_0q_1) & q_0^2 - q_1^2 - q_2^2 + q_3^2 \end{bmatrix}

자세히 보면 q_0^2, q_1^2, 2q_1q_2 와 같이 행렬의 각 셀(Cell)마다 겹치는 중복 곱셈 연산이 무수히 많다.
만약 개발자가 이 9개의 셀을 단순하게 하나씩 계산한다면 막대한 CPU 곱셈 사이클이 낭비될 것이다. 하지만 PX4에 내장된 Matrix C++ 템플릿 라이브러리는 쿼터니언을 역참조하여 DCM으로 캐스팅할 때 내부적으로 공통 부분식(Common Subexpression)을 미리 변수에 할당(Caching)하여 중복 계산을 컴파일 타임 혹은 런타임에 극단적으로 제거한다.

이러한 숨겨진 행렬 대수 라이브러리의 최적화 덕분에, STM32 계열의 저전력 코어에서도 초당 수백 번씩 이 무거운 공간 변환(Spatial Transformation)을 프레임 드랍 없이 부드럽게 소화해 낼 수 있는 것이다.

DCM 변환을 거쳐 속도를 지구를 기준으로 정렬했지만, 아직 이 속도 벡터에는 센서가 태생적으로 느껴버린 지표면의 강렬한 잡아당김, 즉 중력(Gravity)이 불순물처럼 섞여 있다. 다음 절에서는 이 중력을 도려내고 순수한 이동 변위를 적분해 내는 최종 속도/위치 타임스텝 연산 구조를 파헤쳐 본다.