[论文解读] Embedding manifold structures into Kalman filters.

Dongjiao He, Wei Xu+1 more|arXiv (Cornell University)|Feb 7, 2021

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 可实现结果复现，并支持社区扩展。