[论文解读] Kalman Filters on Differentiable Manifolds

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

Inertial Sensor and Navigation参考文献 26被引用 27

一句话总结

本文为在可微分流形上演化的系统开发了一种通用的符号卡尔曼滤波框架，提出了规范的流形表示以及一个在流形上使用的卡尔曼滤波工具包（on-manifold Kalman filter toolkit），用 C++ 实现，便于部署。

ABSTRACT

Kalman filter is presumably one of the most important and extensively used filtering techniques in modern control systems. Yet, nearly all current variants of Kalman filters are formulated in the Euclidean space $\mathbb{R}^n$, while many real-world systems (e.g., robotic systems) are really evolving on manifolds. In this paper, we propose a method to develop Kalman filters for such on-manifold systems. Utilizing $\boxplus$, $\boxminus$ operations and further defining an oplus operation on the respective manifold, we propose a canonical representation of the on-manifold system. Such a canonical form enables us to separate the manifold constraints from the system behaviors in each step of the Kalman filter, ultimately leading to a generic and symbolic Kalman filter framework that are naturally evolving on the manifold. Furthermore, the on-manifold Kalman filter is implemented as a toolkit in $C$++ packages which enables users to implement an on-manifold Kalman filter just like the normal one in $\mathbb{R}^n$: the user needs only to provide the system-specific descriptions, and then call the respective filter steps (e.g., predict, update) without dealing with any of the manifold constraints. The existing implementation supports full iterated Kalman filtering for systems on any manifold composed of $\mathbb{R}^n$, $SO(3)$ and $\mathbb{S}^2$, 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 that the developed toolkit leads to superior filtering performances and computation efficiency comparable to hand-engineered counterparts. Finally, the toolkit is opened sourced at https://github.com/hku-mars/IKFoM to assist practitioners to quickly deploy an on-manifold Kalman filter.

研究动机与目标

• 为状态在流形上演变而非在欧几里得空间中演变的系统提供滤波动机。
• 提供一个规范的离散时间表示，将流形约束与系统动力学分离。
• 推导在流形上完全迭代的符号误差状态卡尔曼滤波（IKFoM）。
• 开发一个开源的 C++ 工具包，使用户无需直接处理流形约束。
• 使用激光雷达-惯性导航系统和真实世界数据集验证该框架。

提出的方法

• 在流形上定义 boxplus、boxminus 及类似 boxplus 的运算来表示扰动和状态更新。
• 提出一个规范的离散时间形式 x_{k+1} = x_k ⊕_M (Δt f(x_k, u_k, w_k)).
• 使用规范表示导出符号化的误差状态形式的迭代卡尔曼滤波。
• 提供原始流形（如 R^n、SO(3)、S^2）的微分和雅可比矩阵，并可扩展到复合流形。
• 实现一个 C++ 工具包，自动应用带有流形约束的预测/更新步骤。
• 演示其在激光雷达-惯性导航等系统中的适用性，并将性能与手工工程实现进行比较。

实验结果

研究问题

• RQ1 如何在保持流形约束的前提下，将卡尔曼滤波重新表述为直接在可微分流形上运行？
• RQ2 是否可通过一个规范、通用的表示来实现一个在流形上进行符号化、迭代的卡尔曼滤波，而无需逐案推导？
• RQ3 将流形约束纳入卡尔曼滤波对机器人系统的精度和效率有何影响？
• RQ4 一个开源工具包是否能让从业者在极少的系统特定编码下部署 on-manifold 卡尔曼滤波？

主要发现

• 提出一个规范的离散时间表示和一个在流形上误差状态的卡尔曼滤波（IKFoM）。
• 表明在规范形式下，流形约束可以在每个卡尔曼滤波步骤中与系统动力学分离。
• 开发一个 C++ 工具包，使流形处理对用户不可见；用户提供系统描述并调用 predict/update。
• 通过实现紧耦合的激光雷达-惯性导航系统和真实世界数据集来验证框架。
• 报告了优越的滤波性能和与手工工程实现相当的计算效率（定性）。
• 工具包在 https://github.com/hku-mars/IKFoM 开源，以促进更广泛的采用。