13.4.3.1. 지자기 센서 비활성화 조건(`SYS_HAS_MAG=0`, `EKF2_MAG_TYPE=None`)에서의 요(Yaw) 상태 변수(State Variable) 초기화 소스코드 분석

Dual Antenna GPS 기반의 정밀 Heading(Yaw) 추정 시스템을 탑재한 멀티로터나 헬리콥터 플랫폼에서, 자기장 간섭을 원천 차단하기 위해 지자기 센서(Magnetometer)를 비활성화하는 접근법은 매우 대중적이다. 기체 프레임 레벨의 SYS_HAS_MAG=0 파라미터나 상태 추정기 레벨의 EKF2_MAG_TYPE=None (값 0) 설정은 기체를 자력망 구속에서 해방시킨다.

그러나 EKF2(Extended Kalman Filter) 알고리즘 입장에서 **“초기 요(Initial Yaw) 각도의 캘리브레이션 부재”**라는 큰 딜레마를 마주하게 된다. 지자기 센서가 없으면, 전원을 막 인가한 직후의 기체는 자신이 동전을 향하고 있는지 북쪽을 향하고 있는지 알 방도가 없다. 본 절에서는 지자기 센서가 완전히 비활성화된 조건에서 EKF2 소스 코드(src/lib/ecl/EKF/)가 어떻게 초기 요(Yaw) 상태 변수(State Variable)를 설정하고 GPS Heading 데이터가 도착하기 전까지 시스템을 대기(Wait)시키는지 심층 분석한다.

1. 지자기 센서 소실에 대한 EKF2의 기본 태도

통상적인 부팅 시퀀스에서 EKF는 가속도계 데이터로 평형(Roll/Pitch)을 잡고, 3D 나침반 데이터 성분으로 요(Yaw) 각도를 정렬(Alignment)한다. 그러나 사용자가 매개변수 조작을 통해 나침반을 명시적으로 꺼 두었을 경우, 메인 초기화 루틴은 완전히 다르게 분기된다.

// src/lib/ecl/EKF/ekf.cpp (EKF 초기화 유사 로직)
if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
    // 지자기 센서를 융합하지 않음.
    _control_status.flags.mag_heading = false;
    _control_status.flags.mag_3D = false;
}

이 상태에서 C++ 로직은 쿼터니언(Quaternion) 상태 벡터의 초기값을 설정해야 한다. 자북(Magnetic North)을 알 수 없으므로, EKF는 기체가 전원을 켤 당시의 임의의 기수 방향을 일단 **진북(True North, 요 각도 $0^\circ$ )으로 임시 가정(Dummy Alignment)**하거나 $0$ 의 값으로 쿼터니언을 초기화한다.

$q_{init} = [1, 0, 0, 0]^T \quad (\text{Yaw} = 0^\circ)$

2. 요(Yaw) 상태 변수(State Variable)의 동적 초기화 대기(Wait) 로직

초기화 이후 기체는 Yaw = 0 인 상태로 루프를 돌고 있지만, 방위가 불확실하므로 네비게이션 모드(Position Control/Mission)로의 진입은 철저히 거부된다(Reject). EKF 코어 안에는 이 “진짜 방위를 기다리는 상태“를 나타내는 내부 플래그가 존재한다.

진짜 방위는 오직 GPS가 3D Fix를 넘어선 RTK Fixed(혹은 충분한 Heading 수렴 상태)가 되고, 두 대의 안테나가 산출한 sensor_gps.heading 데이터가 uORB 버스로 들어올 때 비로소 결정된다.

// src/lib/ecl/EKF/gps_yaw_fusion.cpp (초기 정렬 대기 제어)
if (!_control_status.flags.yaw_align && _gps_sample_delayed.heading_offset >= 0.0f) {
    // 아직 Yaw가 정렬되지 않았고, GPS로부터 유효한 헤딩 정보가 도착했다면!
    
    // 1. 초기 상태 변수 리셋(Reset) 함수 호출
    resetQuatStateYaw(_gps_sample_delayed.heading, _gps_sample_delayed.heading_accuracy);
    
    // 2. 정렬 완료 플래그 활성화
    _control_status.flags.yaw_align = true;
}

2.1 `resetQuatStateYaw` 의 수학적 의미

이 C++ 함수는 단순히 각도 변수 하나를 대입하는 작업이 아니다.
EKF의 24차원 상태 벡터( $x$ ) 중 쿼터니언 부분을 뜯어고치는 과감한 ’수술’이다. 기체의 Roll/Pitch 평형을 유지하면서(보통 자이로와 가속도계 수렴이 끝난 상태) 요(Yaw) 회전 각도만을 GPS가 제공하는 ’진북 기준 절대각’으로 강제로 덮어쓴다.

이와 동시에 상태 오차 공분산 행렬( $P$ )의 요 각도 분산 영역을 넓게 리셋하여, 필터가 “방금 강제로 맞춘 각도니까, 새로운 GPS 데이터들의 추이를 지켜보면서 서서히 최적화하겠다“는 수학적 유연성을 확보하도록 조치한다.

3. 이동 기반 정렬(Kinematic Alignment)의 최후 백업

만약 Dual Antenna 시스템 마저도 일시적 초기화 불량으로 sensor_gps.heading 을 내뱉지 못한다면 어떡할 것인가?
지자기 센서도 없고, GPS 헤딩도 없는 진퇴양난의 상황에서, 기체가 이륙을 감행하거나 수동 조종으로 이동하기 시작하면 EKF2는 최후의 백업 스크립트인 운동학적 정렬(Kinematic Alignment), 속칭 “날아가면서 정렬하기” 옵션을 발동한다.

// 비행 중 위치 변화를 통한 Yaw 추정 
if (moving_speed > 5.0f && !_control_status.flags.yaw_align) {
     // 기체가 일정 속도 이상 전진 중이면
     // GPS 위치 인벤토리의 궤적(Course Over Ground) 벡터 연산
     float course_yaw = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
     resetQuatStateYaw(course_yaw, calculated_variance);
}

한계: 이 방식은 전진 속도가 존재해야만 동작하므로, 제자리 호버링(Hovering) 중에는 요 각도가 점진적으로 미끄러져 추락에 이를 수 있다.

4. 커맨더(Commander) 모듈의 전제조건 시퀀스

C++ 펌웨어 아키텍처 관점에서, 이 EKF2의 내부 사정은 기체의 스위치를 켜는 Commander 로 전파되어 조종사의 인가 로직을 차단한다.
커맨더 모듈은 내부 루프에서 지속적으로 EKF의 local_position_valid 및 yaw_valid (혹은 yaw_align 상태) 플래그를 모니터링한다.

지자기 센서가 없는 상태에서 듀얼 GPS 록(Lock)이 걸리고 EKF 내부에서 resetQuatStateYaw 함수가 정상적으로 실행되어 yaw_align = true 가 되기 전까지, 무선 조종기(Radio)로 아무리 시동(Arming) 스틱을 당겨도 QGC 화면에는 “Preflight Fail: Yaw alignment pending” 혹은 “Estimator not flying checks failed” 와 같은 처절한 텍스트 경고문만이 출력될 뿐 모터는 절대 회전하지 않는다.

결과적으로 PX4 소스 코드는 외부 센서 의존성을 낮추는 하드웨어 추상화를 감행(나침반 비활성화)하면서도, 상태 방정식 내부에서는 단 하나의 GPS 헤딩 시그널이 도달하기만을 숨죽여 기다리다가 단호하게 상태 구조를 뒤엎어버리는 엄격하고 보수적인 안정성 트리(Stability Tree)를 구축하고 있는 셈이다.

13.4.3.1. 지자기 센서 비활성화 조건(SYS_HAS_MAG=0, EKF2_MAG_TYPE=None)에서의 요(Yaw) 상태 변수(State Variable) 초기화 소스코드 분석