27.5.2.2. 안테나 위치 편차 오프셋을 반영한 레버 암(Lever Arm) 보상 수식의 C++ 구현

드론의 자동 비행 성능을 결정짓는 숨은 디테일 중 하나는 레버 암(Lever Arm) 효과의 정밀한 보상이다. 비행 제어기(Pixhawk 등) 내부에 탑재된 IMU 센서와 기체 외부 돛대(Mast)에 설치된 GPS 안테나 간에는 필연적으로 물리적 거리(Offset)가 존재한다. 기체가 제자리에 떠서 요(Yaw), 피치(Pitch), 롤(Roll) 축으로 회전할 때, IMU는 이동 속도를 0으로 측정해야 하지만, 멀리 떨어진 GPS 안테나는 원심력과 접선 속도에 의해 상당한 속도로 궤적을 그리며 이동하게 된다.

PX4-Autopilot의 EKF2 엔진은 이 물리적 불일치로 인한 오차를 수학적으로 제거하기 위해 정교한 오프셋 보상 파이프라인(Offset Compensation Pipeline) 을 C++로 구현하고 있다. 본 절에서는 이 레버 암 보상 수식이 소스 코드 레벨에서 어떻게 야코비안 행렬과 예상 관측치를 변형시키는지 해부한다.

1. 레버 암(Lever Arm)의 물리 및 수학적 모델링

PX4 파라미터 EKF2_GPS_POS_X, Y, Z를 통해 입력되는 안테나 오프셋 데이터는 기체 고정 좌표계(Body Frame) 를 기준으로 한다. 반면 GPS가 뿜어내는 속도 측정치는 지구 고정 좌표계(NED Frame) 를 따른다.

따라서 EKF가 상상하는 ’예상 관측치(Predicted Measurement)’를 구하기 위해서는, 먼저 Body Frame에 있는 안테나의 선속도를 구한 뒤, 이를 NED Frame으로 변환하여 기체 중심(CG)의 속도에 더해주어야 편집한다.

수학적 모델링:
$\mathbf{v}_{\text{GPS, NED}} = \mathbf{v}_{\text{CG, NED}} + \mathbf{R}_{\text{Body}}^{\text{NED}} (\boldsymbol{\omega}_{\text{Body}} \times \mathbf{r}_{\text{offset, Body}})$

$\mathbf{v}_{\text{CG, NED}}$ : IMU(무게 중심) 기준 기체의 속도 (상태 벡터 4~6번)
$\mathbf{R}_{\text{Body}}^{\text{NED}}$ : Body에서 NED로의 회전 행렬 (상태 벡터 0~3번 쿼터니언 기반)
$\boldsymbol{\omega}_{\text{Body}}$ : 자이로스코프에서 측정된 3축 각속도 (Rates)
$\mathbf{r}_{\text{offset, Body}}$ : 사용자 파라미터 세팅으로 입력된 안테나 편차 벡터

2. C++ 소스 코드에서의 보상 수식 구현

PX4 ECL 라이브러리(src/lib/ecl/EKF/gps_fusion.cpp 내부)에서는 위 수식을 매우 효율적인 C++ 외적(Cross Product) 연산과 방향 여현 행렬(DCM, Direction Cosine Matrix) 연산으로 치환하여 실행한다.

// 1. 자이로 각속도와 안테나 오프셋 벡터의 외적 연산
// 이 연산은 기체가 회전함에 따라 Body Frame 내에서 발생하는 안테나 끝단의 선속도를 계산한다.
Vector3f vel_offset_body = cross_product(_state.rates, _gps_pos_offset_body);

// 2. Body Frame 속도를 NED Frame 속도로 회전 변환
// _R_to_earth는 현재 EKF 상태(쿼터니언)가 나타내는 회전 행렬이다.
Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;

// 3. EKF 상태 벡터 기반 예상 속도에 안테나 궤적 속도를 더함
Vector3f vel_pred = _state.vel + vel_offset_earth;

이렇게 계산된 vel_pred가 바로 융합 함수 fuse() 에 전달될 진정한 ’예상 관측치’가 되며, 이 값이 실제 GPS 속도 측정치와 비교되어 잔차(Innovation)를 형성한다.

3. 야코비안 행렬( $\mathbf{H}$ )에 미치는 영향 (Cross-Coupling)

단지 예상 관측치만 보상했다고 해서 EKF가 완벽해지는 것은 아니다. 레버 암 효과는 칼만 필터의 핵심인 야코비안 행렬 편미분 구조를 완전히 뒤틀어 놓는다.

만약 안테나 오프셋이 없다면, GPS 동쪽(East) 속도가 변화했을 때 $\mathbf{H}$ 행렬에 의해 EKF의 동쪽 속도 상태 변수(인덱스 5번)만 업데이트되면 끝난다.

하지만 안테나 오프셋 파라미터(EKF2_GPS_POS_X 등)가 설정된 상태라면, 레버 암 수식에 기체의 회전(쿼터니언) 이 깊숙이 관여( $\mathbf{R}_{\text{Body}}^{\text{NED}}$ )하게 되므로, GPS 측정치에 대한 편미분을 수행할 때 기체의 자세 상태 변수(인덱스 0~3번)에 대한 미분 값이 0이 아니게 된다.

3.1 자동 생성 코드(Auto-generated Code)에 의한 비선형 편미분 전개

이 복잡한 쿼터니언 삼각함수의 편미분을 사람이 일일이 코딩하는 것은 불가능에 가깝다. PX4는 파이썬 SymPy 모듈 기반의 스크립트(matlab_scripts/ 내의 파생 유틸리티들)를 통해 이 수백 줄의 미분 공식을 심볼릭 연산으로 자동 도출하고 C++ 코드로 생성한다.

// src/lib/ecl/EKF/ekf_helper.cpp (심볼릭 자동 생성 코드의 단면)
// GPS 속도 스칼라 융합 시, 자세(Quaternion 0~3번)에 대한 H 행렬 성분 계산 로직
if (_gps_pos_offset_body.length() > 0.01f) {
    // 쿼터니언 상태변수(q0, q1, q2, q3)와 자이로 비율(rates), 오프셋 벡터의 편미분 전개식
    float H_vel_q0 = 2.0f * ( ... 심볼릭 수식 전개 ... );
    float H_vel_q1 = 2.0f * ( ... 심볼릭 수식 전개 ... );
    // ...
    
    // 이 성분들이 0~3번 상태 인덱스를 타겟으로 fuse() 함수에 전달됨
    fuse(gps_vel, vel_pred, H_vel_q0, 0, ...);
}

4. 소결: 레버 암 보상의 군사적/산업적 위력

코드 몇 줄에 불과해 보이지만, 이 철저한 레버 암 오프셋 보상과 그에 연동된 자세(Attitude) 상태 변수의 $\mathbf{H}$ 행렬 편미분 전개는 EKF2 아키텍처의 정밀도를 한 차원 끌어올린다.

고정익의 꼬리 날개나 대형 멀티로터의 긴 붐(Boom) 끝단에 부착된 GPS 센서가 기동 중 뿜어내는 극심한 회전 원심 속도 노이즈 속에서도, 이 C++ 로직은 그 오차를 정확히 발라내어 기체의 순수 무게 중심 속도를 추론할 뿐만 아니라, 역으로 기체의 요(Yaw) 각도 오차를 교정해 내는 크로스-커플링(Cross-Coupling) 현상의 마법을 만들어낸다.

다음 절에서는 이 강력한 메커니즘을 동일하게 공유하지만, 3차원 벡터 특성상 또 다른 기술적 난관을 품고 있는 지자계 퓨전(fuseMag) 모듈의 상태 머신 아키텍처로 넘어가 살펴본다.