27.1.1. 이산 시간(Discrete-time) 확장 칼만 필터 알고리즘 구조

27.1.1. 이산 시간(Discrete-time) 확장 칼만 필터 알고리즘 구조

앞 절에서 살펴본 EKF의 비선형성 처리 철학을 바탕으로, 실제 마이크로컨트롤러(MCU) 내부에서 제어 로직이 연산되기 위해서는 연속적인 아날로그 물리 모델을 불연속적인 디지털 틱(Tick) 단위로 쪼개는 과정이 선행되어야 한다. 이를 제어 공학에서 이산 시간(Discrete-time) 모델링이라 부른다.

PX4 ECL(Estimation and Control Library)의 심장부인 EKF2 필터는 이산 시간 기반 확장 칼만 필터의 표준 공식을 가장 기초적이고 충실하게 따르고 있다. 본 절에서는 매 루프(\Delta t)마다 디지털화된 센서 데이터가 어떤 수학적 연쇄 절차를 거쳐 최종 상태 공간(State Space)으로 수렴하는지 그 기본 알고리즘의 구조를 해부한다.

1. 이산 시간(Discrete-time) 모델의 정의

드론의 물리적 움직임은 본래 뉴턴의 기구학(Kinematics) 범주에 속하는 연속 시간(Continuous-time) 편미분 방정식으로 표현된다(예: \dot{x}(t) = f(x(t), u(t))). 그러나 비행 제어기 컴퓨터는 센서로부터 일정한 샘플링 주기(보통 1kHz ~ 250Hz)로 이산화된 디지털 텍스트(샘플)만을 쪼개어 읽어들인다.

따라서 EKF는 시점 k-1 에서 시점 k 로 넘어가는 유한한 시간 간격 \Delta t를 두고 상태가 어떻게 점프하듯 변하는지를 추적하는 이산 시간 차분 방정식(Difference Equation) 모델로 치환된다.

  • 비선형 상태 전이 모델 (Prediction):
    x_{k} = f(x_{k-1}, u_{k-1}) + w_{k-1}
    (현재 시점 k의 상태는 이전 시점 k-1의 상태와 제어 입력 u, 그리고 프로세스 노이즈 w에 의해 결정된다.)

  • 비선형 관측 모델 (Measurement):
    z_{k} = h(x_{k}) + v_{k}
    (현재 시점 k에서 센서가 측정한 값 z는 현재 기체의 물리적 상태 x와 관측 센서 자체의 노이즈 v의 함수이다.)

ECL 소스 코드 내에서는 기체의 위치 추정치를 매 루프 속도(Velocity) 변수를 통해 쉼 없이 적분(Integration)해 나가야 하므로, 이 시간 오프셋인 델타 타임(\Delta t) 변수가 모든 로직의 중핵(Core) 상수로 쓰인다.

2. EKF 알고리즘의 2단계 5대 공식 (The 5 Equations)

이산 시간 확장 칼만 필터의 알고리즘은 철저하게 수학적으로 증명된 5개의 행렬 방정식으로 구성되며, PX4 ECL에서는 이 공식들이 순서대로 함수 내부에 하드코딩되어 호출된다. P는 상태 변수들 간의 결합 불확실성을 나타내는 오차 공분산(Error Covariance) 행렬, Q는 시스템 노이즈 공분산 행렬, R은 센서 측정 노이즈 공분산 행렬을 나타낸다.

2.1 예측 단계 (Prediction Step, Time Update)

이 단계는 고주파로 들어오는 IMU 센서(자이로스코프, 가속도계) 값을 시스템 내부 요인인 제어 입력 u_{k-1}로 취급하여 다음 상태를 맹목적으로 추측(사전 상태, A priori estimate)하는 과정이다. 외부 외란(센서 측정 검토)이 개입하기 전 순수 물리 모델만의 연산이다.

  1. 사전 상태 벡터 추정 (State Prediction):
    \hat{x}_{k}^{-} = f(\hat{x}_{k-1}^{+}, u_{k-1})
    (지난 루프에서 최종 확정된 최고 품질의 기체 상태(\hat{x}_{k-1}^{+})에 현재의 IMU 속도/가속도 벡터 체인을 적분하여 다음 찰나의 좌표와 자세를 한 발 앞서 계산한다. ECL 코드의 predictStates() 함수에 해당한다.)

  2. 오차 공분산 행렬 예측 (Covariance Prediction):
    P_{k}^{-} = F_{k-1} P_{k-1}^{+} F_{k-1}^{T} + Q_{k-1}
    (예측만 거친 상태이므로, 시간이 지날수록 누적되는 모델링의 불확실성을 수학적으로 팽창(Expand)시킨다. 행렬 F_{k-1}는 앞 절에서 설명한 거대 상태 전이 야코비안 행렬이다. ECL 코드의 predictCovariances() 함수 내에서 거대한 파이프라인으로 전개된다.)

2.2 보정 단계 (Update Step, Measurement Update)

비교적 느린 주기로 GPS나 기압계, 외부 비전 시스템 등 객관적인 외부 관측 데이터 z_{k}가 도착했을 때 트리거된다. 공중에 붕 떠 있던 앞서의 사전 상태 추측치(\hat{x}_{k}^{-})를 현실 세계의 좌표로 강력히 끌고 내려와 교정(사후 상태, A posteriori estimate)하는 과정이다.

  1. 최적 칼만 게인 산출 (Kalman Gain):
    K_{k} = P_{k}^{-} H_{k}^{T} (H_{k} P_{k}^{-} H_{k}^{T} + R)^{-1}
    (상태 추정의 신뢰도 가중치 행렬 K를 산출한다. 관측 노이즈 행렬 R이 크면(즉 GPS가 튀면) 분모가 커져 게인 K가 줄어들고 센서를 무시하게 된다. 반대로 P 공분산 행렬이 크면(나의 예측에 자신이 없으면) 게인이 커져외부 센서 데이터를 맹목적으로 추종하도록 수학적 균형추(Balance)가 맞춰진다.)

  2. 사후 상태 벡터 보정 (State Update):
    \hat{x}_{k}^{+} = \hat{x}_{k}^{-} + K_{k} (z_{k} - h(\hat{x}_{k}^{-}))
    (기체의 최종 상태 벡터 24차원 배열을 확정 갱신한다. 방금 들어온 센서 측정값 z_{k}와, 나의 내부 상태 모델로 추측했던 센서 관측 기대값 h(\hat{x}_{k}^{-}) 사이의 생생한 격차 오차(이를 혁신, Innovation이라 부름)에 칼만 게인 팩터를 곱하여, 아까의 불확실했던 내 예측치를 사후 보상(Compensate)한다. fuse() 류의 센서 융합 함수군 내에서 이루어진다.)

  3. 사후 오차 공분산 갱신 (Covariance Update):
    P_{k}^{+} = (I - K_{k} H_{k}) P_{k}^{-}
    (외부 센서의 개입을 통해 기체 좌표를 정확히 묶어두었으므로, 아까 팽창했던 불확실성 공분산(P) 행렬의 덩치를 강제로 다시 수축(Shrink)시킨다. 이 수축 과정 덕분에 다음 프레임의 예측 단계가 훨씬 통제된 조건(오차 바운더리)에서 다시 시작될 수 있다.)

3. 요약: 무한 궤도(Infinite Loop) 속의 선형화 수학

위 다섯 개의 행렬 방정식은 PX4 펌웨어가 부팅이 완료된 시점부터 조종사가 배터리를 분리하는 그 시간까지 초당 수백 번에 걸쳐 재귀적(Recursive)으로 무한히 실행되며 꼬리를 문다.

구세대의 조잡한 EKF 로직들은 이 비대한 수식들을 하나의 C 파일 안에 수천 줄의 for 문으로 난도질하여 넣어두었고, 결국 유지보수가 불가능한 스파게티 시스템 구조를 가져왔다. 하지만 현재 PX4 시스템의 ECL 아키텍처에서는, 이 거대한 이산 시간 모델링 식들을 현대 C++ 객체 지향 철학 템플릿인 Ekf 클래스와 matrix (선형대수학 전용 헤더 온리 라이브러리) 연산자를 통해 모듈화(Encapsulation)하여 극도의 직관성을 끌어올렸다.

다음 절들부터는 이 5대 이산 시간 알고리즘(예측, 혁신, 게인 산출, 오차 갱신)들이 C++ 소스 코드 내부에서 어떻게 수학적 한계를 우회하며 기계어로 환골탈태하여 살아 숨쉬고 있는지 그 이면의 로직을 조각 단위로 해부해 볼 것이다.