27.1.2.1. 시스템 선형화를 위한 상태 전이 야코비안 행렬 도출 방법론

앞서 살펴본 바와 같이, EKF의 예측 단계에서 가장 무거운 연산 부하를 차지하는 것은 예측 오차 공분산( $P^{-}$ )을 부풀리기 위해 사용하는 $24 \times 24$ 크기의 상태 전이 야코비안 행렬(State Transition Jacobian, $F$ ) 이다.

이 행렬은 기체의 상태 동역학(Dynamics) 방정식 $f(x,u)$ 를 각 상태 변수(State Variable)들로 하나하나 편미분(Partial Derivative)하여 얻어진다. 본 절에서는 PX4 시스템의 코어에서 이 행렬의 원소들이 수학적, 물리적으로 어떻게 도출되는지 그 원천적인 방법론을 해부한다.

1. 24차원 상태 공간(State Space)의 구조

편미분의 대상이 되는 $f(x, u)$ 맵핑을 이해하려면 먼저 PX4 EKF2가 굴리고 있는 24개의 핵심 변수 배열 $x$ 의 정체를 정확히 알아야 한다. 이들은 다음과 같이 기체의 물리적 상태와 센서의 기계적 한계를 동시에 추정하도록 분배되어 있다.

x[0~3]: 자세 쿼터니언 (Quaternion, $q_0, q_1, q_2, q_3$ )
x[4~6]: 3차원 속도 (Velocity NED)
x[7~9]: 3차원 위치 (Position NED)
x[10~12]: 자이로스코프 바이어스 (Gyro Bias)
x[13~15]: 가속도계 바이어스 (Accel Bias)
x[16~18]: 지구 자기장 벡터 (Earth Magnetic Field)
x[19~21]: 기체 프레임 지자기 바이어스 (Body Magnetic Bias)
x[22~23]: 풍속 벡터 (Wind Velocity North, East)

이 24개의 상태들이 다음 타임스텝( $k$ )으로 넘어갈 때 서로 얽히며 만들어내는 변화의 민감도(Sensitivity)를 구하는 것이 야코비안 도출의 핵심이다.

2. 야코비안 미분(Partial Derivative) 전개 예시

행렬 $F$ 의 특정 원소 $F_{i, j}$ 는 “상태 변수 $j$ 가 미세하게 변할 때, 다음 루프의 상태 변수 $i$ 가 얼만큼 널뛰게 되는가?” ($ \partial f_i / \partial x_j $)를 의미한다. 대표적인 물리적 전파(Propagation) 원리 몇 가지를 수식적으로 분해해 보자.

2.1 속도 오차가 위치 오차에 미치는 영향 ( $\partial P / \partial V$ )

가장 직관적인 예시다. 현재 속도( $V$ )에 오차가 생기면 다음 타임스텝 위치( $P$ )는 속도에 시스템 딜레이 시간( $\Delta t$ )을 곱한 만큼 이동해 있을 것이다.

$f_{pos} = pos_{prev} + vel_{prev} \cdot \Delta t$
따라서 $\frac{\partial f_{pos}}{\partial vel_{prev}} = \Delta t$
야코비안 행렬 $F$ 의 위치(행)와 속도(열)가 교차하는 블록 원소들은 단순히 $\Delta t$ (또는 단위 행렬 $I \cdot \Delta t$ ) 상수로 채워진다. 이는 매우 선형적이고 평화로운 미분 결과다.

2.2 자세 오차가 속도 오차에 미치는 영향 ( $\partial V / \partial Q$ )

가장 치명적이고 복잡한 비선형 미분이다. 기체가 기울어져 있으면 IMU가 측정한 가속도를 지구 좌표계(NED)로 변환할 때 방향이 틀어진다.

가속도 변환 공식: $a_{NED} = R(q) \cdot a_{BODY} - g$ (여기서 $R(q)$ 는 쿼터니언 회전 행렬, $g$ 는 중력)
속도 공식: $f_{vel} = vel_{prev} + a_{NED} \cdot \Delta t$
이때 기체 자세 쿼터니언( $q$ )이 아주 미세하게 틀어진다면?
속도를 쿼터니언으로 편미분하는 과정( $\frac{\partial f_{vel}}{\partial q}$ )은 거대한 다항식 미분 체인 룰(Chain Rule)을 동반한다. 결과적으로 야코비안 행렬 안에는 가속도계 측정값( $a_x, a_y, a_z$ ) 성분들과 쿼터니언( $q_0 \sim q_3$ ) 변수들이 어지럽게 곱해진 복잡한 다항식들이 블록 원소로 자리 잡게 된다.

이것의 물리적 의미는 중대하다. “내 기체의 롤(Roll) 각도를 $1^\circ$ 잘못 착각(오차 $P$ )하고 있다면, 중력 가속도( $9.81$ ) 벡터가 x축으로 투영되면서 내비게이션 속도가 1초 뒤에 얼마나 크게 속아 넘어갈 것인가?” 를 EKF가 수학적으로 인지하게 해준다.

2.3 자이로 바이어스가 자세 오차에 미치는 영향 ( $\partial Q / \partial GyroBias$ )

저가형 MEMS 자이로스코프는 온도에 따라 영점(Zero-point)이 미세하게 흐르는 바이어스 현상을 겪는다. 이 바이어스를 제대로 빼주지 않은 채 각속도를 적분하면 기체 자세( $Q$ )가 팽이처럼 계속 돌아가는 치명적인 누적 오차가 발생한다.

야코비안 $F$ 의 쿼터니언(행)과 자이로 바이어스(열) 교차점( $\frac{\partial f_{q}}{\partial Bias_{gyro}}$ )은 이러한 악영향을 수치적으로 나타낸다.
이 기울기 항($\approx -\Delta t \cdot \frac{1}{2} Q_{matrix} $) 덕분에, EKF는 **"나중에 센서(비전, GPS)를 통해 자세($ Q$)가 틀렸다는 걸 깨닫는 순간, 그 원흉인 자이로 바이어스 변수마저도 같이 묶어서 역으로 업데이트(보정)할 수 있는 수학적 명분(가중치 연결고리)“** 을 확보하게 된다.

3. 요약: 물리적 연쇄 반응의 거미줄

위의 방법론을 거쳐 24개의 변수가 만들어내는 모든 경우의 수( $24 \times 24 = 576$ )에 대해 편미분을 수행하여 얻은 결과 행렬 $F_k$ 는 상수 대각 행렬이 아닌 대규모 희소(Sparse) 다항식 행렬이다.

이 거대한 $F$ 야코비안 행렬은 “센서의 작은 결함이 무인기의 어느 관절, 어느 축의 속도로 치명적으로 전파되어 나가는가?“를 보여주는 완벽한 수학적 전파 거미줄(Propagation Web) 이다. PX4 EKF 알고리즘은 매 루프마다 현재의 기체 상태( $x$ )와 IMU 센서 데이터를 이 미분 방정식 거미줄에 대입하여 숫자를 뽑아낸 뒤, 이를 바탕으로 $F P F^T$ 연산을 돌려 내 기체가 가진 불확실성(오차 공분산 $P$ )의 팽창 한계선을 계산해 내는 철두철미함을 보여준다.

다음 절에서는 예측이 아닌 센서 보정(Update) 단계에서 측정값의 척도를 맞춰주기 위해 도출해야 하는 두 번째 거미줄, 관측 야코비안 행렬(Observation Jacobian, $H$ ) 에 대해 파헤쳐 본다.