27.5.3. 지자계 퓨전 (`fuseMag`)

스마트폰부터 상업용 드론에 이르기까지 지구 자기장의 방향을 읽어내어 기체의 기수 방향(Heading, Yaw)을 결정하는 지자계(Magnetometer, 나침반) 센서는 비행 필수품이다. 그러나 EKF 관점에서 지자계 융합은 GPS나 기압계보다 훨씬 더 까다롭고 변칙적인 특성을 지닌다. 주변의 금속 구조물, 모터에서 발생하는 전자기 간섭, 심지어 비행 장소의 위도에 따른 자기장 복각(Inclination)의 변화까지 모든 것이 노이즈의 원인이 되기 때문이다.

PX4-Autopilot의 Ekf::fuseMag() 함수는 이러한 3차원 자기장 벡터 데이터를 24차원의 상태 벡터와 엮어내는 중추 역할을 한다. 본 절에서는 지자계 데이터가 어떻게 관측 행렬에 기여하고, 어떠한 분기(Branch) 로직을 거쳐 최종 융합되는지 펌웨어 코드 수준에서 분석한다.

1. 지자계 퓨전의 수학적 목표와 기하학적 복잡성

GPS가 병진 운동(Translation)을 책임진다면, 지자계 센서는 회전 운동(Rotation) 중 오직 요(Yaw) 축의 절대적인 기준점을 확보하는 데 그 존재 의의가 있다. 롤(Roll)과 피치(Pitch)는 중력 벡터(가속도계)를 통해 명확히 알 수 있지만, 중력과 수직인 요 축은 가속도계만으로는 절대 알 수 없기 때문이다.

지자계 센서는 기체 고정 좌표계(Body Frame)를 기준으로 X, Y, Z축의 3차원 자기장 세기( $\mathbf{m}_{\text{body}}$ )를 가우스(Gauss) 등 측정 단위로 반환한다. EKF는 자신이 상상하고 있는 현재의 ’기체 자세(Quaternion)’와 ‘지구 자기장의 방향(Earth Magnetic Field, 상태 변수 16~18번)’, 그리고 ’기체 자체의 자기장 편향 오차(Body Magnetic Bias, 상태 변수 19~21번)’를 모두 조합하여 예상 관측치를 계산해 내야 한다.

// 1. 예상 관측치(Predicted Measurement) 연산의 논리적 흐름
// _state.mag_I: 지구 자기장 벡터 (NED 프레임)
// _state.mag_B: 기체 내부 금속/전선에 의한 자기장 편향 (Body 프레임)
// _R_to_body: 현재 EKF 쿼터니언 상태를 기반으로 한 NED -> Body 회전 행렬

Vector3f mag_pred = (_R_to_body * _state.mag_I) + _state.mag_B;

이렇게 계산된 mag_pred 3차원 벡터가 실제 나침반 센서 모듈에서 읽어 들인 3차원 측정치와 비교되어 잔차(Innovation) 3형제를 만들어낸다.

2. 야코비안 행렬( $\mathbf{H}$ ) 구성과 크로스 커플링(Cross-Coupling)

지자계 퓨전은 GPS 속도 퓨전(단순한 1:1 매핑)과는 차원이 다른 복잡한 관측 야코비안( $\mathbf{H}$ ) 행렬을 갖는다. 위 수식에서 볼 수 있듯이, 지자계 예상 관측치는 다음 3가지 부류의 상태 변수들과 복잡하게 얽혀(Coupled) 있다.

자세 상태 변수 (인덱스 0~3, $q_0 \dots q_3$ ): 회전 행렬 $\mathbf{R}_{\text{body}}^{\text{NED}}$ 을 구성하므로 가장 치명적인 영향을 미친다.
지구 지구장 상태 변수 (인덱스 16~18, $magN, magE, magD$ ): 위치에 따라 변하는 지구 고유의 자기장 값.
기체 자기장 바이어스 변수 (인덱스 19~21, $magX, magY, magZ$ ): 드론 동체 자체가 뿜어내는 자기장 노이즈.

fuseMag() 함수가 호출될 때, matlab_scripts로 자동 생성된 심보릭 파생 코드 블록은 무려 10개의 상태 변수에 대해 복잡한 삼각함수 편미분 방정식을 전개하여 $\mathbf{H}$ 행렬 값들을 도출해 낸다.

이 거대한 결합(Coupling) 덕분에, 단일 지자계 축의 측정 오차가 발생하면 필터는 “이것이 기체가 회전(Yaw)해서 발생한 변화인지, 기체 부품의 자화(Bias) 때문인지, 아니면 다른 지역으로 이동해서 지구 자기장이 변한 것인지“를 공분산 $\mathbf{P}$ 행렬의 상관도를 바탕으로 지능적으로 분배하여 보정(Correction)한다.

3. 3D 벡터 퓨전과 헤딩(Heading) 퓨전 분기 로직

지자계 센서 데이터 융합에서 또 하나 눈여겨볼 로직은, 상황에 따라 3차원 전체 자기장 벡터(3D Vector)를 융합할지, 아니면 가상의 1차원 요(Yaw, Heading) 각도만을 추출하여 융합할지를 동적으로 스위칭하는 상태 머신(State Machine) 메커니즘이다.

3.1 3D 벡터 퓨전 (3D Vector Fusion)

기본적인 비행 중에는 3D 벡터 퓨전이 활성화된다. 즉, 앞서 설명한 것처럼 기체의 자기장 바이어스와 지구 지구장을 모두 추정하기 위해 X, Y, Z 세 축의 자기장 잔차를 앞선 절의 fuse() 템플릿 함수에 순차적으로 3번 밀어 넣는다 (스칼라 업데이트 기법).

3.2 헤딩(Heading) 퓨전 (1D Yaw Fusion)

반면, 지면에서 기체가 시동을 기다릴 때(On Ground)나 극심한 자기장 교란이 예상되는 이착륙 순간에는 지자계 데이터를 맹신하면 기체가 전복될 위험이 있다. 이때 코드 로직은 3D 퓨전을 즉시 중단하고, 자기장 데이터에서 2D 평면 방위각(Heading) 스칼라 값 하나만을 강제로 추출해 내어 단순화된 헤딩 퓨전(Heading Fusion) 모드로 강등(Fall-back)시킨다.

// src/lib/ecl/EKF/mag_control.cpp 의 제어 분기
if (_control_status.flags.mag_3D) {
    // 3차원 상태 관측 및 바이어스 추정 로직 수행
    fuseMag(); 
} else if (_control_status.flags.mag_hdg) {
    // 오름각/복각 편차를 무시하고 오직 평면 Yaw 제어력만 확보
    fuseHeading(); 
}

이러한 상태 전환 결정을 내리는 상위 제어 로직(자기장 간섭 회피 등)은 PX4 EKF 생존성의 핵심이며, 그 전환 조건에 대한 구체적인 파라미터 임계치와 고도/추력 기반의 제약 조건은 이어지는 하위 절들에서 나누어 분석된다.

4. 소결

PX4의 fuseMag() 함수는 자기장 센서 하나가 24차원 상태 벡터 중 무려 10개 이상의 변수들을 동시에 조율하게 만드는 가장 비선형적이고 파괴적인(?) 퓨전 노드다. 지자계 퓨전의 성공 여부는 곧 ’올바른 회전 행렬의 유지’를 뜻하며, 이는 GPS와 가속도계 데이터를 올바른 방향으로 투영시켜주는 대전제 구실을 한다.

이 복잡한 수학적 오케스트라가 불협화음을 낼 때(예컨대 철재 다리 밑을 지나갈 때) 필터가 어떻게 스스로를 방어하고 3D 퓨전과 1D 퓨전 사이를 넘나드는지, 그 정교한 스위칭 로직의 상세 구조를 다음 절에서 깊이 있게 타진해 본다.

27.5.3. 지자계 퓨전 (fuseMag)