## 0.1 ECL/EKF2 (Estimator) 내 거리 및 플로우 센서 융합(Fusion) C++ 소스 코드 분석

센서 드라이버가 읽어들인 측정값(distance_sensor, optical_flow uORB)은 관성항법장치(IMU) 데이터와 함께 마침내 PX4의 심장부인 확장 칼만 필터(EKF2, Estimator Control Library) 컴포넌트로 빨려 들어간다. src/modules/ekf2 안에서의 융합 파이프라인(Fusion Pipeline)은 철저히 확률 통계학과 자코비안(Jacobian) 행렬 연산 기반의 C++ 코드로 구현되어 있다.

EKF2의 control_optical_flow() 및 control_range_finder() 융합 파이프라인

EKF2 소스코드 디렉토리 내 EKF/control.cpp 와 EKF/vel_pos_fusion.cpp를 추적해 보면, 로컬 센서가 필터 상태 벡터(State Vector)에 관여하는 핵심 관문 함수들을 만날 수 있다.
필터는 주기적으로 GPS, 기압계(Baro), 지자기(Mag) 센서와 더불어 옵티컬 플로우 신호 플래그(_flow_data_ready)와 거리 센서 플래그(_range_data_ready)를 체크한다.

1. 거리 센서(Range Finder) 융합 모듈 (fusion_range_finder.cpp):
   기체가 Roll(\phi)이나 Pitch(\theta)로 심하게 기울어져 고속 비행을 할 경우, 라이다는 바닥에 대각선으로 부딪혀 수직 고도(Z)보다 훨씬 먼 사선 거리(Slant Range)를 측정하게 된다. EKF2는 이를 곧이곧대로 믿지 않고 앞서 계산한 현재의 최신 자세 방향 코사인 행렬(DCM, Direction Cosine Matrix) 성분을 이용해 측정된 d 에 \cos(\phi) \cdot \cos(\theta) 와 같은 투영각 계수를 곱비산하여 순수 Z축 하방 고도(h)로 깎아내는 보정(Tilt Compensation) 루틴을 수행한 후 칼만 갱신(Update) 단계로 넘긴다.

2. 옵티컬 플로우(Optical Flow) 융합 모듈 (fusion_optical_flow.cpp):
   optical_flow 구조체를 넘겨받으면, EKF2는 단순히 들어온 신호를 반영하는 데 그치지 않고 사전에 설정된 품질 커트라인 파라미터 EKF2_OF_QMIN 값과 구조체 내의 quality 속성값을 비교(If Statement)한다. 만일 조명이 어두운 방이라 퀄리티가 임계치 밑으로 떨어지면 필터는 코인(이노베이션, Innovation) 계산 루틴을 강제 스킵시켜 해당 프레임을 버린다(Rejects).
   통과된 데이터는 앞서 설명한 방정식에 따라 자신의 관성 IMU 회전분(Rate)을 빼고 기체 예상 Z 위치 추정치를 곱하여 얻은 평면 속도 예측값 벡터(\hat{v}_x, \hat{v}_y)와 센서의 옵저베이션 평면 속도(v_x, v_y)의 차이(Innovation/Residual)를 구한 뒤, 공분산(Covariance) 매트릭스를 매개로 위치/속도 상태 행렬(State Matrix)을 단단하게 부여잡는(Correction) C++ 행렬 계산식(H * P * H^T + R)을 순차 전개한다.