27.1. 상태 추정(State Estimation)과 EKF의 수학적 토대

드론이 자율 비행을 수행하기 위해서는 제어기에 기체의 현재 상태(위치, 속도, 자세 등)를 피드백(Feedback)으로 제공해야 한다. 그러나 현실 세계의 센서(IMU, GPS 등)는 완벽하지 않으며 항상 화이트 노이즈(White Noise)와 바이어스(Bias)를 안고 있다.

상태 추정(State Estimation)은 이 불확실한 다수의 센서 데이터들을 융합하여, 드론의 실제 물리적 상태에 가장 가까운 최적 추정치(Optimal Estimate)를 산출해 내는 과정이다. PX4의 ECL(Estimation and Control Library) 모듈은 이 과정을 오차 없이 수행하기 위해 확장 칼만 필터(Extended Kalman Filter, EKF) 라는 강력한 수학적 도구를 채택하고 있다. 본 절에서는 EKF가 기체의 상태를 어떻게 확률적으로 추정해 나가는지 그 기반이 되는 수학적 뼈대를 살펴본다.

1. 선형 칼만 필터(Linear Kalman Filter)의 한계

전통적인 칼만 필터(KF)는 시스템의 동역학 모델(Process Model)과 관측 모델(Measurement Model)이 모두 선형(Linear)이라는 엄격한 가정 하에 작동한다.
상태 방정식과 관측 방정식은 간단한 행렬 방정식으로 다음과 같이 표현될 수 있다.

시스템 동역학 모델: $x_{k} = A x_{k-1} + B u_{k-1} + w_{k-1}$
센서 관측 모델: $z_{k} = H x_{k} + v_{k}$

여기서 $x$ 는 상태 벡터, $A$ 는 상태 전이 행렬 주축, $z$ 는 센서 관측 벡터, $H$ 는 관측 행렬, $w$ 와 $v$ 는 각각 시스템 노이즈와 관측 노이즈(가우시안 정규 분포를 따른다고 가정)를 의미한다.

그러나 무인 항공기(UAS)의 움직임, 특히 3차원 공간에서의 방위각(Euler Angles) 회전이나 쿼터니언(Quaternion) 운동학, 공기 역학적 드래그(Drag) 등은 고도로 복잡한 비선형성(Non-linearity)을 가진다. 삼각함수( $\sin, \cos$ )가 난무하는 3차원 회전 운동을 단순히 행렬 $A$ 나 $H$ 같은 고정된 선형 상수로 정의하는 것은 애초에 불가능하다.

2. EKF(Extended Kalman Filter)의 비선형(Non-linear) 처리 철학

확장 칼만 필터(EKF)는 이러한 치명적인 비선형 모델링 문제를 해결하기 위해 고안되었다. EKF의 핵심 철학은 “함수가 비록 비선형이더라도, 기체가 현재 위치한 상태 공간(Operating Point)의 아주 좁은 영역 주변에서는 테일러 급수(Taylor Series) 1차 항만 취하여 부분적으로 선형화(Linearization)할 수 있다” 는 것이다.

EKF는 비선형 함수 $f$ 와 $h$ 를 도입하여 시스템을 새롭게 정의한다.

비선형 동역학 모델: $x_{k} = f(x_{k-1}, u_{k-1}) + w_{k-1}$
비선형 관측 모델: $z_{k} = h(x_{k}) + v_{k}$

이러한 수식 체계 아래서 기존 선형 필터의 고정된 상태 전이 행렬 $A$ 와 관측 행렬 $H$ 는 소멸되며, 그 자리를 루프(Loop)마다 비선형 함수 $f$ 와 $h$ 를 편미분(Partial Derivative)하여 실시간 연산해 내는 야코비안 행렬(Jacobian Matrix) $F_{k}$ 와 $H_{k}$ 가 대신 채운다.

$F_{k} = \left. \frac{\partial f}{\partial x} \right\vert_{\hat{x}_{k-1}}$
$H_{k} = \left. \frac{\partial h}{\partial x} \right\vert_{\hat{x}_{k}}$

이 야코비안 행렬은 “현재 상태 추정치 부근에서 기체의 상태가 아주 미세하게 요동칠 때, 다음 상태나 센서 관측값이 얼마나 변동하는가“에 대한 순간 기울기(Gradient) 정보를 담고 있다. PX4의 ECL-EKF2 코어 소스 역시 매 타임스텝(Timestep)마다 24차원의 방대한 상태 벡터를 바탕으로 이 거대한 야코비안 행렬을 실시간 도출하는 데 막대한 연산 자원을 소모한다.

3. 재귀적(Recursive) 연산 루틴: 예측(Predict)과 보정(Update)

EKF의 수학적 연산은 매 틱(Tick)의 시스템 타이머에 맞춰 예측(Predict) 단계와 보정(Update) 단계를 무한히 반복 수행하는 체인(Chain) 구조로 이루어진다. PX4의 소스 코드(src/lib/ecl/EKF) 아키텍처 또한 이 수학적 구조를 그대로 반영하여 predictStates(), predictCovariances(), 그리고 fuse() 등 센서 별 보정 함수군으로 그 역할 구분이 명확히 나뉘어 있다.

graph TD;
    A[과거 상태 및 공분산] --> B[1. 예측 단계 (Predict)];
    B --> C(고주파 제어입력: IMU 각속도/가속도);
    C --> D[사전 상태 및 오차 공분산 도출];
    D --> E[2. 보정 단계 (Update)];
    E --> F(저주파 측정값: GPS, 기압계 등);
    F --> G[칼만 게인(Kalman Gain) 역산];
    G --> H[사후 상태 및 공분산 갱신];
    H --> A;

3.1 예측 (Prediction)

IMU(관성 측정 장치) 데이터를 마치 자동차의 엑셀러레이터 제어 입력( $u_{k-1}$ )처럼 적극 활용하여 물리 법칙에 따라 기체의 다음 찰나의 상태를 ’추측’해 내는 단계다. 물리적 운동학(Kinematics) 모델을 통해 상태 방정식을 시간으로 적분해 나가며, 적분이 진행될수록 필연적으로 모델의 오차가 눈덩이처럼 누적되므로 불확실성을 나타내는 오차 공분산 행렬( $P$ ) 역시 팽창하게 된다.

3.2 보정 (Update)

예측 단계를 통해 팽창된 오차는 GPS나 기압계, 비전 센서 등 외부 독립 관측 센서의 값( $z_{k}$ )이 들어오는(Fuse) 순간에만 보정된다.
수학적으로 “필터가 예측해 낸 내 위치( $h(\hat{x}_k)$ )“와 “GPS가 측정한 실제 위치( $z_k$ )” 사이의 기하학적 오차 벡터를 혁신(Innovation) 이라 칭한다. EKF는 이 혁신 오차와 센서의 노이즈 특성 행렬을 혼합하여 칼만 게인(Kalman Gain, $K$ ) 을 도출해 낸다.

칼만 게인은 쉽게 설명하면 “내가 공기역학 모델(IMU)로 예측한 값을 얼마나 강력히 믿을 것인가 vs GPS가 보내온 절대 측정 텍스트를 얼마나 신뢰할 것인가” 를 결정짓는 수학적 가중치(Weight)다. EKF는 이 게인을 지렛대 삼아 오차의 분산(Variance)이 가장 작아지는 확률론적 최적의 방향으로 상태 벡터( $x$ )를 꺾어(끌어당겨) 갱신(Update) 하게 된다.

이 예측과 융합(보정)의 쉼없는 줄다리기가 바로 무인 항공기가 단 1m의 절대 위치 오차도 허용치 않고 허공에 닻을 내린 듯 호버링(Hovering) 할 수 있는 궁극의 수학적 비결이다.