27.5.5.2. VIO 오도메트리 데이터 융합 시 좌표계 회전 정렬 오차 공분산 반영 로직

27.5.5.2. VIO 오도메트리 데이터 융합 시 좌표계 회전 정렬 오차 공분산 반영 로직

외부 컴퓨터(Companion Computer)에서 연산되어 MAVLink를 타고 들어오는 VIO(Visual Inertial Odometry) 오도메트리 데이터는 EKF 입장에서 “아주 똑똑하지만, 방향 감각이 자기중심적인 조수“와 같다. VIO 시스템은 자신이 처음 켜진 순간의 렌즈가 바라보는 방향을 무조건 북쪽(X축)으로 가정하는 로컬 프레임(Local Frame) 을 사용하기 때문이다.

반면 비행 제어기(Pixhawk) 내부의 EKF는 절대적인 지구 자북(또는 진북)을 기준으로 하는 글로벌 NED 프레임(Global NED Frame) 을 고수한다. 이 두 좌표계 사이의 회전 편차(Yaw Alignment Offset)를 정확하게 추정하고, 그 오차의 불확실성(공분산)을 칼만 필터 수학에 무사히 녹여내는 것이 VIO 퓨전의 최대 난관이다. 본 절에서는 PX4-Autopilot이 ev_pos_fusion.cpp 와 ev_yaw_fusion.cpp 에 어떻게 이 회전 정렬 오차 공분산을 반영했는지 상세히 분석한다.

1. 프레임 정렬(Frame Alignment)의 파라미터화

VIO 데이터를 EKF에 융합하기 이전에, 두 좌표계 간의 위치 오프셋(Translation)과 회전 오프셋(Rotation)을 일치시켜야 한다. 위치 오프셋은 단순한 덧셈/뺄셈으로 해결되지만, 회전 오프셋(특히 Yaw 편차)은 외적 연산이 수반되는 비선형적인 파라미터다.

PX4 EKF2는 이 회전 오프셋 자체를 필터 내부의 상태 변수(State Variable)로 끌어안았다.

  • 상태 변수 중 _state.ev_rot(External Vision Rotation) 배열이 VIO 프레임과 EKF NED 프레임 사이의 회전 쿼터니언(또는 오일러 각) 편차를 저장한다.

VIO 장비로부터 (x, y, z) 위치 관측치가 도착하면, EKF는 가장 먼저 이 _state.ev_rot 회전 행렬을 곱하여 VIO의 (x, y, z)를 EKF의 (North, East, Down)으로 강제 변환시킨 뒤에야, 비로소 잔차(Innovation)를 구한다.

2. 관측 야코비안(\mathbf{H}) 행렬에 편입된 회전 정렬 상태

이 과정에서 수학적으로 가장 복잡해지는 지점은 바로 야코비안 행렬(\mathbf{H})의 계산이다. VIO 기반 위치 융합(fuseEvPos)을 수행할 때, EKF는 단순히 내 위치(Position) 상태 변수 3개(7, 8, 9번 인덱스)만 건드리지 않는다.

VIO에서 넘어온 위치 데이터(\mathbf{p}_{\text{ev}})가 회전 정렬 상태 변수(\theta_{\text{ev\_yaw}})를 거쳐 변환되었으므로, 체인 룰(Chain Rule)에 의해 위치 잔차에 대한 \theta_{\text{ev\_yaw}} 의 편미분 값이 0이 아니게 된다.

\mathbf{p}_{\text{NED}} = \mathbf{R}_{\text{ev\_yaw}} \times \mathbf{p}_{\text{ev\_local}}
\frac{\partial \mathbf{p}_{\text{NED}}}{\partial \theta_{\text{ev\_yaw}}} \neq 0

2.1 자동 생성된 비선형 미분 전개식

이 복잡한 삼각함수 편미분 역시 심볼릭 연산 스크립트를 통해 C++ 코드로 자동 생성되어 있다.

// src/lib/ecl/EKF/ev_pos_fusion.cpp 내부의 의사코드

// 1. VIO 위치 데이터 융합 시, EKF 위치 상태(Pos N, E, D)에 대한 H 행렬 성분 (단순 1.0 매핑)
H_pos_n = 1.0f; 

// 2. VIO 위치 데이터 융합 시, 'VIO 회전 정렬 상태 변수(ev_yaw)'에 대한 H 행렬 성분 도출
// (VIO의 현재 로컬 측정 위치가 클수록(원점에서 멀어질수록) 회전 각도 오차에 대한 민감도가 극심하게 증폭된다)
float H_ev_yaw_n = -sinf(ev_yaw_state) * ev_pos_local_x - cosf(ev_yaw_state) * ev_pos_local_y;

// 3. 융합 수행 (이 하나의 VIO 위치 스칼라 관측치가 위치 상태와 EV Yaw 상태를 동시에 업데이트)
fuse(measured_pos_n, predicted_pos_n, H_pos_n, H_ev_yaw_n, ...);

특히 위 코드의 2번 항목에서 보듯, VIO 센서가 로컬 원점(0,0,0)에서 멀리 날아갈수록, 아주 미세한 회전 정렬 오차(Yaw Offset Error)가 NED 프레임상에서는 수 미터(m)의 위치 오차를 창출해 낸다. 따라서 \mathbf{H} 행렬 성분 시야 편미분식에 현재 VIO의 측정 거리(ev_pos_local_x, y)가 스케일링 팩터로 그대로 곱해져 있는 것을 확인할 수 있다.

3. 회전 정렬 오차 공분산의 학습 (Covariance Learning)

이러한 \mathbf{H} 행렬의 교차 결합(Cross-coupling) 설계는 VIO 퓨전 안정성의 핵심이다.

만약 드론이 실내에서 카메라 렌즈만 믿고 앞으로 직진(VIO 프레임 X축 전진)하고 있다고 가정해 보자. 그런데 EKF 내부의 가속도계 데이터가 “너는 완벽히 직진하는 게 아니라 약간 오른쪽으로 치우치고 있어(가속도 Y축 발생)“라고 반성문(Innovation)을 제출한다면 필터는 어떻게 반응할까?

이때 칼만 필터 공분산 연산식(\mathbf{K} = \mathbf{P} \mathbf{H}^T \dots)의 마법이 발동된다. \mathbf{H} 행렬에 ev_yaw 편미분 항이 들어있으므로, 필터는 VIO 위치 잔차를 줄이기 위해 단순히 ’기체 위치 상태 변수’를 이동시키는 것에 그치지 않고, “아, 내가 VIO 프레임과 NED 프레임 사이의 회전 정렬 각도(ev_yaw)를 잘못 알았구나!“라고 스스로 깨닫고 EV 회전 정렬 상태 변수 자체를 미세하게 비틀어 수정(Update) 해 버린다.

즉, VIO가 끊임없이 뿜어내는 수천 개의 (x, y, z) 위치 좌표 궤적과 IMU 가속도계의 관성 궤적이 지속적으로 비벼지는 과정에서, 두 프레임 간의 오프셋 각도 공분산 오차가 “자동으로 수렴(Learn and Converge)“하는 구조가 달성된 것이다.

4. 소결

PX4의 외부 비전 오도메트리 퓨전 로직은 단순한 ‘좌표 복사-붙여넣기’ 도구가 아니다. 완전히 이질적이고 주관적인 기준 좌표계를 갖춘 외부 카메라(VIO) 컴퓨터와, 깐깐하고 절대적인 기준점을 고집하는 EKF(IMU)가 서로의 의견을 조율해 가는 고도의 통계학적 외교(Diplomacy) 메커니즘이다.

체인 룰을 거쳐 \mathbf{H} 행렬에 스며든 ‘좌표계 회전 정렬 공분산’ 수식은 비전 센서의 드리프트를 IMU 관성력으로 찍어 눌러 무효화하는 가장 강력한 무기다.

지금까지 우리는 27.5절 전체에 걸쳐 EKF2가 GPS, 지자계, 기압계, 옵티컬 플로우, VIO 등 각양각색의 센서들을 융합하는 개별 코어를 살펴보았다. 이어지는 대망의 27.6절에서는 이러한 무거운 EKF 알고리즘을 칩셋 메모리에 3개, 혹은 4개씩 띄워놓고 가동하는 극한의 이중화 안전 장치, 다중 인스턴스(Multi-Instance) EKF 및 페일오버(Failover) 아키텍처를 분석한다.