### 0.0.1 `ekf2_gps_blending.cpp` 분석: 속도(Velocity) 및 위치 오차 분산(Variance) 역수에 비례하는 가중치($W_1, W_2$) 연산 알고리즘

### 0.0.1 ekf2_gps_blending.cpp 분석: 속도(Velocity) 및 위치 오차 분산(Variance) 역수에 비례하는 가중치(W_1, W_2) 연산 알고리즘

PX4-Autopilot의 센서 융합 모듈, 특히 EKF2 내부에서 다중 GPS 센서의 운명을 가름하는 수학적 판사격인 파일은 src/modules/ekf2/ekf2_gps_blending.cpp (혹은 최신의 센서 허브 아키텍처 파일)이다.

이 모듈의 핵심 임무는 단순명료하다. 매 업데이트 사이클마다 두 GPS가 보고하는 “내 데이터는 이만큼 부정확할 수 있어요“라는 공산(Variance) 지표를 바탕으로, 두 데이터를 버무릴 황금비율, 즉 블렌딩 가중치(Blending Weight, W) 를 도출해 내는 것이다.

0.1 분산(Variance, \sigma^2)의 수집과 튜닝(Tuning)

GPS 수신기는 uORB 토픽(sensor_gps)을 통해 자신의 건강 상태를 나타내는 여러 정확도(Accuracy) 지표를 송출한다.

  • eph (Expected Position Horizontal): 수평 위치 오차의 추정치(m)
  • epv (Expected Position Vertical): 수직 위치 오차의 추정치(m)
  • s_variance_m_s (Speed Variance): 3차원 속도 벡터의 오차 분산 (m/s)

블렌딩 알고리즘은 이 값들을 그대로 날것(Raw)으로 쓰지 않는다. 하드웨어 제조사 특성상 오차 추정치가 지나치게 낙관적이거나 비관적일 수 있기 때문에, PX4는 EKF2_GPS_POS_X, EKF2_GPS_VEL_X 와 같은 노이즈 게인(Gain) 파라미터를 곱하여 분산 값을 한 번 스케일링(Scaling)한다. 이를 통해 동일한 필드 위에서 두 센서가 공정하게 비교될 수 있는 단일한 종합 분산 지표(V_{blend}) 를 산출해 낸다.

0.2 가중치 연산의 수학적 전개: 역분산(Inverse-Variance) 방정식

통계학적으로 가장 오차가 적은 최적의 평균값을 도출하는 해법은 분산의 역수에 비례하여 가중치를 할당하는 것이다. 신뢰도(정확도)는 곧 에러 분산의 역수 \left(\frac{1}{\sigma^2}\right) 와 같기 때문이다.

소스 코드 내부의 가중치 산출 로직은 다음 수식처럼 전개된다.

  1. 각 센서별 가중 파라미터(Weight Parameter) 계산:
    P_1 = \frac{1}{\max(V_{blend,1},\ \epsilon)}
    P_2 = \frac{1}{\max(V_{blend,2},\ \epsilon)}
    여기서 \epsilon 은 1e-6 등 분모가 ’0’이 되는 치명적인 Divide-by-Zero 에러를 막기 위한 아주 작은 보호 상수(Safeguard Constant)다.

  2. 정규화(Normalization)를 통한 가중치(W_1, W_2) 도출:
    W_1 = \frac{P_1}{P_1 + P_2}
    W_2 = \frac{P_2}{P_1 + P_2}
    이를 통해 W_1 + W_2 = 1.0 (100%) 이 되는 정규화된 0.0 ~ 1.0 사이의 가중치 계수를 획득한다.

0.3 코드 트레이싱: blend_gps_data() 역학

실제 C++ 코드 내부의 blend_gps_data() 함수를 들여다보면, 확보한 가중치 W_1 과 W_2 를 사용하여 두 개의 sensor_gps 구조체를 하나의 _gps_blended 구조체로 물리적으로 압제(Flatten)하는 과정이 펼쳐진다.

  • 위도(lat)와 경도(lon) 같은 10^7 스케일의 int32 변수들은 스케일을 벗기지 않기 위해 배정밀도 실수(Double Precision, double)로 캐스팅된 뒤, W_1 과 W_2 가 곱해져 더해지고 다시 int32로 강제 형변환(Casting)된다.
  • 고도(alt, alt_ellipsoid), 3축 속도(vel_n_m_s, vel_e_m_s, vel_d_m_s), 그리고 방위각(cog_rad)까지 모든 연속적인(Continuous) 물리량 데이터가 동일한 가중 가산 알고리즘 계통을 통과하며 융합된다.
  • 단, 위성 개수(satellites_used), 픽스 타입(fix_type - 예: 3D Fix, RTK Float) 같은 불연속적인 이산 변수(Discrete Variable)나 논리 플래그(Flag)들은 평균을 낼 수 없으므로, 두 센서 중 W 가중치가 더 높은(즉, Primary-like) 센서의 값을 그대로 상속(Copy)받도록 강제 설계되어 있다.

이처럼 ekf2_gps_blending.cpp의 철저한 역분산 수학 모델은 노이즈가 섞인 이질적인 관측치들을 매끄러운 단 하나의 좌표 평면으로 봉합해 내는 센서 융합의 백미를 보여준다.