13.4.3.3. `GPS_YAW_OFFSET` 파라미터 적용 로직: 안테나 기구적 배치각을 Body Frame으로 변환하는 DCM(Direction Cosine Matrix) 회전 연산

13.4.3.3. GPS_YAW_OFFSET 파라미터 적용 로직: 안테나 기구적 배치각을 Body Frame으로 변환하는 DCM(Direction Cosine Matrix) 회전 연산

Dual Antenna GPS 시스템이 연산해 낸 요(Yaw) 각도는 두 안테나를 잇는 베이스라인(Baseline)이 ‘진북(True North)’ 대비 얼마나 틀어져 있는지를 나타내는 각도이다. 그러나 EKF2(Extended Kalman Filter)가 궁극적으로 알고 싶어 하는 것은 베이스라인의 방향이 아니라 드론 기체의 전면부(Body X-axis, Nose)가 향하는 방향이다.

드론의 기구학적 한계상 두 안테나를 완벽하게 기체의 종축(앞뒤 일직선) 상에 배치하지 못하고, 프로펠러 간섭을 피해 양 측면(좌우 암대) 또는 대각선으로 부착하는 경우가 빈번하다. 본 절에서는 이러한 기계적 오프셋(Offset)을 소프트웨어적으로 완벽하게 상쇄하여 진북 기반의 기체 방위각을 재건(Reconstruction)하는 GPS_YAW_OFFSET 파라미터의 C++ 파싱 메커니즘과 DCM(Direction Cosine Matrix) 회전 변환 로직을 분석한다.

1. 기하학적 패러독스: 기체 프레임(Body Frame) vs 안테나 프레임(Antenna Frame)

비행 제어의 기준 좌표계는 기체의 무게 중심을 원점으로 하고 코(Nose) 방향을 +X로 두는 기체 프레임(Body Frame, 통상 FRD: Forward-Right-Down)이다. 그러나 GPS 칩셋이 토해내는 상대 벡터(\Delta X, \Delta Y)는 오로지 Base 안테나에서 Rover 안테나를 향하는 선분일 뿐 기체의 형태를 알지 못한다.

  • 설치 예시 (가로 배치): 드론의 오른쪽 날개 끝에 Base를, 왼쪽 날개 끝에 Rover를 설치했다.
  • 센서의 보고: 드론이 북쪽을 보고 있을 때, Base \rightarrow Rover 화살표는 명백히 ’서쪽(West, 270^\circ)’을 가리킨다.
  • 오류 상황: 아무런 보정 없이 이 각도를 EKF2에 집어넣으면, 드론은 자신이 북쪽을 보고 있음에도 서쪽을 보고 있다고 착각하여 제어 루프가 즉각적으로 이륙 직후 기체를 곤두박질치게 만들 것이다.

2. GPS_YAW_OFFSET 의 파라미터 적용 기법

이러한 기구적 배치를 수학적으로 바로잡는 파라미터가 바로 EKF2_GPS_YAW_OFFSET (QGroundControl에서는 도(Degree, ^\circ) 단위로 표기됨)이다.

  • 설정 규칙: 베이스 안테나에서 로버 안테나를 바라보는 선(Antenna Frame X-axis)이, 드론의 정면(Body Frame X-axis)을 기준으로 시계 방향(Clockwise)으로 몇 도 틀어져 있는지 입력한다.
  • 앞선 ‘가로 배치’ 예시의 경우, 드론 정면을 기준으로 베이스(우축) \rightarrow 로버(좌축) 선분은 반시계 방향으로 90^\circ, 즉 시계 방향으로는 270^\circ (또는 -90^\circ) 만큼 틀어져 있으므로 GPS_YAW_OFFSET = -90 을 입력해야 한다.

3. 회전 행렬(DCM)을 이용한 C++ 2D 회전 변환 로직

PX4 펌웨어의 EKF 코어(src/lib/ecl/EKF/gps_yaw_fusion.cpp 부근)는 단순한 덧셈/뺄셈을 넘어, 이 오프셋 각도를 라디안(Radian)으로 치환한 후 오일러 회전 행렬(Rotation Matrix)의 기본기를 이용하여 보정을 수행한다.

안테나가 측정한 미가공(Raw) 요 각도를 \psi_{raw}, 파라미터로 입력된 오프셋 각도를 \alpha 라 할 때, 기체의 실제 진북 헤딩 \psi_{body} 를 구하는 가장 원시적이면서도 안정적인 방법은 1차원 선형합 연산 후 랩핑(Wrapping)을 거치는 것이다.

\psi_{body} = \psi_{raw} - \alpha

3.1 C++ 소스 코드 레벨의 랩핑(Wrap) 및 정대 로직

// 1. 파라미터에서 오프셋 획득 후 라디안 변환
float offset_rad = math::radians((float)_params.gps_yaw_offset);

// 2. 센서에서 올라온 생데이터 헤딩 각도 획득
float raw_heading = _gps_sample_delayed.heading;

// 3. 각도 차감 및 파이(-pi ~ pi) 대역으로 랩핑
float body_heading = wrap_pi(raw_heading - offset_rad);

여기서 wrap_pi() 함수는 오일러 각도의 불연속점(Singularity) 문제를 방지하기 위해 361^\circ 를 1^\circ 로, -185^\circ 를 +175^\circ 로 자동 치환해 주는 PX4 math 라이브러리의 핵심 유틸리티이다.

3.2 분산(Variance) 행렬의 불변성(Invariance)

위치(x,y)를 회전 변환할 때는 자코비안(Jacobian) 행렬을 곱하여 불확실성 타원(Error Ellipsoid)마저 회전시켜야 하지만, 요(Yaw) 방향이라는 1\text{D} 축을 따라 회전 스칼라 오프셋을 더하는 행위는 **분산(Variance)**의 크기에 하등의 영향을 주지 않는다.

즉, GPS가 보고한 heading_accuracy (오차 분산치)는 오프셋 보정 이후에도 그 수치를 그대로 유지하여 칼만 이득(Kalman Gain) 도출에 사용된다.
\sigma_{body\_heading}^2 = \sigma_{raw\_heading}^2
이러한 직관적인 통계 속성 덕분에 EKF2의 오프셋 보정 필터링 로직은 불필요한 공분산 행렬(Covariance Matrix) 재계산 오버헤드를 줄이며 최적의 구동 속도를 뽐낼 수 있다.

4. 최종 EKF 상태 벡터 융합(State Fusion)

교정된 기체 방위 body_heading 에 도달하면, 비로소 EKF2의 fuseYaw() 루틴이 이 각도를 EKF2의 24차 상태 벡터(State Vector) 중 회전 쿼터니언(Quaternion, q_0, q_1, q_2, q_3) 부분으로 흡수한다.
지자기 센서(Magnetometer)가 산출하던 3\text{D} 자기장 벡터 의존성을 완전히 벗어던지고, 오로지 이중 안테나의 전자기파 위상 지연(Phase Delay)과 한 줄의 1차 선형 방정식(raw_heading - offset_rad)만이 이끌어낸 **철갑을 두른 절대 방위(Iron-clad Absolute Heading)**가 완성되는 순간이다.

결과적으로 PX4 개발진이 GPS_YAW_OFFSET 파라미터를 제공하는 것은 단순한 편의 제공이 아니다. 공학 설계상 어쩔 수 없이 대칭을 깨고 안테나를 배치해야만 하는 드론 하드웨어 엔지니어들의 숨통을 틔워주며, 기체의 중심축(Body Frame)과 위성의 궤도축(Inertial Frame) 사이를 매끄럽게 결속시키는 가장 원초적이고 기하학적인 소프트웨어 렌치(Wrench)라 할 수 있다.