面向程序员的卡尔曼滤波器非数学介绍

Question

阅读我的宣言 代码作为数学的替代方案 。

本文的代码可以在 此 Colab Notebook 您是否应该选择跟随。

为什么选择卡尔曼滤波器？

卡尔曼滤波器非常巧妙。如果您从未听说过它们，那么考虑它们的一种非常直观（并且可以说是简化）的方法是将它们视为一个漏斗，您可以在其中从多个噪声源倒入信息，将它们压缩成一个更准确的统计数据。如果这一切听起来很模糊，请不要担心。稍后，我们将将该声明剥离为更易于理解的示例，以期加深我们的直觉。最好早点说，没有比数学更好的工具来研究和推理卡尔曼滤波器了。但同样真实的是，卡尔曼滤波器的基础数学具有挑战性，并且包含线性代数、概率论和微积分的组成部分。因此，它可能并非所有人都能轻松访问。本文的目的是希望为您提供一种 易于理解的直觉 ，也许会 激励 您自己在这个主题上 深入挖掘 。现在，让我们开始记住这一点： “接下来的内容旨在提供直觉，并且可能并不完全完整” 。

让我们首先问“为什么需要卡尔曼滤波器？”对这个问题的一个简单但故意迟钝的答案是，现实生活并不完美。考虑这个激励人心的例子：想象一艘在一维空间中航行的船，从港口 (x=0) 出发并航行了一段时间。船舶的发动机被配置为向船舶提供恒定速度，例如 10m/s。

我们首先提出一个问题，离开港口 2 秒后，船到底在哪里？当然，您会说船距离港口 2*10=20m 距离，因为毕竟， 距离 = 速度 * 时间 。在理想的世界中，这确实是正确的，并且根本不需要卡尔曼滤波器。但是，在现实世界中，事情从来没有这么简单。首先，可能没有一种发动机能够产生足够的力，使每个时间点始终保持 10m/s 的精确速度。果然，有时速度可能是 10.00001，有时可能是 9.99999 m/s，或者介于两者之间的某个数字，但正如所说，99.99% 的完美最终仍然是不完美的。其次，即使你说你确实拥有如此完美的引擎，但当你施加精确测量的力时，你永远不会获得预期的完美速度。波浪运动可能会导致您的船稍微减慢一点，或者风可能会导致它加速，或者谁知道什么会以谁知道的方式影响它。

因此，仅通过测量您想要的位置，您永远无法确定您的船位于何处。

那么我们是否注定永远不知道自己身处何处？不完全是！这就是 传感器 发挥作用的地方。想象一下，您（水手）随身携带了 GPS。 GPS 就能够在任何给定时刻精确地告诉您所在的位置！事实上，您现在甚至不需要船的速度，因为无论船如何行驶，您的 GPS 总能准确地告诉您您所在的位置。问题解决了吗？就像我说的，不完全是。事实上，传感器常常存在缺陷且不可靠。也就是说，他们确实会给你一个关于你所在位置的衡量标准，但这个衡量标准可能并不精确。因此，您的 GPS 可能会在 3 秒后告诉您，您距离港口 29.998 米或 30.002 米，甚至，这是极不可能的，距离港口 100 米，但关键是您永远不能完全依赖在它上面。此外，您永远无法确定您的传感器永远不会发生故障。以 GPS 传感器为例。一旦您发现自己处于没有卫星覆盖的区域，就等于无法使用。事实上，如果您有一个可以保证永远不会离线的传感器，并以任意准确度测量您想知道的信息，则根本不需要卡尔曼滤波器。

至此，我们现在准备好回答 为什么我们需要卡尔曼滤波器 。答案与我们之前已经确定的答案并没有真正改变。卡尔曼滤波器是一个漏斗，它采用两个或多个不完美且不可靠的信息源，并对您想知道的内容生成更准确的估计。在此示例中，卡尔曼滤波器会将您在任何时间所在位置的速度估计以及您当时所在位置的 GPS 估计（如果存在）作为输入，然后为您提供比这两者更准确的估计。那些加在一起！事实上，如果您有更多的信息源，例如雷达或声纳，甚至您当前在水中看到的鱼种类，理论上您可以结合这些测量结果来对您的位置进行更准确的估计。

等等，数学在哪里？

所以，现在的问题是，我们如何理解卡尔曼滤波器的作用以及它是如何在不使用数学的情况下做到这一点的，例如（来自维基百科）：

我们首先假设船上不是只有一名乘客，而是有一千名乘客，每个乘客都有 GPS 自己的设备。现在，每个乘客都可以通过首先按以下方式进行基于速度的估计来估计他们所在的位置（从而估计船舶的位置）：

from random import gauss
def new_position(last):
 velocity = 10
 wind = gauss(0, 2)
 wave = gauss(0, 0.1)
 return last + velocity + wind + wave

注意：有关 gauss 函数的可选但更完整的说明，请参阅下面的附录。现在，只需说它生成第二个参数指示的顺序的随机数（正/负）就足够了。

本质上，1000 名乘客中的每一位都在这样做：采取最后一个已知位置（在现在之前的时间），添加速度，并且知道风和水波将稍微改变路线，添加一些随机估计的波动。现在，如果这些乘客确实有估算风速和水速的好方法，他们就会使用它。但由于他们没有这样做，所以他们通过使用随机数来估计影响。确实，这也是现实生活中发生的事情。我们无法测量所有内容，因此我们只需使用一些简单的方法来估计它们，就像上面使用平均值 (0) 和偏差参数（0.1 和 2）所做的那样。

现在我们处于卡尔曼滤波的第二阶段，即测量。在此阶段，所有乘客都知道他们对自己的 状态 （他们所在的位置）只有不完善的了解（由于风和水 噪音 ），因此寻求使用其工作原理如下的传感器来改进它：

def sensor(t):
 if t == 3:
 # oops, passing through a thunderstorm. GPS fluctuating!
 sensor_noise = gauss(5, 10)
 elif t == 6:
 # uh-oh, satellite unavailable!
 sensor_noise = gauss(-5, 10)
 else:
 sensor_noise = gauss(0, 1)
 return true_position[t] + sensor_noise

请记住，传感器是不精确的设备，即它们确实返回大部分正确的统计数据，在本例中是变量 true_position ，但它们本质上具有我们曾经的噪音再次使用随机生成的模型进行建模高斯函数的数字。此外，我们还在这里对传感器的不可靠性进行建模，即在某些情况下（t = 3 和 t = 6），由于某些因素导致传感器基本上不可用，这并非不可想象。因此，每位乘客在任何给定时间使用传感器时，实际上都会得到不同的测量结果。

真实的旅程

想象一下，这艘船现在离开港口，每秒行驶这些距离：

true_position  = [0, 9, 19.2, 28, 38.1, 48.5, 57.2, 66.2,
                 77.5, 85, 95.2]

也就是说，船从港口（x=0）出发，在第一秒行驶 9 米，在第二秒行驶 10.2 米，最终为 19.2 米，依此类推。乘客现在的任务是利用他们拥有的嘈杂且不可靠的测量结果，尽可能准确地预测每秒的这些不同位置。

因此，在时间 t = 1 时，乘客可以从上述函数中获取这些读数：

# New position at t=1 if the last one t=0 was 0
new_position(0) =>     9.37          (error -0.37)
# Sensor reading at t = 1s
sensor(1) =>     8.98                (error +0.02)

对于所有乘客，依此类推。现在的问题是，真相是什么？是我们的牛顿物理学知识更可靠，还是 GPS 传感器更可靠？在这种特殊情况下，由于我们已经知道船的真实位置距离 true_position 变量为 9m，因此答案可能是显而易见的，但情况并非总是如此。在这种情况下，为了结合这两个单独的统计数据，我们实际上采用了非常简单的方法：我们取两者的 平均值 ！对于上面的例子，这实际上最终给我们带来了：

combined =>     (9.37+8.98)/2 =>     9.17         (error -0.17)

请注意，组合统计数据的误差如何小于单独的速度估计 ，但比本示例中的传感器估计更差 。但事情是这样的，我们实际上可以做得比简单平均更好。考虑一下这样的情况：您知道您的传感器实际上是最先进的并且非常可靠。这实际上意味着您应该更喜欢传感器所说的内容而不是速度更新的内容。实际上，您可以通过使用称为加权平均值的方法来做到这一点。考虑以下代码：

def combine(A, B, trustA, trustB):
 total_trust = trustA + trustB
 return (A * trustA + B * trustB) / total_trust

它结合了来自来源 A 和 B 的两个数字，但也考虑了您对这些来源的信任程度。因此，如果您将其称为：

combine(9.37, 8.98, 10, 1) => 9.33         (error -0.33)
combine(9.37, 8.98, 1, 10) => 9.01         (error -0.01)

在第一次调用中，您对源 A（速度）的信任度远高于源 B（传感器），即 10 比 1，因此您得到的答案倾向于源 A，即更接近 9.37。然而，在第二次调用中，情况实际上相反，答案倾向于源 B。这种基于信任的加权平均是卡尔曼滤波器的核心，这就是赋予其数据组合能力的原因。

但现在，我们面临一个新问题。 哪个来源更值得信赖或者如何计算信任？ 应该优先考虑速度吗？或者 GPS 测量应该受到青睐？决定这一点的是偏差或方差度量。大家想一想，还有什么更值得信赖的呢？一种波动剧烈的信息源还是一种波动不大的信息源？换个角度来看，假设您收听 10 个天气广播电台，其中 4 个告诉您将下雨，其中 6 个告诉您将是晴天。现在想象一下，您登录了 10 个天气网站，其中 9 个告诉您将下雨，1 个告诉您将是晴天。这里哪个来源更可靠？您愿意相信大多数气象广播电台告诉您的内容吗（晴天）？或者您倾向于相信天气网站告诉您的信息（下雨）吗？合理的行动方针是更倾向于 网站 的结论，因为其中许多网站与结论一致，即它们的方差较低，而气象广播电台，至少在这个例子中，似乎存在波动他们的结论很疯狂，所以也许不应该太相信。

完整的更新步骤如下所示：

from statistics import variance

# Find updated positions per passenger at t seconds
def update(t, last):
 velocity_updates = []
 sensor_updates = []

 for p in range(1000): # for each passenger
 # new velocity update based on last known position
 # for the passenger
 velocity_updates.append(new_position(last[p]))
 sensor_updates.append(sensor(t))
 
 # Calculate trust metrics for velocity and sensor measurements
 # Remember that as fluctuation increases, trust decreases
 # And vice-versa
 fluctuation_velocity = variance(velocity_updates)
 fluctuation_sensor = variance(sensor_updates)

 # calculate trust
 trust_velocity = 1/fluctuation_velocity
 trust_sensor = 1/fluctuation_sensor

 # combine these together for each passenger
 combined = []
 for p in range(1000):
 combined.append(combine(A = velocity_updates[p],
 B = sensor_updates[p],
 trustA = trust_velocity,
 trustB = trust_sensor))
 # Sensor updates & velocity updates returned for plotting purposes
 return sensor_updates, velocity_updates, combined

注意：有关方差函数的更多信息请参见附录。现在，只需将其视为对数字列表中波动的度量。

此代码相对简单。对于每位乘客，它都会进行基于噪声速度的测量和基于噪声传感器的测量。基于所有乘客的这些测量值，然后计算每个测量值的信任度量作为方差的倒数（因为随着方差的增加，信任度降低），然后调用具有相关信任参数的组合方法。值得注意的是， 这里的每位乘客都在为自己进行位置更新 。在此类单独更新结束时，可以将船舶本身的实际位置推断为所有乘客位置的平均值。

结果

为了连接上面提供的整个代码，我们使用以下代码。

# We'll do a final plot using this list
plot_data = []

def update_plot(t, sensor, velocity, combined_position):
 # add true position at this time
 plot_data.append({'passenger': 'true', 'type': 'true', 'time': t,
 'position': true_position[t]})
 # for each passengers
 for p in range(1000):
 plot_data.append({'passenger': p, 'type': 'sensor', 'time': t,
 'position': sensor[p]})
 plot_data.append({'passenger': p, 'type': 'velocity', 'time': t,
 'position': velocity[p]})
 plot_data.append({'passenger': p, 'type': 'combined', 'time': t,
 'position': combined_position[p]})

update_plot(0, [0]*1000, [0]*1000, [0]*1000)
estimated_positions = [0]*1000 # all estimates start from 0
for t in range(1, 10): # ten seconds
 _sensor, _velocity, estimated_positions = update(t, estimated_positions)
 update_plot(t, _sensor, _velocity, estimated_positions)

update_plot 函数仅执行基本的簿记操作，以存储用于绘图目的的瞬态统计信息。这里的主要迭代只是最底层的 for 循环，它通过使用乘客当前的最佳估计来不断更新任何给定时间的位置估计。除此之外，代码基本上是不言自明的。

使用 seaborn 库绘制时，可以看到以下结果：

有点由于目前的规模，很难解析。让我们放大这两个区域，特别是 t=0.75 到 t=1，即传感器正常工作时，以及 t=2 到 t=4，当出现故障时。

注：信封指的是不确定性。线路中的包络线越宽，我们对数字的不确定性就越大。

在第一种情况下，如您所见，所有 1000 名乘客的综合位置估计比单独的速度估计要好（绿色） ）虽然我们的估计确实比第一种情况下的传感器读数更差，但在第二种情况下，我们实际上比单独出现故障的传感器读数要好得多！这是因为卡尔曼滤波器会自动调整由不可预见的波动引起的剧烈变化，并始终为我们提供相当可靠的指标。如下图所示，一旦我们的传感器恢复（t=4 到 t=5），卡尔曼滤波器就会再次开始偏向传感器（有点难以看到，因为传感器读数和真实值重叠太多）。

结论和评论

我想相信您至少对如何做有一些直觉卡尔曼滤波器工作。卡尔曼滤波器的实际理论基础同样令人着迷，如果您的工作需要，我鼓励您进一步研究它。同时，我希望这表明代码作为一种形式语言可以在多大程度上帮助将直觉传递给乍一看可能令人畏惧的概念。我还希望能够借助简单的代码对我感兴趣的主题提供更多见解。

附录

高斯函数

这里您需要了解的唯一特殊函数是 正态分布函数 即 高斯(0, 0.1) 和高斯(0, 2) 。简而言之，它为您提供一个随机数，该随机数通常接近 0（技术上正确的说法是它以 0 为中心），而获得远离 0 的数字的机会由第二个参数（即 2）控制和 0.1。

因此，如果您调用 gauss(0,0.1) ，您更有可能得到 0.06、-0.07、-0.06、0.02、-0.23、-0.06、0.09 等数字，没有特别的限制。命令。

而如果你调用 gauss(0, 2)，你更有可能得到 1.05、1.03、-1.06、0.32、1.29、-0.40、-1.72 等数字，没有特别的意义。命令。

直观地说，第二个参数也称为标准偏差，控制您测量的波动程度。在上面的代码中，这意味着您通常预计风会偏离太多（有风的日子？），而水波会偏离较少（平静的水面）。请注意下面的直方图中，偏差=2 和偏差=0.1 产生数字的频率（特别注意 x 轴）。尽管数字的范围有很大不同，但这两个直方图的形状看起来大致相同。这种明显的钟形形状称为高斯分布、正态分布或钟形曲线分布，在自然界中经常出现。

方差

方差是一致性的度量。也就是说，如果你是一致的，那么你的方差就会很低，反之亦然。在上图中，您无法完全看到方差，因为 x 轴实际上是自动调整的。如果我们要在相同的轴限制内绘制上面的直方图，我们会得到这样的结果：

注意到第一张图像如何更宽？那是因为其中的数字相差很大。也就是说，您期望在其中找到大量的 -2、2、0 和一些 4、-4。但在第二张图片中，您会期望找到大量 0 和 0.1、-0.1 等，但您会期望找到更少的 -2、2 等。正确地说，第一个分布具有更高的方差 (4 准确地说）比第二个只有（0.01）。有关方差的更多信息可以在线找到。