引言
这里描述的卡尔曼滤波框架是一个非常强大的工具,适用于任何优化问题,特别是视觉里程计、传感器融合定位或SLAM。它旨在提供非常准确的结果,可以在线或离线工作,计算效率较高,并且易于在Python中设计滤波器。
功能介绍
具有符号雅可比矩阵计算的扩展卡尔曼滤波器
大多数动态系统可以描述为隐马尔可夫过程。要估计这种带有噪声测量的系统状态,可以使用递归贝叶斯估计器。对于线性马尔可夫过程,常规的线性卡尔曼滤波器是最优的。不幸的是,许多系统是非线性的。扩展卡尔曼滤波器可以通过在每一步对非线性系统进行线性化来建模系统,当线性化效果足够好时,这提供了接近最优的估计器。如果线性化引入了太多噪声,可以使用迭代扩展卡尔曼滤波器、无迹卡尔曼滤波器或粒子滤波器。对于大多数应用来说,这些估计器都是过度设计。它们增加了很多复杂性,并且需要大量额外的计算。
传统上,扩展卡尔曼滤波器的实现是通过编写系统的动态方程,然后手动象征性地计算线性化的雅可比矩阵。对于复杂系统,这既耗时又容易出现计算错误。这个库使用sympy符号计算雅可比矩阵,以简化系统的定义并消除引入计算错误的可能性。
误差状态卡尔曼滤波器
3D定位算法通常还需要估计物体在3D空间中的方向。方向通常用欧拉角或四元数表示。
欧拉角有几个问题,同一方向可以有多种表示方式,万向节锁可能导致失去一个自由度,最后,当误差较大时,它们的行为非常非线性。具有一个严格正维度的四元数不会受到这些问题的影响,但有另一组问题。四元数需要归一化,否则会无限增长,但这无法在卡尔曼滤波器中干净地实施。最重要的是,四元数有4个维度,但只表示3个自由度,所以有一个冗余维度。
卡尔曼滤波器旨在最小化系统状态的误差。可以设计一个卡尔曼滤波器,其中状态和状态误差在不同的空间中表示。只要有一个误差函数可以根据真实状态和估计状态计算误差即可。在卡尔曼滤波器的误差中有冗余维度是有问题的,但在状态中则不然。一个很好的折衷方案是使用四元数表示系统的姿态状态,使用欧拉角描述姿态误差。这个库支持定义任意误差,该误差可以在与状态不同的空间中。Joan Solà对使用ESKF进行稳健的3D方向估计进行了全面描述。
多状态约束卡尔曼滤波器
如何将基于特征的视觉里程计与卡尔曼滤波器集成?问题在于无法为定位卡尔曼滤波器在图像空间中的2D特征观测写出观测方程。需要给特征观测一个深度,使其具有3D位置,然后才能在卡尔曼滤波器中写出观测方程。通过跨帧跟踪特征并估计深度可以实现这一点。然而,解决方案并不那么简单,通过跨帧跟踪特征估计的深度取决于这些帧中相机的位置,因此取决于卡尔曼滤波器的状态。这创造了一个正反馈循环,卡尔曼滤波器错误地增加了对其位置的信心,因为特征位置更新强化了它。
解决方案是使用MSCKF,这个库完全支持这一点。
Rauch–Tung–Striebel平滑
在使用卡尔曼滤波器进行离线估计时,可能存在一个初始化期,其中状态估计不佳。全局估计器不会受到这个问题的影响,为了使我们的卡尔曼滤波器与全局优化器具有竞争力,我们可以使用RTS平滑器反向运行滤波器。这些结合可能的多次前向和后向数据传递,应该使性能非常接近全局优化。
马氏距离异常值拒绝器
许多测量并不来自高斯分布,因此有不符合卡尔曼滤波器统计模型的异常值。如果不加以处理,这可能会导致许多性能问题。这个库允许使用马氏距离统计测试来处理传入的测量,以解决这个问题。请注意,良好的初始化对于防止良好测量被拒绝至关重要。