27.5.3.1. 3차원 벡터 퓨전과 헤딩 퓨전 간의 전환 상태 머신 로직

27.5.3.1. 3차원 벡터 퓨전과 헤딩 퓨전 간의 전환 상태 머신 로직

앞 절에서 살펴본 바와 같이 지자계(Magnetometer) 센서는 3차원 벡터 데이터를 EKF에 제공한다. 하지만 PX4-Autopilot은 이 3차원 데이터를 맹목적으로 모두 신뢰하지 않는다. 비행체의 현재 상태(상공 비행 중인지, 지상에 대기 중인지)와 자기장 교란의 발생 여부에 따라 EKF는 지능적으로 관측 행렬(\mathbf{H})의 차원을 축소하거나 확장하는 상태 머신(State Machine) 을 가동한다. 본 절에서는 ecl/EKF/mag_control.cpp 내에 구현된 ’3차원 벡터 퓨전(3D Vector Fusion)’과 ‘헤딩 퓨전(Heading Fusion)’ 간의 전환 논리와 C++ 소스 코드 레벨의 스위칭 메커니즘을 심도 있게 분석한다.

1. 3차원 벡터 퓨전 (3D Vector Fusion)의 원리와 한계

정상적인 비행 상태(In-flight)에서 PX4 EKF2는 3D 벡터 퓨전 모드로 작동한다. 이 모드에서는 지자계 센서가 측정한 X, Y, Z 세 축의 가우스(Gauss) 값을 모두 EKF 관측 모델에 밀어 넣는다 (fuseMag() 호출).

  • 목적: 기체의 요(Yaw) 방위각뿐만 아니라, 기체 내부에 존재하는 하드웨어적 자기장 편향(Body Magnetic Bias)과 비행 지역의 지구 자기장 편차(Earth Magnetic Field)까지 역산출(Estimation)하기 위함이다.
  • 작동 방식: EKF 상태 벡터 16~21번 인덱스를 활성화하여 3차원 공간의 자기장 왜곡을 실시간으로 추정 및 보상한다.

한계점: 3D 벡터 퓨전을 안정적으로 수행하려면 역설적으로 기체가 상공에서 충분히 움직여 주어야 한다(가속도 및 선회 기동). 기체가 지상(On-ground)에 가만히 맴돌고 있거나, 철강으로 된 다리미나 철골 구조물 위에 놓여 있을 경우, EKF는 일시적인 자기장 교란을 ’지구 자기장의 변화’로 오인하여 필터 상태를 심각하게 망가뜨릴(Divergence) 위험이 있다.

2. 헤딩 퓨전 (1D Heading Fusion)으로의 강등(Fall-back)

이러한 지상 대기 상태에서의 한계를 극복하기 위해 PX4는 헤딩 퓨전 모드라는 안전장치를 마련해 두었다.

  • 동작 원리: 3D 자기장 벡터 전체를 신뢰하지 않는다. 대신, 현재 기체의 롤(Roll)과 피치(Pitch) 각도(가속도계로 구한 안정적인 값)를 이용하여 3차원 자기장 벡터를 수평 2D 평면으로 역투영(Projection)한다. 이렇게 얻어낸 2차원 자기장 벡터에서 삼각함수 atan2()를 통해 1차원의 요 각도(Yaw, Heading)스칼라 값만을 추출해 낸다 (fuseHeading() 호출).
  • 특징: 기체 자기장 바이어스나 지구 자기장 수직 복각(Inclination) 데이터의 학습을 완전히 포기한다. 오로지 2차원 평면상의 나침반 방향만을 EKF에 주입하여, 이륙 전 기체가 뒤집히거나 요 축이 발산하는 것을 막는 방어적 로직이다.

3. 전환 상태 머신(State Machine)의 C++ 로직 열람

mag_control.cpp 파일 내부의 제어 루프를 들여다보면, 특정 조건이 만족할 때마다 플래그(Flag)를 토글(Toggle)하며 퓨전 모드를 넘나드는 전형적인 상태 머신 패턴을 확인할 수 있다.

stateDiagram-v2
    [*] --> Heading_Fusion : 부팅 및 지상(On-ground) 대기
    
    Heading_Fusion --> 3D_Vector_Fusion : 이륙(In-air) 및 \n충분한 기동시간 경과
    
    3D_Vector_Fusion --> Heading_Fusion : 착륙(Landed) 감지
    3D_Vector_Fusion --> Heading_Fusion : 심각한 자기장 교란 지속(Anomaly)
    
    3D_Vector_Fusion --> Stop_Mag_Fusion : 3D, Heading 모두 기각 상태 시 \n(지자계 완전 배제)

이 상태 전환 과정은 C++ 코드에서 다음과 같은 조건식들로 세밀하게 통제된다.

// src/lib/ecl/EKF/mag_control.cpp 의 의사코드 (상태 전환 로직)

// 1. 현재 지상에 있는지 상공에 있는지 플래그 획득
bool is_on_ground = _control_status.flags.in_air ? false : true;

// 2. 파라미터로 설정된 퓨전 타입 획득 (EKF2_MAG_TYPE)
bool mag_use_3d = (_params.mag_fusion_type == MAG_FUSE_TYPE_3D);

if (mag_use_3d && !is_on_ground) {
    // 3A. 비행 중(In-air)일 경우: 3D 벡터 퓨전 모드 활성화 여부 조율
    if (!_control_status.flags.mag_3D) {
        // 이전에 Heading 모드였다면 3D 상태 변수들을 재초기화(Reset)한 후 전환
        resetMagStates();
        _control_status.flags.mag_hdg = false;
        _control_status.flags.mag_3D = true;
    }
} else {
    // 3B. 지상(On-ground)이거나 3D 퓨전이 비활성화된 경우: Heading 모드로 폴백
    if (_control_status.flags.mag_3D) {
        // 3D 퓨전 플래그를 내리고 다시 Heading 모드로 강등
        _control_status.flags.mag_3D = false;
    }
    _control_status.flags.mag_hdg = true;
}

// 4. 실행 계층 (결정된 플래그에 따라 실제 퓨전 함수 호출)
if (_control_status.flags.mag_3D) {
    fuseMag();      // 3차원 야코비안 연산 실행
} else if (_control_status.flags.mag_hdg) {
    fuseHeading();  // atan2를 이용한 1차원 스칼라 업데이트 실행
}

3.1 상태 전환 시의 충격 완화 (Reset & Alignment)

상태 머신이 Heading_Fusion 에서 3D_Vector_Fusion 으로 절체(Switching)되는 순간, 관측 차원이 1차원에서 3차원으로 급변한다. 아무런 하강 조치 없이 모드를 전환할 경우 수학적 불연속성에 의해 EKF 상태 변수가 심하게 튀게 된다(State Jump).

이를 방지하기 위해 PX4는 모드 전환 직전 resetMagStates() 함수를 호출한다. 이 함수는 현재의 Heading 값을 역으로 대입하여 3차원 지구 자기장(mag_I)의 초기 추정값을 다시 정렬(Alignment)함으로써 파이프라인 전환 과정에서의 충격을 매끄럽게 흡수한다.

4. 소결

PX4 EKF2의 지자계 퓨전 로직은 수학 공식을 코드로 옮긴 것을 넘어, 산업 현장에서 불규칙하게 발생하는 자기장 외란에 맞서는 치열한 방어 논리다. 3D 벡터 퓨전과 헤딩 퓨전 간의 스위칭 상태 머신은 지상 대기 시의 안전성을 보장하고, 이륙 후에는 칼만 필터의 잠재력을 최대한 끌어올리기 위한 지능적인 타협점이다.

다음 절에서는 이 상태 머신이 단순히 이착륙(In-air/On-ground) 여부만을 따지는 것을 넘어, 모터 구동 시 발생하는 추력(Thrust) 전류 데이터와 어떻게 연동되어 능동적으로 자기장 간섭을 회피하는지 그 고도화된 연계 로직을 살펴본다.