[논문 리뷰] Embedding manifold structures into Kalman filters.

Dongjiao He, Wei Xu외 1명|arXiv (Cornell University)|2021. 02. 07.

Target Tracking and Data Fusion in Sensor Networks인용 수 18

한 줄 요약

이 논문은 로봇 시스템의 다양한 다양체 구조(SO(3) 및 S² 등)에 대해 오차 상태 칼만 필터에 다양체 기하학을 통합하는 일반적이고 기호 기반의 칼만 필터 프레임워크를 제안한다. ⊕ 및 ⊟⊕와 같은 표준 연산을 도입함으로써 다각체 기하학과 시스템 동역학을 분리하여, 수동으로 유도된 솔루션과 경쟁 가능한 성능을 보이는 자동화된 필터 유도 및 고성능 C++ 구현을 가능하게 한다. 라이다-자이로스코프 내비게이션에서 성능을 확보한다.

ABSTRACT

Error-state Kalman filter is an elegant and effective filtering technique for robotic systems operating on manifolds. To avoid the tedious and repetitive derivations for implementing an error-state Kalman filter for a certain system, this paper proposes a generic symbolic representation for error-state Kalman filters on manifolds. Utilizing the $\boxplus\backslash\boxminus$ operations and further defining a $\oplus$ operation on the respective manifold, we propose a canonical representation of the robotic system, which enables us to separate the manifold structures from the system descriptions in each step of the Kalman filter, ultimately leading to a generic, symbolic and manifold-embedding Kalman filter framework. This proposed Kalman filter framework can be used by only casting the system model into the canonical form without going through the cumbersome hand-derivation of the on-manifold Kalman filter. This is particularly useful when the robotic system is of high dimension. Furthermore, the manifold-embedding Kalman filter is implemented as a toolkit in $C$++, with which an user needs only to define the system, and call the respective filter steps (e.g., propagation, update) according to the events (e.g., reception of input, reception of measurement). The existing implementation supports full iterated Kalman filtering for systems on manifold $\mathcal{S} = \mathbb{R}^m imes SO(3) imes \cdots imes SO(3) imes \mathbb{S}^2 imes \cdots imes \mathbb{S}^2 $ or any of its sub-manifolds, and is extendable to other types of manifold when necessary. The proposed symbolic Kalman filter and the developed toolkit are verified by implementing a tightly-coupled lidar-inertial navigation system. Results show superior filtering performances and computation efficiency comparable to hand-engineered counterparts. Finally, the toolkit is opened sourced at https://github.com/hku-mars/IKFoM.

연구 동기 및 목표

• 로봇 시스템의 다각체 상에서 오차 상태 칼만 필터를 수기로 반복적으로 유도하는 데서 오는 오류를 제거하기 위해.
• ⊕ 및 ⊟⊕와 같은 기호 연산을 사용하여 다각체 상의 로봇 시스템을 표준화된 표현으로 정의하기 위해.
• 다양체 기하학과 시스템 동역학을 분리하는 일반적이고 확장 가능한 칼만 필터 프레임워크를 개발하기 위해.
• 고차원 로봇 시스템에 대해 수동으로 자코비안과 탄성 공간 연산을 유도하지 않고도 효율적으로 필터링할 수 있도록 하기 위해.
• 실시간 로봇 시스템에 즉각 통합할 수 있는 실용적이고 오픈소스의 C++ 툴킷을 제공하기 위해.

제안 방법

• 다양체 상에서 상태 및 오차 전파를 표현하기 위해 ⊟⊕ 및 ⊕ 연산을 사용하는 기호 기반 프레임워크를 도입한다.
• 다양체 기하학을 시스템 동역학에서 분리하여 일반적인 필터 수식을 가능하게 하는 표준 시스템 표현을 정의한다.
• SO(3), S² 등의 다각체를 탄성 공간 상의 연산을 통해 그 내재 기하학을 코딩함으로써 칼만 필터에 다각체 구조를 통합한다.
• 시스템 모델을 필터 알고리즘에서 분리하여 고수준의 시스템 정의에서 자동으로 필터 단계를 생성할 수 있도록 한다.
• 사용자가 시스템 모델을 정의하고 이벤트를 통해 표준 필터 단계(전파, 업데이트)를 호출할 수 있도록 C++ 툴킷으로 구현한다.
• ℝ^m × SO(3)^k × S²^l 및 그 부분다양체를 포함한 복잡한 다각체 상에서 전체 반복 칼만 필터링을 지원하며, 다른 다각체로의 확장성도 제공한다.

실험 결과

연구 질문

• RQ1 임의의 다각체 상에서 오차 상태 칼만 필터의 유도를 자동화할 수 있는 일반적이고 기호 기반의 프레임워크를 설계할 수 있는가?
• RQ2 상태 및 오차 전파의 일관성과 정확성을 보장하기 위해 다각체 특화 연산인 ⊕ 및 ⊟⊕를 어떻게 체계화할 수 있는가?
• RQ3 기호 기반, 다각체 통합 칼만 필터가 수동 최적화된 구현과 비교해 성능 면에서 어느 정도 유사한 성능을 달성할 수 있는가?
• RQ4 실시간 로봇 응용 프로그램을 지원하기 위해 C++로 효율적으로 구현할 수 있는가?
• RQ5 밀접하게 결합된 라이다-자이로스코프 내비게이션과 같은 고차원 시스템에서 이 프레임워크의 성능 및 정확도는 어떻게 스케일링되는가?

주요 결과

• 제안된 기호 기반 칼만 필터 프레임워크는 수동으로 자코비안을 계산하거나 탄성 공간 연산을 도출하지 않고도 오차 상태 필터의 자동 유도를 가능하게 한다.
• C++ 툴킷은 SO(3) 및 S²와 같은 복잡한 다각체 상에서 전체 반복 칼만 필터링을 지원하며, 시스템 모델링을 위한 즉시 사용 가능한 인터페이스를 제공한다.
• 밀착된 라이다-자이로스코프 내비게이션 시스템에서, 이 프레임워크는 수동으로 최적화된 대안들과 비교해 유사한 필터링 성능을 달성했다.
• 시스템 정의 이외의 사용자 노력이 최소화된 채로 실시간 로봇 응용 프로그램에 적합한 계산 효율성을 입증했다.
• 프레임워크는 다른 다각체로도 확장 가능하며, ℝ^m × SO(3)^k × S²^l 의 부분다양체를 기본적으로 지원한다.
• https://github.com/hku-mars/IKFoM 에서 오픈소스 툴킷을 통해 재현 가능성과 커뮤니티 기반 확장이 가능하다.