13.4.1.3. EKF2_GPS_CHECK 파라미터: RTK Fix/Float 전환 시 혁신 분산(Innovation Variance) 검정 게이트(Gate) 크기 조절을 통한 위치 튐(Position Jump) 필터링
RTK(Real-Time Kinematic) GNSS 시스템은 도심지나 산악 지형 같은 열악한 전파 환경(Multipath Environment)에서 양날의 검으로 돌변할 수 있다. 완벽한 환경에서는 센티미터(\text{cm}) 단위의 RTK Fixed 상태를 유지하며 드론을 레일 위에 올려놓은 듯 제어하지만, 3\text{G} / 4\text{G} 기지국 데이터가 끊기거나 교량 하부로 진입하는 순간 RTK Float, 심할 경우 일반 3D Fix 모드로 강등(Fallback)된다.
이러한 수신 상태의 급격한 전환(Mode Transition) 시, GPS가 보고하는 ’절대 좌표’가 순간적으로 1 \sim 2\text{ 미터} 이상 튀어버리는 위치 튐(Position Jump) 현상이 발생하게 된다. 본 절에서는 PX4-Autopilot의 EKF2가 이러한 극단적인 센서 노이즈 스파이크를 어떻게 걸러내는지, 그 핵심 방어 기제인 EKF2_GPS_CHECK 비트마스크 파라미터와 혁신 분산 검정(Innovation Variance Gate Test) 수학적 필터링 로직을 심층적으로 분석한다.
1. 혁신(Innovation)과 게이트(Gate)의 수학적 개념
칼만 필터(Kalman Filter)의 작동 원리를 이해하기 위해서는 ’혁신(Innovation)’이라는 용어를 정의해야 한다.
- 혁신(Innovation, y): 외부 센서(GPS)가 방금 보내온 새로운 측정값(z)과 필터가 자체적으로 (IMU 데이터를 적분하여) 예측하고 있던 예상값(Hx) 간의 차이이다. (y = z - Hx)
만약 GPS가 평소처럼 안정적이라면 이 차이값(y)은 매우 작을 것이다. 그러나 RTK Fixed가 풀리면서 좌표가 한순간에 2\text{ 미터} 튀어 나갔다면, 필터 내부에서는 y = 2.0\text{ m} 라는 엄청난 혁신값이 도출된다.
이때 EKF2는 이 거대한 값을 무턱대고 상태 벡터(x)에 반영하지 않는다. 대신 **게이트 검정(Gate Test)**을 수행한다.
- 이 혁신값(y)이 필터가 스스로 허용하고 있는 불확실성(Covariance, S = HPH^T + R) 범위 내에 들어오는가?
- 수학적으로, y^T S^{-1} y 라는 검정 통계량이 특정 임계값(Threshold, 게이트 크기)을 초과하는가?
초과한다면, EKF2는 이 GPS 측정치를 ’에러 수치(Outlier)’로 간주하고 칼만 이득(Kalman Gain) 적용을 전면 거부(Reject)한다.
2. EKF2_GPS_CHECK 비트마스크 파라미터 해부
EKF2_GPS_CTRL (어떤 센서를 쓸 것인가)과는 다르게, EKF2_GPS_CHECK 는 “센서를 쓸 때 얼마나 가혹한 잣대(Gate)로 품질을 검사할 것인가?” 를 지정하는 방어적 튜닝 파라미터이다.
총 9개의 비트 플래그로 이루어져 있으며, RTK 시스템 도입 시 특히 중요한 비트는 다음과 같다.
| 비트 인덱스 | 10진수 값 | 검사 항목 | 기능 설명 |
|---|---|---|---|
| Bit 0 | 1 | Fix Type | 최소 3D Fix 이상의 위성 신호 상태를 요구한다. |
| Bit 1 | 2 | Num Satellites | 최소 연결 위성 수(통상 파라미터 설정값 6 \sim 7개 이상)를 강제한다. |
| Bit 2 | 4 | PDOP | 위치 정밀도 저하율(Position Dilution of Precision)의 상한선을 점검하여 위성 기하학적 배치가 엉망일 때 센서를 기각한다. |
| Bit 6 | 64 | Speed Error | GPS가 보고한 속도 오차 분산(s_variance_m_s)이 한계치를 넘어가면 기각한다. |
이 플래그들이 활성화되어 있으면, EKF2 메인 루프인 Ekf::controlGpsFusion() 블록 진입 전에 Ekf::gps_is_good() 함수가 실행되어 사전에 정의된 물리적 허용선명(예: 위성 6개 미만 등)을 넘었을 때 GPS 데이터의 유입 밸브를 원천 봉쇄하게 된다.
3. RTK Fix/Float 전환 시의 위치 튐(Position Jump) 필터링 C++ 로직
드론이 나무 아래로 들어가면서 RTK 신호 상태가 Fixed(6) \rightarrow Float(5) \rightarrow 3D(3) 로 추락하는 상황을 코드 레벨에서 시뮬레이션해보자.
- RTK 상태 다운그레이드 감지:
gps/devices드라이버는 즉각sensor_gps토픽의fix_type과 오차 분산치(eph,epv)를 수십 배 뻥튀기시켜 Publish 한다. - 공분산(Variance) 증가: EKF2 메인 루프는 이 커진
eph(Measurement Noise, R) 값을 받아들여, 내부 혁신 공분산 행렬(S)의 크기를 대폭 늘린다. - 게이트 크기의 확장(Gate Expansion): S 값이 커졌다는 것은, 필터가 “이제부터 들어오는 GPS 데이터는 신뢰도가 낮으므로(Float 모드니까), 측정값들이 평소보다 넓은 반경으로 튀더라도 정상 범위로 인정하겠다“라며 포용력을 넓힌 상태를 의미한다.
[혁신 기각 (Innovation Rejection) 방어 로직]
그러나 만약 이 게이트가 확장되는 속도보다 수신 좌표가 ‘튀는’ 간격이 찰나의 순간에 과도하게 크게 발생했다면? (예: 1 틱(Tick, 0.01\text{초}) 만에 좌표가 3\text{m} 날아감)
// src/lib/ecl/EKF/gps_control.cpp 유사 로직
// 혁신 벡터(innov)가 허용 게이트(gate_size)를 돌파했는가?
if (innov_x * innov_x / innov_var_x > _gps_pos_gate * _gps_pos_gate) {
// 혁신 검증 실패: 이 GPS 데이터 조각은 Outlier(아웃라이어)로 판명!!
_gps_pos_fusion_rejected = true;
// 칼만 보정(Correction) 건너뜀
return;
}
이 방어벽(_gps_pos_fusion_rejected)에 걸리는 순간, EKF2는 GPS 데이터를 무시(Bypass)한다. 드론의 제어 루프 입장에서는 그 순간 GPS가 꺼진 것과 동일하므로, IMU 데이터만을 이용해 관성으로 기존의 부드러운 궤적을 억지로 이어 나간다(Dead-reckoning).
이러한 거부가 5\text{초} 간 지속되면, 그때서야 필터는 “아예 GPS 원점이 영구적으로 이동했구나“라고 판단하고 상태 벡터 내의 GPS Offset 값을 리셋(Reset)하여 새로운 위치 좌표 체계로 부드럽게 재융합(Re-fusion)을 시작한다.
결론적으로 EKF2_GPS_CHECK 통과, 그리고 ‘Innovation Variance Gate’ 라는 이중 삼중의 필터 방어망 덕분에, 최신 PX4 기체는 RTK Fix 상태가 풀리는 격변하는 통신 단절의 순간에도 미친 듯이 옆으로 홱 틀어박히지(Position Jump Fly-away) 않고, 관성 센서에 기대어 매끄러운 제어 연속성(Control Continuity)을 유지할 수 있는 것이다.