卡尔曼滤波器的非数学介绍

如果你想查看的话，本文的代码可以在我的Github上查看。

卡尔曼滤波器非常巧妙。如果你从未听说过卡尔曼滤波器，那么一种非常直观（也可以说是还原）的思考方式就是将其视为一个漏斗，在这里你可以从多个嘈杂的信息源中获取信息，并将其浓缩为一个更精确的统计数据。

如果这一切听起来含糊不清，请不要担心。稍后，我们将把这句话剥离成一个更容易理解的例子，希望能进一步加深我们的直觉。要研究和推理卡尔曼滤波器，没有比数学更好的工具了。但同样，卡尔曼滤波器的基础数学具有挑战性，包含线性代数、概率论和微积分等内容。因此，并非所有人都能轻松掌握。这篇文章的目的就是希望为你提供一个易于理解的直观印象，或许能促使你自己深入研究这个问题。现在，让我们开始吧，同时牢记这一点："以下内容仅提供直觉，可能并不完整"。

让我们先问一句："为什么卡尔曼滤波器是必要的？对于这个问题，一个简单而又故意模糊的答案是：现实生活并不完美。请看这个激励性的例子：想象一艘船在一个维度上行驶，从港口出发（x=0）并行驶一段距离。这艘船的发动机被设定为为船提供一个恒定的速度，例如 10 米/秒。

我们首先要问的问题是，在离开港口 2 秒钟后，船到底在哪里？很自然，你会说船离港口的距离是 210=20m，因为毕竟距离 = 速度 时间。在理想世界里，这的确是正确的，根本不需要卡尔曼滤波器。但在现实世界中，情况绝非如此简单。首先，可能没有足够的发动机能够产生足够的力，使每个时间点的速度始终保持在 10 米/秒。当然，你可能会在某些时候获得 10.00001 的速度，或在其他时候获得 9.99999 m/s 或介于两者之间的某个数字，但正如所说，99.99% 的完美终究还是不完美。其次，即使你说你确实拥有这样一个完美的发动机，但当你施加一个精确测量的力时，你也不可能获得预期的完美速度。波浪运动可能会让你的船稍微慢一点，或者风可能会让它加速，或者谁也不知道什么东西会以什么方式对它产生影响。

因此，仅仅通过测量你想要的位置，你永远无法确定你的船在哪里。

那么，我们是否注定永远无法真正知道自己的位置呢？不尽然！这就是传感器的用武之地。想象一下，你，水手，随身携带一个全球定位系统。这样，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

注：有关高斯函数的可选但更完整的解释，请参阅下面的附录。目前，只需说明它会产生一个随机数（正数/负数），其顺序由第二个参数指示即可。

从本质上讲，这 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 时，乘客可以通过上述函数得到这些读数：

# 如果 t=0 时的新位置为 0，则 t=1 时的新位置为 0new_position(0) => 9.37 (error -0.37)# t = 1s 时的传感器读数 sensor(1) => 8.98                (error +0.02)

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

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。这种基于信任的加权平均法是卡尔曼滤波器的核心，也是它的数据组合能力所在。

但现在，我们遇到了一个新问题。哪个来源更可信，或者如何计算可信度？是应该优先考虑速度呢？还是应该优先考虑 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），卡尔曼滤波器就会再次偏向于传感器（由于传感器读数和真实值重叠太多，所以有点难看）。

我相信你至少对卡尔曼滤波器的工作原理有了一些直观的了解。卡尔曼滤波器的实际理论基础同样引人入胜，如果你的工作需要，我鼓励你继续深入研究。与此同时，我希望这篇文章能证明，代码作为一种形式语言，能在多大程度上帮助人们对那些乍看之下令人生畏的概念产生直觉。我也希望能够通过简单的代码，向大家传授一些我认为很有吸引力的话题的更多见解。

高斯函数

这里唯一需要知道的特殊函数是正态分布函数，即 gauss(0, 0.1) 和 gauss(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）。有关方差的更多信息，请上网查阅。