27.1.2.2. 센서 특성을 반영한 관측 야코비안 행렬 도출 방법론

앞선 절에서 기체 내부의 물리적 전파를 다루는 상태 전이 야코비안( $F$ )을 살펴보았다면, 이번에는 외부 세계와 소통하기 위한 관측 야코비안 행렬(Observation Jacobian, $H$ ) 에 대해 알아볼 차례다.

$H$ 행렬은 EKF의 보정(Update) 단계에서 센서가 내뱉어낸 데이터와 EKF의 예측치 사이의 오차(혁신, Innovation)를 칼만 게인으로 변환하기 위한 필수 변환 규격표 역할을 한다. 센서의 종류에 따라 수식의 복잡도가 극단적으로 갈리는 것이 특징이다.

1. 관측 야코비안( $H$ ) 행렬의 근본적 의미

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

이 편미분 수식이 던지는 물리적 질문은 다음과 같다.
“내 24개의 기체 상태 변수( $x$ ) 중 특정 변수 하나가 $1$ 만큼 살짝 변했을 때, 센서(관측 함수 $h$ )의 계기판 바늘은 얼만큼 까딱하게 되는가?”

이 행렬의 형태는 $N \times 24$ 차원을 가지며, 여기서 $N$ 은 특정 센서가 한 번에 측정하는 데이터의 차원 수(예: 3D GPS는 3차원, 기압계는 1차원)이다.

2. 센서별 관측 야코비안 도출의 난이도 극과 극

PX4 ECL에서 $H$ 행렬을 도출하는 로직은 대상 센서가 기체의 ’자세(Attitude)’에 종속되어 있느냐 아니냐에 따라 수식의 복잡도가 천차만별로 나뉜다.

2.1 선형 센서 (Linear Sensors): GPS, 기압계

이 센서들은 지구를 기준으로 한 절대 좌표(NED)를 있는 그대로 뱉어낸다. EKF의 내부 상태 공간 좌표계와 센서의 좌표계 프레임이 일치(Nav Frame)하므로 변환이 거의 필요 없다.

GPS 위치 야코비안 도출:
GPS는 위도, 경도, 고도(North, East, Down) 3차원 위치를 측정한다. 관측 함수 $h_{GPS}(x)$ 는 단순히 24차원 상태 벡터 중 위치 위상(Index 7, 8, 9)을 그대로 빼보는 선형 매핑에 불과하다.
따라서 이것을 상태 벡터 전체로 편미분해 보았자, 위치에 해당하는 열(Column)에만 $1$ 이 남고 나머지 21개의 열은 전부 상수 $0$ 으로 미분되어 날아가 버린다.
이러한 희소 행렬(Sparse Matrix)은 컴퓨터 연산 관점에서 극도로 효율적이며 별도의 심파이(SymPy) 자동 생성 코드 없이도 직관적인 C++ 하드코딩이 가능하다.

2.2 비선형 센서 (Non-linear Sensors): 지자기 센서, 비전 오도메트리

문제가 되는 것은 기체 프레임(Body Frame)에 단단히 고정(Mount)되어 기체와 함께 빙글빙글 도는 센서들이다. 대표적인 것이 동체의 기수를 찾는 지자기 센서(Magnetometer) 다.

지자기 센서 야코비안 도출의 복잡성:
지자기 센서는 지구 자기장 벡터를 동체 X, Y, Z축을 기준으로 측정한다. 하지만 EKF 내부에서 관리하는 지구 자기장 상태 변수(Index 16~18)는 드론이 북쪽을 보든 남쪽을 보든 변하지 않는 절대 항법 프레임(Nav Frame) 기준의 벡터다.

따라서 EKF가 지자기 센서의 관측값을 예측( $h_{MAG}(x)$ )하려면, 이 절대 공간의 3차원 자기장 벡터를 기체의 ’현재 예상 쿼터니언 자세’를 사용하여 역(Inverse) 삼각함수 회전 변환을 거쳐야만 비로소 기체 프레임 가우스(Gauss) 값과 비교가 가능해진다.

$h_{MAG}(x) = R_{Nav}^{Body}(q_{0,1,2,3}) \cdot Mag_{Nav} + MagBias_{Body}$

이 관측 함수를 각 상태 변수로 편미분( $\frac{\partial h_{MAG}}{\partial x}$ )하게 되면 참사가 벌어진다. 16~18번 지구 자기장 상태뿐만 아니라, 0~3번 상태인 쿼터니언 변수 4개에 대해서도 지독한 다항식 연쇄 미분(Chain Rule)이 발생하기 때문이다.

“내 드론의 롤(Roll) 각도를 살짝( $\Delta q$ ) 다르게 뒤틀었을 때, 기체에 붙어있는 지자기 센서의 Z축 계기판 바늘은 얼만큼(기울기) 영향을 받을까?” 라는 계산이 이 미분을 통해 쏟아져 나온다.

2. 융합 관문 통과와 $H$ 행렬의 소멸

PX4 ECL 소스(mag_fusion.cpp, vel_pos_fusion.cpp)를 열어보면, 이 비선형 야코비안 $H$ 연산의 부담을 덜기 위해 $3 \times 24$ 등 거급행렬을 한 번에 곱하는 짓을 하지 않는다. 대신 순차적 처리(Sequential Processing) 라는 수치 해석적 꼼수를 사용하여 센서의 각 축(X, Y, Z) 차원을 $1 \times 24$ 차원의 1차원 행렬(벡터)로 세 번에 나누어 개별 분리 융합(Independent Update)하는 방식을 취한다.

이렇게 $H$ 야코비안 벡터가 해당 루프의 관측 편미분 값을 가득 담고 태어나면, 앞 절에서 배운 혁신 공분산( $S = H P H^T + R$ ) 과 칼만 게인( $K = P H^T S^{-1}$ ) 연산 식의 분모 분자를 채우는 불쏘시개 임무를 완수한 뒤, 해당 $dt$ 프레임의 메모리에서 허무하게 소멸된다.

야코비안 편미분 행렬은 결국 비선형의 굴곡을 거친 현실 세계의 센서 데이터를, 선형 행렬 대수학만이 판치는 C++ 필터 연산기의 제단 위로 평평하게 펴서 매끄럽게 끌어올리기 위한 ‘수학적 다리미’ 에 불과하다.

다음 절에서는 앞선 $F$ 와 이 $H$ 행렬들이 파이썬 스크립트 위에서 어떻게 C++ 스파게티 코드로 토해져 나오는지 자동 생성 툴체인(Toolchain) 메커니즘을 구체적으로 들여다본다.