27.1.2. EKF 내 편미분 및 야코비안(Jacobian) 행렬 연산

확장 칼만 필터(EKF) 공식을 수식으로만 접하면 $F$ 행렬이나 $H$ 행렬이 단순히 몇 개의 고정된 숫자로 채워진 상수 배열처럼 보일 수 있다. 하지만 드론의 자가 위치 추정 시스템에서 이 행렬들은 매 루프마다 실시간으로 수십 회의 삼각함수와 미분 연산을 거쳐 새롭게 조립되는 살아 숨 쉬는 기하학적 형태를 띤다.

본 절에서는 비선형 시스템을 구스르고 선형 필터의 궤도로 끌어들이기 위한 필수적인 수학적 도구, 편미분(Partial Derivative) 과정과 야코비안(Jacobian) 행렬의 정체에 대해 파헤쳐 본다.

1. 선형화(Linearization)를 위한 편미분의 당위성

앞 절들에서 드론의 운동학(Kinematics)과 센서 모델은 삼각함수( $\sin, \cos$ )와 쿼터니언(Quaternion)의 곱셈이 난무하는 고도로 복잡한 비선형 함수( $f, h$ )임을 확인했다. 이러한 방정식들은 “현재 속도로 1초를 날면 어떻게 될까?” 같은 예측은 훌륭하게 해낼 수 있다.

하지만 EKF의 가장 중요한 존재 이유인 “나의 불확실성(공분산 $P$ )이 이 함수를 타고 흘러갔을 때 어떻게 팽창하거나 수축할 것인가?” 를 파악하기 위해서는, 단순히 함수에 값을 집어넣어 보는 것만으로는 답을 얻을 수 없다.

오차의 전파 속성(Propagation of Error)을 구하려면, “특정 상태(예: 피치 각도)가 아주 미세하게( $\Delta x$ ) 변했을 때, 결과값(예: 전진 속도)이 얼마의 비율( $\Delta y$ ) 로 크게 요동치는가?“에 대한 ‘순간 기울기(Gradient)’ 정보가 필수적이다. 비선형 함수를 다변수 상태 공간 내의 한 점(Operating Point)에서 순간적으로 평평하게 자르는 행위, 이것이 바로 편미분(Partial Differentiation) 이자 EKF의 선형화 철학이다.

2. 야코비안(Jacobian) 행렬의 정의와 구조

다변수 함수의 모든 조합에 대한 편미분 결괏값을 거대한 2차원 표(행렬)로 묶어 놓은 구조체를 선형대수학에서는 야코비안 행렬(Jacobian Matrix) 이라고 부른다.

PX4 ECL 튜닝과 코드 분석에 있어 가장 중요한 두 개의 야코비안 행렬은 다음과 같다.

2.1 상태 전이 야코비안 행렬 ( $F_k$ )

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

크기: $24 \times 24$ 정방 행렬 (총 576개 원소)
물리적 의미: “이전 타임스텝의 내 현재 상태 추정치( $\hat{x}_{k-1}$ ) 주변에서 상태가 미세하게 요동칠 때, 다음 타임스텝 상태가 변화하는 정도의 비율”
활용처: predictCovariances() 함수에서 오차 공분산( $P$ )을 예측(팽창)시키는 대수식( $F P F^T$ )의 핵심 양날개로 쓰인다.
이 행렬 안에는 ‘쿼터니언 오차가 위치 공간으로 전파되는 기울기’, ‘자이로 바이어스가 자세 각도에 미치는 기울기’ 등 물리 법칙의 인과 관계가 미분된 형태로 고스란히 담겨 있다.

2.2 관측 야코비안 행렬 ( $H_k$ )

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

크기: $(측정 차원) \times 24$ (예: 3D GPS 관측이면 $3 \times 24$ 행렬, 기압계 관측이면 $1 \times 24$ 행렬)
물리적 의미: “내 현재 사전 예측( $\hat{x}_{k}^{-}$ ) 위치 주변에서 상태가 조금 흔들릴 때, 센서(예: GPS)가 측정해 낼 것으로 기대되는 값이 얼마나 민감하게 변동하는가?”
활용처: 칼만 게인( $K$ )을 산출하는 공식과, 사후 공분산( $P^{+}$ )을 수축시키는 단위 행렬 뺄셈 연산에서 쓰인다.
가장 극단적인 예로 지자기(Magnetometer) 퓨전 시의 $H$ 행렬은, 기체가 어떤 자세(Attitude)를 취하고 있는지에 따라 편미분 기울기 값이 시시각각 변동하므로 엄청난 연산 부하를 동반한다.

3. 코드 자동 생성(Auto-Generation) 아키텍처 도입

이 거대한 $24 \times 24$ 크기의 $F$ 야코비안 행렬을 매 틱(Tick)마다 실시간으로 C++ 배열을 통해 채워 넣어야(Populate) 한다고 상상해 보라. 인간의 손으로 576개의 다차원 편미분 체인 룰(Chain Rule) 수식을 종이에 풀고, 이를 다시 C++ 코드로 타이핑하는 것은 사실상 불가능하며 100% 휴먼 에러(Human Error)를 유발한다.

실제로 초기 오픈소스 필터 코드들에서는 사람의 실수로 부호 하나가 틀리게 입력되어 기체가 특정 각도에서만 이유 없이 추락하는 고질적인 버그들이 빈번히 발생했었다.

이러한 문제를 원천 차단하기 위해 현재의 PX4 ECL 아키텍처 개발팀은 기계적인 코드 자동 생성(Auto-generation) 파이프라인을 구축했다.

기호 연산(Symbolic Computation): 파이썬(Python)의 SymPy 라이브러리를 이용하여 기구학 및 센서 모델 방정식을 기하학 문자로 입력해 둔다.
미분 연산기: 파이썬 스크립트가 수학 공식에 따라 변수 24개에 대한 편미분을 컴퓨터 프로세서의 힘으로 무오류(Error-free) 도출해 낸다.
C++ 코드 방출: SymPy가 미분해 낸 길고 복잡한 수식들을 최적화하여 묶어낸 뒤, 이를 그대로 .cpp 소스 코드 문자열 형태로 방출(Export)하여 파일로 저장한다. (src/lib/ecl/EKF/python/ 폴더 내 스크립트 참조)

ECL 코드 베이스를 열어보면 predictCovariances 함수나 센서 융합 루틴 내부의 행렬 채우기 for 문들이 유독 기계적으로 길고 딱딱하게 작성되어 있는 것을 볼 수 있는데, 이는 개발자가 짠 논리 흐름이 아니라 파이썬 스크립트가 뱉어낸 $F$ 나 $H$ 야코비안 편미분 방정식 기계어 생성물의 흔적이다.

이 혁신적인 심파이(SymPy) 자동화 체계 덕분에, PX4 개발자들은 3D 수학의 얽힌 실타래를 풀다 밤새울 필요 없이 기체의 동역학 방정식(원론) 수정에만 오로지 집중할 수 있게 되었다. 다음 절들에서는 이 자동 생성 파이프라인이 $F$ 와 $H$ 에 대해 실제로 어떤 C++ 형태의 코드를 내뱉는지 구체적으로 매핑해 본다.