13.4.2.1. `SENS_GPS_MASK` 기반 위치 혼합(Blending) 알고리즘 (`src/modules/sensors/vehicle_gps_position.cpp`)

다중 GPS 아키텍처에서 두 대의 수신기(Primary, Secondary)가 각기 다른 좌표를 보고할 때, 단순히 한쪽의 손을 들어주는 스위칭(Switching) 방식은 예기치 못한 타임아웃 딜레이와 위치 점프(Position Jump)를 유발할 위험이 농후하다. 이러한 한계를 극복하기 위해 PX4-Autopilot의 센서 융합 모듈은 최첨단 통계 기법인 가중 혼합(Weighted Blending) 알고리즘을 도입하였다.

본 절에서는 사용자가 SENS_GPS_MASK 파라미터를 통해 블렌딩 모드를 어떻게 활성화하는지 그 메커니즘을 알아보고, 실제 펌웨어를 구동하는 C++ 소스 코드(src/modules/sensors/vehicle_gps_position.cpp) 내부에서 이 혼합기(Blender) 로직이 어떤 수학적 필터링을 거쳐 최적의 단일 좌표를 EKF2 필터로 건네주는지 기술한다.

1. `SENS_GPS_MASK` 파라미터와 블렌드 모드 제어

PX4 시스템에서 다중 센서 간의 거버넌스(결정권)를 조율하는 역할은 EKF 모듈이 아닌 그 앞단의 sensors 모듈(src/modules/sensors)이 담당한다. 사용자는 SENS_GPS_MASK 비트마스크 파라미터를 조작하여 GPS 데이터들의 병합 방식을 정의한다.

0 (Disable): 다중 GPS를 사용하더라도 블렌딩하지 않고 오직 오차가 더 적은 한쪽 포트(Fallback)로만 스위칭한다.
Bit 0 (1) 활성화: 위치 혼합(Position Blending)을 허용한다.
Bit 1 (2) 활성화: 속도 혼합(Velocity Blending)을 허용한다.
Bit 2 (4) 활성화: Heading 블렌딩을 허용한다.

일반적으로 고성능 듀얼 RTK 구성 시 가장 유려한 비행성을 확보하기 위해서는 수치 $3$ (Bit 0 + Bit 1)를 세팅하여 위치와 3D 속도 파라미터를 동시에 혼합시키는 것이 권장된다.

2. 블렌드 알고리즘 C++ 아키텍처 분석

2.1 인스턴스 데이터의 수집과 정합 검사

vehicle_gps_position.cpp 의 메인 루틴은 uORB 상에 떠다니는 여러 개의 sensor_gps 인스턴스 버퍼들을 무한정 들여다보고(Polling) 있다. 하지만 무턱대고 모든 데이터를 섞지는 않는다. 혼합기(Blender)에 들어가기 전 자격 심사라는 깐깐한 관문을 거친다.

// 1. 블렌드에 참여할 다중 수신기의 자격 판별 유사 코드
bool is_gps_valid[2] = {false, false};
for (int i=0; i<2; ++i) {
    if (gps[i].fix_type >= 3 &&    // 최소 3D Fix 이상인가?
        gps[i].eph < eph_max &&    // 수평 오차(HDOP)가 허용치 이내인가?
        !is_timeout(gps[i])) {     // 최신 데이터인가?
        is_gps_valid[i] = true;
    }
}

만일 터널 진입 등의 이유로 2번 GPS가 일시적인 통신 음영에 빠져 데이터가 끊기거나 단독 측위( $Fix=3$ )로 강등당하면, C++ 로직은 즉각적으로 2번 데이터를 불량으로 간주하고 블렌딩 비율을 $100 : 0$ (Primary 단독 모드)으로 되돌려버린다.

2.2 로우패스 필터(Low-pass Filter)를 통한 가중치 계산 체계

두 GPS 모두 ‘유효(Valid)’ 판정을 받았다면 이제 본격적인 수학적 황금비 계산에 돌입한다. 블렌딩의 핵심은 ‘누구의 말을 더 신뢰할 것인가?’ 즉, 보고된 오방향 분산수치(Reported Variance)의 역수(Inverse)로 가중치를 부여하는 것이다. 이를 역분산 가중 평균(Inverse-Variance Weighting) 이라 칭한다. (상세 수학적 증명은 차절에서 다룸)

그런데 신뢰도 가중치( $W_1, W_2$ )가 매 프레임( $10\text{Hz}$ )마다 센서 노이즈로 인해 수시로 $60:40 \rightarrow 30:70$ 으로 급파도 치듯 변하면, 출력되는 혼합 좌표 역시 스파이크(Spike) 현상을 동반하게 된다.

이를 억제하기 위해 PX4 개발진은 가중치 변화율에 지수 평활법(Exponential Moving Average) 기반의 로우패스 필터 설정을 적용해 두었다.

// 2. 가중치(Weight)의 부드러운 전이(Sub-filtering)
float target_weight_1 = var_2 / (var_1 + var_2); // 역분산 가중치 산출

// 이전 프레임의 가중치에 LPF 적용 알고리즘
_blended_weight_1 = _blended_weight_1 * (1.0f - LPF_CONST) + target_weight_1 * LPF_CONST;
_blended_weight_2 = 1.0f - _blended_weight_1;

이로 인해 2번 GPS의 수신 상태가 점진적으로 나빠지더라도 타겟 지점에 적용되는 가중치 수치는 수 초에 걸쳐 부드럽게 감소하므로, 드론의 조종 면(Control Surface)에는 어떠한 날카로운 위치 튐 제어력(Jerk)도 들어가지 않게 된다.

2.3 최종 State 구조체 결합 및 Publish

가중치가 산출되면 비로소 sensor_gps 의 위상 데이터들이 선형 결합(Linear Combination)을 이룬다.

// 3. 최종 혼합 좌표와 혼합 분산치(Variance) 도출 연산
blended_gps.lat = (gps[0].lat * _blended_weight_1) + (gps[1].lat * _blended_weight_2);
blended_gps.lon = (gps[0].lon * _blended_weight_1) + (gps[1].lon * _blended_weight_2);
blended_gps.vel = (gps[0].vel * _blended_weight_1) + (gps[1].vel * _blended_weight_2);
...
// 결합된 불확실성(새로운 eph) 계산
blended_gps.eph = sqrt((gps[0].eph * _blended_weight_1)^2 + (gps[1].eph * _blended_weight_2)^2);

// uORB로 최종 Master GPS Topic 발행!
orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &blended_gps);

2.4 구조적 의의: EKF2를 보호하는 ‘방파제’ 역할

src/modules/sensors 디렉토리 아래에 이 거대한 블렌딩 로직이 존재하는 철학적 이유는 명확하다. 관제 및 상태 추정의 끝판왕인 EKF2 필터는 본연의 ‘멀티센서 칼만 필터 예측’ 코드만으로도 이미 수백 행에 달하는 매트릭스 수식을 연산하며 과부하에 시달린다.

여기에 “내 밑에 GPS가 두 개나 있는데 누구 걸 써야 하지?”, “갑자기 한쪽 선이 뽑혔어!” 같은 분기 예외 처리(Exception Handling) 로직을 EKF2 내부에 모두 몰아넣는다면, 시스템 복잡도는 기하급수적으로 높아지고 소프트웨어 안정성은 붕괴할 것이다.

따라서 vehicle_gps_position.cpp 는 EKF2라는 거장에 앞서 더러운 원시 데이터( $N$ 개의 GPS)들의 투쟁 구도를 수학적 통계(가중 평균 혼합)와 LPF(로우패스 필터)라는 무기로 정리 정돈하여, 단 하나로 완벽하게 포장된 vehicle_gps_position 구조체로 내어주는 고도로 추상화된 미들웨어 프록시(Middleware Proxy) 역할을 위대하게 수행하고 있는 셈이다.

13.4.2.1. SENS_GPS_MASK 기반 위치 혼합(Blending) 알고리즘 (src/modules/sensors/vehicle_gps_position.cpp)

1. SENS_GPS_MASK 파라미터와 블렌드 모드 제어