为甚么GPS要使用kalman滤波

卡尔曼滤波器使您可以滤除噪声并组合不同的测量值以计算答案。它们是一种特别强大的过滤器，并且数学上很优雅。如果没有矩阵数学软件包，它们通常很难计算，简单过滤器的示例以及带有简单矩阵软件包的一般情况都包含在源代码中。鲁道夫·卡尔曼（Rudolf Kalman）和其他人开发的一种理想的方法是通过测量某些东西来估算某物，其模糊的适用性（通过测量某物来估算某物）具有如此强大的功能，因此在测量值和估算值之间不必存在直接关系。

例子1

假设您需要测量汽车的位置，但是您的位置估算值在任何一次都可以精确到+/- 10米，但是速度测量可以精确到0.1米。此外，如果速度噪声误差是由于 与位置测量结果不同的物理过程引起的，则速度和位置测量结果都可以在卡尔曼滤波器中组合以计算位置。这将是特别准确的，因为由于位置误差而使任何一个估计值产生偏差的误差将通过速度减轻，反之亦然。

例子2

假设您有GPS卫星测距（称为伪距），则任何一组测量的误差都将不相关：名义上的情况是，其中一个较高而另一个较低。它们具有独立的基于时钟的测量误差，并且会穿过大气的不同区域。可以通过矩阵最小二乘数学来完成从伪距计算位置的解决方案，可以将其重新布置为迭代非线性解决方案，以……改善位置的估计，直到误差接近零为止。好的GPS系统通常将其公式化得更远，然后将其重写为卡尔曼滤波器。用于导航的卡尔曼滤波器还可以组合多普勒（不同类型的噪声）累积的载波，分数载波，加速度计等。

揭秘卡尔曼滤波器

通常有2组方程：一组用于更新“状态”，另一组使用最新“状态”来预测系统的未来状态。例如，如果您每秒获得10倍的测量值，但需要每秒进行100倍的火箭航向校正，则将过滤器更新为10倍，但是在两次更新之间，您可以预测向前进行10次航向校正的状态。

一般的理论是使用基于最后估计的确定性的增益将测量值添加到当前估计中。如果估算值稳定，则增益很小，并且只有很小的测量效果被滤除。

预测：

X = F * X + H * U将状态（X）前滚至新时间。

P = F * P * F ^ T + Q将不确定度在时间上向前滚动。

更新：

Y = M – H * X称为创新，=测量–状态。

S = H * P * H ^ T + RS =通过H + R变换的残差协方差

K = P * H ^ T * S ^ -1 K =卡尔曼增益=方差/残差协方差。

X = X + K * Y更新并获得新的测量值

P =（I – K * H）* P更新协方差。

哪里：

X =状态

F =前滚X，通常是一些时间增量。

U =每单位时间dt的值相加。

P =协方差–每件事物彼此之间如何变化。

Y =剩余（测量状态和最后状态的变化量）。

M =测量

S =协方差的残差。

R =最小的创新协方差，可防止滤波器锁定在解决方案中。

K =卡尔曼增益

Q = P的最小更新协方差，防止P变得太小。

H =实际滚动到预测。

I =单位矩阵。

根据我的经验，进行良好的卡尔曼滤波器设计的关键是要注意如何将每个测量转换为状态，以及状态噪声和测量噪声如何根据它们之间的相关性进行匹配。在上述等式中，每个字母不是单个数字，而是一个对象（矩阵）。

保持阶数（矩阵大小）较小或至少稀疏（矩阵中的零点很重要）很重要。上式中的关键是S ^ -1是S的倒数（等于乘以1 / S）。S的逆并非总是可以计算的，因此通常使用伪逆。如果值接近零（四舍五入），这可能很难计算。简而言之……要么做大量的阅读和工作，然后学习不同的方法，要么使矩阵较小，或者使用数学软件包。

从效率和易用性的角度来看，通常最好将每件事分解成一个基于矩阵的小型过程并将它们链接在一起。这通常是一个好主意，因为链中的每个过程都可能更容易理解，描述和测试。每个人都有自己的特殊情况来处理重置，由于缺少数据而产生的惯性运动，由于边界条件而引起的边界检查等。

这是上一篇有关卡尔曼滤波的文章的续篇，可以认为是更详尽，更有用的示例。卡尔曼滤波器在以尽可能小的失真来自适应地去除信号噪声方面做得特别出色。此示例适用于使用GPS和廉价加速度计的2D导航。

假设您想修剪草坪，或者让机器人放下一封信，或者非常精确地在越野路上导航。GPS提供的位置和速度不准确（分别为2.5 m rms，10 cm rms），使用2D卡尔曼滤波器的位置和速度可以改善性能。

仅使用GPS意味着最多可以通过平均（sqrt（1 / n））改善信噪比，在5至10 Hz时，精度会随着航向的变化而迅速降低。GPS也将“跳跃”在位置上，因为会定期选择从解决方案中添加/删除不同的卫星。有时跳高10-15米。GPS的速度（由多普勒计算得出）具有较小的噪声，并且当卫星星座改变时，“跳跃”的频率也较小。

解决方案是添加另一个数据源。有几种加速度计可以测量线性加速度。简而言之，它们提供了速度的变化率，并且在坐标转换后，可以在与GPS相同的坐标空间中进行变化。除了线性加速度计外，第二个GPS的角度还可以提供航向并进一步完善解决方案，但是对于此示例，线性加速度计应足以显着改善。仅依靠加速度计会导致漂移，因为噪声和舍入积分。

加速度计的理想选择是噪声小（0.002 m rms），可以快速测量（1Hz – 5kHz），误差是高斯（平均），误差源与GPS完全不同。由于噪声源中没有连接，因此加速度计和GPS误差源往往会相互抵消。

要求

假设我们可以获得不错的硬件（以下是价格合理的非常可用的硬件规格）。

GPS 2.5 m rms，10 Hz 加速度计2 mm rms，1 kHz

经营目标：

目标<25厘米rms以实现低动态运动

设计

源代码本可以使用3D卡尔曼滤波器（位置，速度，加速度），但GPS与加速度之间没有真正的相关性。使用两个过滤器并分别进行优化比较简单。一个过滤器将速度计算为2D卡尔曼（速度，加速度），从而通过加速度测量对GPS多普勒进行平滑/校正。第二个过滤器获取高度准确的速度信息并将其过滤到位。为了使它在水平面上起作用，使用了两个X（位置，速度）和Y（位置和速度）滤波器。

测试场景是从停止加速，巡航，减速，旋转，缓慢停止，然后启动然后回航。

结论

所提供的股票系数具有良好的精度和良好的响应能力：一旦检测到加速度，它将立即帮助移动位置，GPS会慢慢校正漂移。我希望将来将其用于机器人项目，并希望它可以帮助其他人。