27.6.1.1. 감지된 센서 개수(IMU 및 지자계 조합)에 따른 추정기 클래스 배열 초기화 및 워크 큐 할당

27.6.1.1. 감지된 센서 개수(IMU 및 지자계 조합)에 따른 추정기 클래스 배열 초기화 및 워크 큐 할당

본 소절에서는 EKF2 래퍼 모듈이 하드웨어 계층으로부터 전달받은 관성 측정 장치(Inertial Measurement Unit, IMU) 및 지자기 센서(Magnetometer)의 수량 매핑을 바탕으로, 동적인 확장 칼만 필터(EKF) 클래스 배열을 초기화하고 이를 단일 워크 큐(Work Queue) 모델 내에 할당하는 논리적 과정을 코드 레벨에서 상세히 분석한다.

1. 다중 센서 조합(Sensor Combination) 논리 기반 배열 초기화

PX4-Autopilot은 비행 제어기(Flight Controller)의 운영체제 부팅 단계에서 직렬 통신 버스(SPI, I2C, CAN 등)를 스캔하여 가용한 하드웨어 센서 트리를 식별한다. 이후 EKF2 제어 모듈은 파라미터 EKF2_MULTI_IMU 및 EKF2_MULTI_MAG의 시스템 설정값을 바탕으로 다중 추정기 생성 모델을 구축한다.

물리적으로 n개의 IMU와 m개의 지자계가 혼재된 시스템의 경우, 연산 자원을 극도로 소모하는 모든 순열 조합(n \times m)의 EKF를 무분별하게 구동하지 않는다. 대신, 플랫폼은 성능과 신뢰성의 최적화 관점에서 주(Primary)로 사용될 기준 센서의 배열을 선별 할당한다. EKF2 클래스의 내부 변수인 _ekf_instances는 구조적으로 EKF 클래스 객체를 가리키는 포인터 배열(Array of Pointers) 또는 리스트 형태로 관리되며, 시스템은 감지된 유효 IMU 개수를 기준으로 루프를 순회하여 배열의 각 슬롯(Slot)에 독립된 객체를 동적으로 할당(new)한다.

// 다중 EKF 인스턴스 초기화 로직 (src/modules/ekf2/EKF2.cpp 개념 모델)
void EKF2::allocate_instances() {
    // 논리적 최대 인스턴스 수와 물리적 IMU 수량의 최소값을 획득
    uint8_t num_instances = math::min(_detected_imus, EKF2_MAX_INSTANCES);
    
    for (uint8_t i = 0; i < num_instances; i++) {
        // 배열의 슬롯(Slot)에 독립적인 칼만 필터 인스턴스 동적 할당
        _ekf_instances[i] = new EKF();
        
        // i번째 인덱스에 해당하는 IMU 센서를 i번째 EKF 코어의 메인 IMU로 강제 매핑
        _ekf_instances[i]->set_imu_id(get_imu_id_from_index(i));
        
        // 지자기 센서는 가용한 여유 자원에 따라 분배 (예: Round-Robin 매핑)
        _ekf_instances[i]->set_mag_id(get_mag_id_from_index(i % _detected_mags));
        
        // 각 인스턴스 고유의 상태 벡터(State Vector) 및 공분산 행렬 초기화
        _ekf_instances[i]->init();
    }
}

이와 같은 센서 매핑 논리는 특정 센서 노드에 하드웨어 결함이나 전원 차단 부재가 발생하더라도, 배열 내 다른 인스턴스들이 분리된 독립 센서 데이터를 바탕으로 추정을 지속하게 하여 전역적인 공분산 발산(Covariance Divergence)을 억제하는 원스텝 데이터 격리(One-Step Data Isolation) 효과를 제공한다.

2. 워크 큐(Work Queue) 스케줄링 및 단일 틱(Tick) 할당 아키텍처

로봇 운영 환경(ROS 2)이나 다중 코어 기반의 분산 아키텍처를 채택하는 일부 상용 GCS 관제 환경과 달리, PX4의 기반 실시간 운영체제(RTOS)인 NuttX 환경 하에서는 리소스의 효율성과 스케줄링 타이밍 보장을 위해 단일 워크 큐(Work Queue) 모델 기법이 널리 채택된다.

EKF2 래퍼 모듈은 다수의 개별 EKF 인스턴스별로 POSIX 스레드(Pthread)를 분리하여 생성하는 방식을 채택하지 않는다. 무분별한 스레드의 생성은 심각한 컨텍스트 스위칭(Context Switching) 지연과 메모리 단편화 문제를 유발하기 때문이다. 대신, 구조체 내에 포함된 px4::WorkItem 인스턴스로서 단일 Run() 콜백 함수를 등록하고, 운영체제의 스케줄러가 일정한 베이스 틱(Base Tick)마다 이 스레드를 호출하도록 프로그래밍되어 있다.

graph TD
    subgraph NuttX RTOS
        WQ[Work Queue Thread\npx4_work_queue]
    end

    subgraph EKF2 Wrapper Module
        Task[EKF2::Run() Callback Task]
    end

    subgraph Instances [EKF Instances Array]
        Inst0[EKF Instance 0 Update & Predict]
        Inst1[EKF Instance 1 Update & Predict]
        Inst2[EKF Instance 2 Update & Predict]
    end

    WQ -->|Scheduled Trigger\ne.g., 250Hz / Every 4ms| Task
    Task -->|Sequential Tick| Inst0
    Task -->|Sequential Tick| Inst1
    Task -->|Sequential Tick| Inst2

이러한 단일 스레드-다중 인스턴스 구동 설계의 학술적 이점은 다음과 같이 입증된다.

  1. 상호 배제(Mutual Exclusion) 오버헤드 최소화: 루프 기반의 단일 스레드 내에서 배열을 순차 진행하므로, 자원 선점을 위한 불필요한 뮤텍스(Mutex) 또는 세마포어(Semaphore) 락(Lock) 없이 동시성 연산이 가능하다.
  2. 결정론적 실행 타이밍 보장(Deterministic Timing): 센서 융합 간 시간 지연(Latency)의 변동(Jitter)을 방지하여 필터 지연 버퍼 및 보상 메커니즘이 안정적으로 연산 결과물(Innovation)을 산출하도록 지원한다.

결과적으로 PX4의 개별 인스턴스 배열 초기화 기법과 단일 워크 큐 스케줄링 간의 조화는, 하드웨어 이중화가 제공하는 안전성과 임베디드 시계열 환경 제약 간의 완벽한 수학적, 소프트웨어적 균형을 이룬 핵심 설계 사상이라 해석할 수 있다.