27.1.1.2. 예측 단계: 사전 상태 추정 및 오차 공분산 예측 연산의 흐름

이산 시간 EKF의 5대 공식 중 첫 번째 관문인 예측(Prediction) 단계는 외부 센서(GPS 등)의 도움 없이 기체의 관성(Inertia)과 물리 법칙에만 전적으로 의존하여 찰나의 미래(\Delta t)를 그려보는 과정이다.

PX4 ECL 펌웨어에서는 이 강력한 물리 엔진 연산이 src/lib/ecl/EKF/ekf.cpp 내의 메인 업데이트 루프에서 predictStates() 와 predictCovariances() 라는 두 개의 핵심 함수로 나뉘어 엄격하게 실행된다. 본 절에서는 이 두 함수의 내부로 들어가 수학적 예측이 어떻게 C++ 프로그래밍 코드로 숨 쉬고 있는지 그 흐름을 추적한다.

1. 사전 상태 추정: predictStates()

가장 먼저 실행되는 함수는 기체의 24차원 상태 벡터를 다음 타임스텝으로 이동시키는 predictStates() 이다. 이 함수의 유일한 연료는 고주파로 들어오는 IMU(자이로스코프, 가속도계)의 델타 앵글(Delta Angle, \Delta \theta)과 델타 벨로시티(Delta Velocity, \Delta v) 데이터이다.

1.1 각속도 적분과 자세(Attitude) 쿼터니언 업데이트

IMU에서 읽어온 회전각(\Delta \theta)에서 EKF가 내부적으로 관리하는 자이로 바이어스(오차)를 빼내어 순수한 회전 변위 벡터를 구한다.

// ECL 소스 코드 의사코드(Pseudocode) 패턴
Vector3f delta_angle_corrected = _imu_sample.delta_ang - _state.gyro_bias;

기체는 3차원 공간에서 회전하므로 짐벌 락(Gimbal Lock) 현상을 피하기 위해 쿼터니언(Quaternion) 방식을 사용한다.
순수 회전 벡터로 작은 회전 쿼터니언(\Delta q)을 만들고, 이를 이전 루프의 기체 자세 쿼터니언(q_{k-1})에 운동학적으로 곱(Hamilton Product)하여 새로운 사전 자세 추정치 q_{k}^{-}를 도출하고 정규화(Normalize)한다. 이로써 드론이 방금 얼마나 비틀어졌는지 연산이 끝난다.

1.2 가속도 변환과 속도/위치(Velocity/Position) 업데이트

자세가 확정되면, 병진 운동(Translation)을 계산할 차례다. IMU의 델타 벨로시티(\Delta v)에서 가속도 바이어스를 뺀다.
하지만 이 가속도는 드론의 뱃머리를 기준으로 한 동체 프레임(Body Frame) 가속도이다. 앞서 구한 최신 쿼터니언 q_{k}^{-} 을 이용해 이를 동서남북 기준인 항법 프레임(Nav Frame)으로 회전 변환한다.

// 회전 행렬을 이용한 좌표계 변환 및 중력 보상
Vector3f delta_vel_nav = _R_to_earth * delta_vel_corrected;
delta_vel_nav(2) += gravity * dt; // Z축(Down) 벡터에 중력 9.81m/s^2을 보상

이제 순수하게 기체를 밀어내는 이동 벡터(delta_vel_nav)만 남았다. 이를 이전 속도 벡터에 더해 새로운 사전 속도 추정치(v_{k}^{-})를 완성하고, 이 속도를 시간(dt)에 대해 한 번 더 더해주어(사다리꼴 적분 등 활용) 최종 사전 위치 추정치(p_{k}^{-})를 확정 짓는다. 여기까지가 predictStates() 의 물리적 종착지이다.

2. 오차 공분산 예측: predictCovariances()

상태 추정이 끝났다고 EKF가 쉬는 것은 아니다. 방금 구한 나의 위치(\hat{x}_{k}^{-})가 “얼마나 못 미더운지” 그 불확실한 정도를 계산해야 한다. 이것이 오차 공분산 행렬(P)이며, 24개의 상태들이 서로 얼마나 오차를 공유하고 있는지를 나타내는 24 \times 24 (총 576개 원소) 대형 대칭 행렬이다.

예측 분산 공식: P_{k}^{-} = F_{k-1} P_{k-1}^{+} F_{k-1}^{T} + Q_{k-1}

이 공식이 의미하는 바는 명확하다. “시간이 한 스텝 지나면(F), 기존의 불확실성(P)은 증폭되며, 거기에 센서 자체의 진동/노이즈 불확실성(Q)이 추가로 덤터기 매겨진다” 는 철칙이다.

2.1 상태 전이 야코비안 행렬 F 의 도출

식의 중핵인 F 행렬(야코비안)을 실시간으로 도출하는 과정이 바로 ECL 코어 연산 부하의 주범이다. 방금 predictStates()에서 사용했던 비선형 삼각함수와 쿼터니언 수식들을 각 24개의 상태 변수(위치 극좌표, 쿼터니언 등)로 일일이 편미분(Partial Derivative)한 거대한 기울기 행렬이다.

다행히 PX4 개발진은 이 미친 듯한 미분 연산을 사람이 손으로 코딩하다 발생할 휴먼 에러를 막기 위해, 파이썬(Python) 기반의 SymPy(심파이) 기호 연산 라이브러리를 이용하여 C++ 코드를 자동 생성(Auto-generation)하도록 아키텍처를 설계해 두었다. (src/lib/ecl/EKF/python/ 폴더 참조)

2.2 공분산 행렬 P 의 팽창 확정

생성된 야코비안 행렬 F의 원소들을 바탕으로, predictCovariances() 함수 내에서는 기나긴 대수학 방정식들이 펼쳐진다.
특히 회전(자세) 오차가 위치와 속도 오차로 어떻게 전파(Propagation)되는지, 자이로 바이어스 오차가 쿼터니언 오차를 얼마나 갉아먹는지 등이 F P F^T 행렬 곱셈 연산 속에 스며들어 있다.

여기에 마지막으로 시스템 통계적 노이즈 속성인 Q 행렬(개발자가 파라미터로 튜닝하는 EKF2_GYR_NOISE, EKF2_ACC_NOISE 등의 분산값)을 더해주면, 예측 단계의 막이 모두 내리며 풍선처럼 불확실성이 팽창된 사전 오차 공분산 행렬 P_{k}^{-} 가 완성된다.

3. 요약: 무거운 짐을 안고 가는 예측망

결론적으로 EKF 예측 단계의 핵심은 두 축으로 요약된다.

1. 관성 도항(Inertial Coasting): 찰나의 시간 동안 IMU의 변화량만을 맹신하여 기체의 공간상 좌표를 외삽(Extrapolation)해 내는 용감한 전진.
2. 불확실성의 팽창(Uncertainty Expansion): 외부 GPS 보정이 없었기에 ’내 위치가 맞을 확률’을 기하급수적으로 깎아내리며 자기 반성(공분산 증가)을 철저히 기록해 두는 겸손함.

이 부풀어 오른 불확실성 풍선(P_{k}^{-})은, 잠시 후 외부 센서의 달콤한 측정값이 도착하여 바늘로 찌르는 순간(보정 단계, Update) 격렬하게 그 크기를 수축하게 될 것이다. 다음 절에서는 이 보정 단계를 위해 측정값의 오차를 정의하는 혁신(Innovation) 과정에 대해 파헤쳐 본다.