Chapter 27. Estimator(ECL/EKF2) 아키텍처 및 소스 코드 분석

• 27.1. 상태 추정(State Estimation)과 EKF의 수학적 토대
• 27.1.1. 이산 시간(Discrete-time) 확장 칼만 필터 알고리즘 구조
• 27.1.1.1. 비선형 동역학 함수 및 관측 함수의 정의와 물리적 의미
• 27.1.1.2. 예측 단계: 사전 상태 추정 및 오차 공분산 예측 연산의 흐름
• 27.1.1.3. 혁신(Innovation) 도출 및 혁신 공분산 산출 과정
• 27.1.1.4. 관측 신뢰도에 따른 최적 칼만 게인(Kalman Gain) 산출 알고리즘
• 27.1.1.5. 보정 단계: 사후 상태 업데이트 및 공분산 업데이트 로직
• 27.1.2. EKF 내 편미분 및 야코비안(Jacobian) 행렬 연산
• 27.1.2.1. 시스템 선형화를 위한 상태 전이 야코비안 행렬 도출 방법론
• 27.1.2.2. 센서 특성을 반영한 관측 야코비안 행렬 도출 방법론
• 27.1.2.3. 파이썬 기호 연산 라이브러리(SymPy)를 활용한 C++ 야코비안 코드 자동 생성 원리
• 27.2. PX4 ECL(Estimation and Control Library) 아키텍처 및 빌드 시스템
• 27.2.1. 펌웨어 독립적 ECL 코어 설계
• 27.2.1.1. 코어 필터링 알고리즘 분리 및 C++ 표준 라이브러리 사용 최소화 원칙
• 27.2.1.2. 단위 테스트 프레임워크 연동 및 시뮬레이션 환경에서의 ECL 추상화
• 27.2.2. ekf2 모듈 래퍼(Wrapper) 클래스 구조 (src/modules/ekf2)
• 27.2.2.1. 메인 클래스 구조 및 PX4 모듈 베이스(ModuleBase) 상속 분석
• 27.2.2.2. 스케줄링 로직: 작업 항목(WorkItem) 인터페이스를 통한 주기적 호출 방식
• 27.2.2.3. 센서 폴링 레이트 통제를 위한 구독 콜백(SubscriptionCallback) 활용
• 27.2.3. 데이터 전달 브릿지 메커니즘
• 27.2.3.1. 센서 통합 데이터를 ECL 내부 구조체로 변환하는 직렬화(Serialization) 과정
• 27.2.3.2. 퓨전 결과값을 로컬 위치 및 자세 토픽으로 역직렬화하여 발행(Publishing)하는 과정
• 27.3 24상태 EKF 모델 동역학 및 소스 코드 매핑
• 27.3.1. 벡터 인덱싱과 상태 변수 할당 (StateSample 구조체 분석)
• 27.3.1.1. 쿼터니언 기반 자세 상태 변수 할당 (인덱스 0~3)
• 27.3.1.2. NED 좌표계 기준 3차원 속도 상태 변수 할당 (인덱스 4~6)
• 27.3.1.3. NED 좌표계 기준 로컬 위치 상태 변수 할당 (인덱스 7~9)
• 27.3.1.4. 자이로스코프 델타 앵글 바이어스 할당 (인덱스 10~12)
• 27.3.1.5. 가속도계 델타 벨로시티 바이어스 할당 (인덱스 13~15)
• 27.3.1.6. 지구 자기장(Earth Magnetic Field) 모델 변수 할당 (인덱스 16~18)
• 27.3.1.7. 기체 고정축 자기장(Body Magnetic Field) 왜곡 모델 변수 할당 (인덱스 19~21)
• 27.3.1.8. 2차원 풍속(Wind Velocity) 추정 모델 변수 할당 (인덱스 22~23)
• 27.3.2. 관성 항법 방정식(Inertial Navigation Equations)의 C++ 구현체 분석
• 27.3.2.1. 상태 예측 함수(predictState) 내부의 쿼터니언 적분식 코드 흐름
• 27.3.2.2. 가속도계 비력(Specific force)의 로컬 프레임 변환을 위한 방향 코사인 행렬(DCM) 연산 최적화
• 27.3.2.3. 중력 가속도 보상 및 타임스텝 기반의 속도/위치 1차 수치 적분(Euler Integration) 구현
• 27.3.3. 공분산 예측 소스 코드 분석 (predictCovariance)
• 27.3.3.1. 자동 생성된 24x24 상태 전이 행렬의 희소(Sparse) 연산 코드 구조
• 27.3.3.2. 상태 공분산 배열 업데이트 로직 및 부동소수점 오차에 의한 비대칭성 제어 기법
• 27.4. 센서 데이터 지연(Delay) 처리 및 링 버퍼(Ring Buffer) 아키텍처
• 27.4.1. 상태 지연 문제의 정의
• 27.4.1.1. IMU 데이터 도달 시간 대비 타 센서(GPS 등) 처리 지연에 따른 상태 비동기화 현상
• 27.4.2. 링 버퍼(RingBuffer) 템플릿 클래스 구조
• 27.4.2.1. 센서별 데이터 큐(Queue) 사이즈 동적 할당 원리
• 27.4.2.2. 타임스탬프 기반 이진 탐색 알고리즘을 이용한 보간 데이터 추출 로직
• 27.4.3. 지연 보상 융합 사이클 (Delayed Fusion Horizon)
• 27.4.3.1. 지연된 관측치 도달 시, 해당 타임스탬프의 과거 상태로 되돌아가는 롤백(Rollback) 메커니즘
• 27.4.3.2. 칼만 업데이트 수행 후, 현재 시간까지 버퍼 내 IMU 데이터를 활용한 상태 재적분(Re-propagation) 로직
• 27.5. 센서 퓨전 코어(Sensor Fusion Core) 소스 레벨 분석
• 27.5.1 센서 퓨전 공통 로직
• 27.5.1.1. 퓨전 템플릿 함수(fuse) 호출 패턴: 관측 행렬 구성부터 업데이트까지의 파이프라인
• 27.5.1.2. 혁신 검사(Innovation Test): 이상치(Outlier) 거부를 위한 통계적 비율 계산 함수 구조
• 27.5.2 GPS 퓨전 (fuseVelPosHeight)
• 27.5.2.1. 관측 행렬 내 속도/위치 인덱스 매핑 및 단일 스칼라 업데이트 기법 적용
• 27.5.2.2. 안테나 위치 편차 오프셋을 반영한 레버 암(Lever Arm) 보상 수식의 C++ 구현
• 27.5.3. 지자계 퓨전 (fuseMag)
• 27.5.3.1. 3차원 벡터 퓨전과 헤딩 퓨전 간의 전환 상태 머신 로직
• 27.5.3.2. 지상고 및 추력 데이터 기반 외부 자기장 간섭 회피 로직
• 27.5.4. 기압계 및 고도 퓨전 (fuseBaro)
• 27.5.4.1. 기압계 온도 드리프트 보상 추정 로직
• 27.5.4.2. 이착륙 시 발생하는 지면 효과 강하(Drop) 현상의 모델링 코드
• 27.5.5. 옵티컬 플로우 및 VIO 퓨전
• 27.5.5.1. 광학 흐름 센서의 자이로 레이트 보상 및 초점 거리 환산식 연산
• 27.5.5.2. VIO 오도메트리 데이터 융합 시 좌표계 회전 정렬 오차 공분산 반영 로직
• 27.6. 다중 인스턴스(Multi-Instance) EKF 및 페일오버(Failover) 아키텍처
• 27.6.1. 다중 추정기 생성 모델 (EKF2.cpp)
• 27.6.1.1. 감지된 센서 개수(IMU 및 지자계 조합)에 따른 추정기 클래스 배열 초기화 및 워크 큐 할당
• 27.6.2. 최적 인스턴스 평가 알고리즘 (EKF2Selector)
• 27.6.2.1. 상태 평가 점수(Health Score) 산출: 혁신 분산 누적 및 페널티 부여 산술식
• 27.6.2.2. 추정기 상태 및 혁신 메시지 구독을 통한 실시간 순위 결정 로직
• 27.6.3. 매끄러운 페일오버(Seamless Switching) 제어 기술
• 27.6.3.1. 주(Primary) EKF 인스턴스 교체 조건 판별 로직
• 27.6.3.2. 전환 시 발생하는 상태 단차(Step) 방지를 위한 센서 바이어스 보상 오프셋 리셋 함수
• 27.7. 시스템 상태 진단 메시징 및 트러블슈팅
• 27.7.1. 추정기 상태(estimator_status) 토픽의 비트마스크 분석
• 27.7.1.1. 필터 결함 플래그(filter_fault_flags): 수치 연산 실패 및 공분산 비대칭 원인 역추적
• 27.7.1.2. 솔루션 상태 플래그(solution_status_flags): 위치/자세 데이터의 제어기 사용 가능 여부 검증
• 27.7.1.3. 혁신 검사 플래그(innovation_check_flags): 개별 센서 융합 거부 상태 실시간 트래킹
• 27.7.2. 주요 EKF 강제 리셋 및 복구 소스 흐름
• 27.7.2.1. 공분산 발산 한계 초과 시 수행되는 로컬 좌표계 재초기화 파이프라인
• 27.7.2.2. GPS 손실 후 재수신 시 로컬 원점 재정렬 및 글로벌 좌표 오프셋 수정 알고리즘