本期继续大神Roger Labbe 的 Kalman and Bayesian Filters in Python。上一期 【经典教程翻译】卡尔曼与贝叶斯滤波器：直觉理解滤波器背后的原理（上），这一期介绍滤波器的一些概念，引出广义卡尔曼滤波器 g-h 滤波器的思考方式，并且通过可以实验的例子来建立 g 和 h 因子的作用。

所有文章首发于 MyEncyclopedia公众号，文章链接为

【经典教程翻译】卡尔曼与贝叶斯滤波器：直觉理解滤波器背后的原理（上）

【经典教程翻译】卡尔曼与贝叶斯滤波器：直觉理解滤波器背后的原理（下：滤波器的思考框架）

英文版原书链接在
https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

可动手的 Python Jupyter Notebook环境在

https://nbviewer.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/table_of_contents.ipynb

MyEncyclopedia公众号以机翻为主，人肉校对为辅将这本书籍介绍给大家。由于校对时间和水平有限，可能会有翻译错误，如果大家想深入学习咀嚼，请前往原版网站查阅。

g-h 滤波器

该算法称为g-h 滤波器，又或者称为 滤波器。 和 对应于我们在示例中使用的两个比例因子。 是我们用于测量的缩放比例（在我们的示例中为重量）， 是测量值随时间变化的比例（在我们的示例中为磅/天）。 和 是另一种不同的名称。

g-h 滤波器是大量滤波器的基础，包括卡尔曼滤波器。换句话说，卡尔曼滤波器是 g-h 滤波器的一种形式，我将在本书后面进行证明。你可能听说过最小二乘滤波器，还有可能没听说过 Benedict-Bordner 的滤波器，都是 g-h 滤波器。每个滤波器都有不同赋值 和 的方式，但除此之外算法框架是相同的。例如，Benedict-Bordner 滤波器将一个限制在一定范围内的常量给 和 。卡尔曼等其他滤波器会在每个时间步动态地生成 和 。

让我重复以下关键点，因为它们非常重要。如果你不理解这些，你就不会理解本书的其余部分。如果你确实理解了它们，那么本书的其余部分将自然地为你展开，作为对我们将要问的各种关于 和 “假设”问题的数学阐述。尽管数学公式可能看起来截然不同，但算法将完全相同。

• 多个数据点比一个数据点更准确，所以无论多么不准确，都不要丢弃。
• 始终选择两个数据点之间的数以创建更准确的估计。
• 根据当前估计以及我们认为它会改变多少来预测下一次测量和变化率。
• 然后选择新的估计作为预测值和下一次测量值之间的一部分，根据每个测量值的准确度进行缩放。

让我们看一下该算法的直观描述。

book_plots.predict_update_chart()

image

让我介绍一些更正式的术语。系统是我们要估计的对象。在本章中，系统就是我们试图权衡的任何东西。有些文献称此为受控体（plant）。该术语来自控制系统理论。

系统的状态是我们感兴趣的系统的当前配置或值。上个例子中我们只对重量读数感兴趣。如果我把一个100公斤的重量放在秤上，状态就是100公斤。我们根据与我们相关的内容来定义状态。刻度的颜色与我们无关，因此我们不将这些值包含在状态中。制造商的 QA 工程师可能会在状态中包含颜色，以便她可以跟踪和控制制造过程。

测量值是系统的测量值。测量值可能不准确，因此它的值可能与状态不同。

状态估计是我们的滤波器对状态的估计。例如，对于 100 公斤的重量，由于传感器误差，我们的估计可能是 99.327 公斤。这通常缩写为估计，我在本章中已经这样做了。

换句话说，状态应该被理解为系统的实际值。这个值通常对我们是隐藏的。如果我站在秤上，你就会得到一个测量值。我们称此为可观察的，因为你可以直接观察此测量。相比之下，你永远无法直接观察我的体重，你只能测量它。

这种隐藏和可观察的语言很重要。任何估计问题都包括通过可观察的测量形成对隐藏状态的估计。如果你阅读过文献，那么在定义问题时会使用这些术语，因此你需要熟悉它们。

我们使用过程模型对系统进行数学建模。在本章中，我们的过程模型假设我今天的体重是昨天的体重加上我最后一天的体重增加。过程模型不对传感器进行建模或以其他方式说明。另一个例子是汽车的过程模型。过程模型可能是“距离等于速度乘以时间”。这个模型并不完美，因为汽车的速度可以在非零时间量内变化，轮胎可以在路上打滑，等等。系统错误或过程错误是这个模型中的错误。我们永远不知道这个值；如果我们知道，我们可以改进我们的模型以达到零错误。你可能还会看到系统模型。它们的意思都是一样的。

预测步骤称为系统传播。它使用过程模型_形成新的状态估计。由于过程误差，这个估计是不完美的。假设我们随着时间的推移跟踪数据，我们说我们将状态传播到未来。一些教材称之为进化。

更新步骤称为测量更新。系统传播和测量更新的一次迭代称为时期(epoch)。

现在让我们探索几个不同的问题领域以更好地理解该算法。考虑试图在轨道上跟踪火车的问题。轨道将火车的位置限制在一个非常特定的区域。此外，火车又大又慢。他们需要很长时间才能显着地减速或加速。因此，如果我知道火车在时间 t 位于 23 公里的公里标记处并以 18 公里/小时的速度行驶，我就可以非常有信心地预测它在时间 t + 1 秒的位置。为什么这很重要？假设我们只能以±250 米以下精度测量它的位置。火车以每小时 18 公里的速度行驶，即每秒 5 米。在 t+1 秒，火车将在 23.005 公里处，但测量值可能在 22.755 公里到 23.255 公里之间的任何地方。因此，如果下一次测量显示位置在 23.4，我们知道那一定是不准确的。即使在时间 t 工程师猛踩刹车，火车仍将非常接近 23.005 公里，因为火车不能在 1 秒内减速很多。如果我们要为这个问题设计一个滤波器（我们将在本章中更进一步！），我们会想要设计一个滤波器，将高权重赋予为预测而非测量。

现在考虑跟踪抛出的球的问题。我们知道弹道物体在重力场中在真空中沿抛物线运动。但是扔到地球上的球会受到空气阻力的影响，因此它不会沿完美的抛物线运动。棒球投手在投出曲线球时利用了这一事实。假设我们正在使用计算机视觉跟踪体育场内的球，这是我在工作中所做的事情。计算机视觉跟踪的准确性可能不高，但通过假设球沿抛物线运动来预测球的未来位置也不是非常准确。在这种情况下，我们可能会设计一个滤波器，为测量和预测赋予大致相等的权重。

现在考虑尝试在飓风中追踪氦气派对气球。我们没有合法的模型可以让我们预测气球的行为，除非是在非常短的时间尺度上（例如，我们知道气球不能在 1 秒内飞 10 英里）。在这种情况下，我们将设计一个滤波器，强调测量结果而不是预测结果。

本书的大部分内容都致力于用数学方式表达最后三段中的关注点，然后让我们找到最佳解决方案（在某种数学意义上）。在本章中，我们将只是直观地给 和 分配不同的值，尽管这种方式不是最理想的。但基本思想是将有些不准确的测量与有些不准确的系统行为模型混合，以获得比任何一个信息源本身都更好的过滤估计。

我们可以将其表达为一种算法：

初始化

1. 初始化滤波器表示的系统状态
2. 初始化我们关于系统状态的概率分布

预测

1. 使用系统规则来预测下一时刻的状态
2. 根据预测的不确定性调整状态的概率分布

更新

1. 获取测量值并赋予其关于状态的概率分布
2. 计算估计值和测量值的残差
3. 新的估计在残差连线中某个地方

我们将在整本书中使用相同的算法，尽管有一些修改。

数学符号

我将开始介绍文献中使用的符号和变量名称。其中一些已经在上面的图表中使用过。测量值通常表示为z，（本书中的表示，一些文献使用y)。 下标 表示时间步长，所以 是这个时间步长的数据。粗体表示向量或矩阵。到目前为止，我们只考虑了一个传感器，因此只有一个传感器测量，但通常我们可能有 n 个传感器和 n 个测量。 表示我们的状态，加粗表示它是一个向量。对于我们的秤示例，它代表初始重量和初始增重率，如下所示：

这里，我使用牛顿表示法，在 x 上加个点来表示速度。更准确地说，点表示 x 对时间的导数，当然是速度。对于 62 公斤的体重，每天增加 0.3 公斤，我们有

所以，算法很简单。状态初始化 ，初始估计。然后我们进入一个循环，预测时间或步骤的状态 来自时间（或步骤）的值 。然后我们得到测量值 ，再选择测量和预测之间的某个中间点，创建估计 .

练习：编写通用算法

在上面的示例中，我明确地对此进行了编码，以解决我们在本章中一直在讨论的称重问题。例如，变量命名为“weight_scale”、“gain”等。我这样做是为了让算法更容易理解——你可以很容易地看到我们正确地实现了每一步。但是，那是为一个特定问题编写的代码，算法对于任何问题都是相同的。因此，让我们将代码重写为通用代码以解决任何问题。通用算法使用以下函数签名：

def g_h_filter ( data, x0,  dx, g,  h,  dt ):
    """
    对具有固定 g 和 h 的 1 个状态变量执行 gh 滤波器。
    'data' 包含要过滤的数据。
    'x0' 是我们状态变量的初始值
    'dx' 是我们状态变量的初始变化率
    'g' 是 g-h's 比例因子
    'h' 是 g-h's 比例因子
    'dt' 是长度时间步长
    """

将数据作为 NumPy 数组而不是列表返回。通过传入与之前相同的权重数据对其进行测试，绘制结果，并直观地确定它是否有效。

from kf_book.gh_internal import plot_g_h_results
def g_h_filter(data, x0, dx, g, h, dt):
    pass #  your solution here

# uncomment to run the filter and plot the results
#book_plots.plot_track([0, 11], [160, 172], label='Actual weight')
#data = g_h_filter(data=weights, x0=160., dx=1., g=6./10, h=2./3, dt=1.)
#plot_g_h_results(weights, data)

解决方案与讨论

import matplotlib.pylab as pylab

def g_h_filter(data, x0, dx, g, h, dt=1.):
    x_est = x0
    results = []
    for z in data:
        # prediction step
        x_pred = x_est + (dx*dt)
        dx = dx

        # update step
        residual = z - x_pred
        dx = dx + h * (residual) / dt
        x_est = x_pred + g * residual
        results.append(x_est)
    return np.array(results)

book_plots.plot_track([0, 11], [160, 172], label='Actual weight')
data = g_h_filter(data=weights, x0=160., dx=1., g=6./10, h=2./3, dt=1.)
plot_g_h_results(weights, data)
print(weights)
print(data)

[158.0, 164.2, 160.3, 159.9, 162.1, 164.6, 169.6, 167.4, 166.4, 171.0, 171.2, 172.6]
[159.2  161.8  162.1  160.78  160.985  163.311  168.1  169.696  168.204 169.164  170.892  172.629]

image

这应该是直截了当的。我只是将体重增加代码中的变量名称替换为变量名称x0，dx 等，没有其他改变。

选择 和

g-h 滤波器不是一个滤波器——它是滤波器一大分类。Eli Brookner 在Tracking and Kalman Filtering Made Easy中列出了 11 个，我相信还有更多。不仅如此，每种滤波器都有许多子类型。每个滤波器的区别在于 和 的选择。所以我不能在这里给出“一刀切”的建议。一些滤波器设置 和 作为常量，其他则是动态地。卡尔曼滤波器在每一步动态地改变它们。一些滤波器允许 和 取一个范围内的任何值，其他也可以通过某个函数 使一个值依赖于另一个值： 。

本书的主题不是 g-h 滤波器的整个系列；更重要的是，我们对这些滤波器的贝叶斯方面更感兴趣，我还没有谈到这一点。因此，我不会涵盖选择深入 和 。Tracking and Kalman Filtering Made Easy是该主题的绝佳资源。如果这让你觉得我采取的立场很奇怪，请认识到卡尔曼滤波器的典型公式根本不使用 和 。卡尔曼滤波器是一个 g-h 滤波器，因为它在数学上简化为该算法。当我们设计卡尔曼滤波器时，我们使用的设计标准可以在数学上简化为 和 , 但卡尔曼滤波器形式通常是一种更强大的思考问题的方法。如果现在还不太清楚，请不要担心，一旦我们发展了卡尔曼滤波器理论，它就会清楚。

值得看看 和 对结果的影响，所以我们将通过一些例子来工作。这将使我们深入了解此类滤波器的基本优势和局限性，并帮助我们了解更复杂的卡尔曼滤波器的行为。

练习：创建测量函数

现在让我们编写一个为我们生成噪声数据的函数。在本书中，我将噪声信号建模为信号加白噪声。我们还没有涵盖统计学以完全理解白噪声的定义。从本质上讲，可以将其视为没有模式的随机变化的信号。我们说它是一个序列不相关的随机变量，具有零均值和有限方差。如果你不遵循这一点，你将在高斯章节的结尾处找到相关资料。如果你不了解统计学知识，你可能无法成功完成此练习。那么，请阅读解决方案和讨论。

白噪声可以通过numpy.random.randn()产生。我们想要一个函数，输入为起始值、每步的变化量、步数和我们想要添加的噪声。它的返回是list。通过创建 30 个点对其进行测试，调用g_h_filter()过滤，然后调用plot_g_h_results()绘制结果。

# your code here

解决方案

from numpy.random import randn
def gen_data(x0, dx, count, noise_factor):
    return [x0 + dx*i + randn()*noise_factor for i in range(count)]

measurements = gen_data(0, 1, 30, 1)
data = g_h_filter(data=measurements, x0=0., dx=1., dt=1., g=.2, h=0.02)
plot_g_h_results(measurements, data)

image

讨论

randn()返回以 0 为中心的随机数 - 它大于零和小于零的可能性一样大。它有一个标准偏差——如果你不知道那是什么意思，请不要担心。我绘制了 3000 次调用randn()- 你可以看到这些值以零为中心，大部分范围从略低于 -1 到略高于 +1，尽管偶尔它们会大得多。

plt.plot([randn() for _ in range(3000)], lw=1);

image

练习：糟糕的初始条件

现在编写代码，使用gen_data和g_h_filter来过滤从 5 开始的 100 个数据点，导数为 2，噪声比例因子为 10，并使用 g=0.2 和 h=0.02。将你对 x 的初始猜测设置为 100。

# your code here

解决方案与讨论

zs = gen_data(x0=5., dx=2., count=100, noise_factor=10)
data = g_h_filter(data=zs, x0=100., dx=2., dt=1., g=0.2, h=0.02)
plot_g_h_results(measurements=zs, filtered_data=data)

image

由于错误的初始猜测为 100，滤波器从与远离测量数据的估计值开始。你可以看到它在最终贴近测量数据前起伏震荡。这种稳定前的起伏震荡是滤波器中非常普遍的现象，滤波器设计中的大量工作都致力于最大限度地减少起伏。这是一个我们还没有准备好讨论的话题，但我想向你们展示这个现象。

练习：极度噪声

重新运行相同的测试，但这次使用噪声因子 100。通过将初始条件从 100 降低到 5 来消除初始起伏。

# your code here

解决方案与讨论

zs = gen_data(x0=5., dx=2., count=100, noise_factor=100)
data = g_h_filter(data=zs, x0=5., dx=2., g=0.2, h=0.02)
plot_g_h_results(measurements=zs, filtered_data=data)

image

这对我来说看起来不太好。我们可以看到，也许滤波后的信号变化小于噪声信号，但它远离直线。如果我们只绘制过滤后的结果，没有人会猜到信号从 5 开始并在每个时间步递增 2。虽然在某些地方滤波器似乎确实降低了噪音，但在其他地方它似乎会过高和过低。

在这一点上，我们还没有足够的知识来真正判断这一点。我们添加了很多噪声；也许这是过滤所能达到的最好效果。然而，除了这一章之外的众多章节的存在表明我们应该可以做得更好。

练习：加速度的影响

编写一个新的数据生成函数，为每个数据点添加一个恒定的加速因子。换句话说，在计算每个数据点时增加 dx，以便速度 (dx) 不断增加。设置噪声为0， 和 plot_g_h_results并使用或你自己的例程绘制结果。尝试不同的加速度和时间步长。解释你所看到的。

# your code here

解决方案与讨论

def gen_data(x0, dx, count, noise_factor, accel=0.):
    zs = []
    for i in range(count):
        zs.append(x0 + accel * (i**2) / 2 + dx*i + randn()*noise_factor)
        dx += accel
    return zs
   
predictions = []
zs = gen_data(x0=10., dx=0., count=20, noise_factor=0, accel=9.)
data = g_h_filter(data=zs, x0=10., dx=0., g=0.2, h=0.02)
plot_g_h_results(measurements=zs, filtered_data=data)

image

每个预测都滞后于信号。如果你考虑正在发生的事情，这是有道理的。我们的模型假设速度是恒定的。g-h 滤波器计算的一阶导数 （我们用 表示导数）而不是二阶导数 . 所以我们假设 。 在每个预测步骤中，我们将 x 的新值预测为 。 但由于加速，预测必然落后于实际值。然后我们尝试计算一个新值 ，但由于 ，我们只部分调整 到新的速度。在下一次迭代中，我们将再次落后。

请注意，我们不能通过调整 或者 来纠正这个问题。这称为系统的滞后误差或系统误差。这是 g-h 滤波器的基本属性。也许你的想法已经在为这个问题提出解决方案或解决方法。正如你所预料的那样，针对这个问题已经进行了大量研究，我们将在本书中提出针对这个问题的各种解决方案。

“带回家”的一点是，滤波器的好坏取决于用于表达系统的数学模型。

练习：变化

现在让我们看看不同的效果 。在进行此练习之前，请记住 是用于在测量和预测之间进行选择的比例因子。你认为 值大时起什么作用？值小呢？

现在，让noise_factor=50和dx=5。绘制结果 .

# your code here

解决方案与讨论

np.random.seed(100)
zs = gen_data(x0=5., dx=5., count=50, noise_factor=50)
data1 = g_h_filter(data=zs, x0=0., dx=5., dt=1., g=0.1, h=0.01)
data2 = g_h_filter(data=zs, x0=0., dx=5., dt=1., g=0.4, h=0.01)
data3 = g_h_filter(data=zs, x0=0., dx=5., dt=1., g=0.8, h=0.01)

with book_plots.figsize(y=4):
    book_plots.plot_measurements(zs, color='k')
    book_plots.plot_filter(data1, label='g=0.1', marker='s', c='C0')
    book_plots.plot_filter(data2, label='g=0.4', marker='v', c='C1')
    book_plots.plot_filter(data3, label='g=0.8', c='C2')
    plt.legend(loc=4)

image

很明显， 越大，我们就越遵循测量而不是预测。当 我们几乎完全遵循信号，几乎不拒绝任何噪声。人们可能天真地认为 应该总是非常小，以最大限度地抑制噪声。然而，这意味着我们大多忽略了有利于我们预测的测量结果。当信号变化不是由于噪声而是实际状态变化时会发生什么？我们来看一下。我将创建的数据是从 经过九个步骤变化到 。

zs = [5, 6, 7, 8, 9, 10, 11, 12, 13, 14]
for i in range(50):
    zs.append(14)

data1 = g_h_filter(data=zs, x0=4., dx=1., dt=1., g=0.1, h=0.01)
data2 = g_h_filter(data=zs, x0=4., dx=1., dt=1., g=0.5, h=0.01)
data3 = g_h_filter(data=zs, x0=4., dx=1., dt=1., g=0.9, h=0.01)

book_plots.plot_measurements(zs)
book_plots.plot_filter(data1, label='g=0.1', marker='s', c='C0')
book_plots.plot_filter(data2, label='g=0.5', marker='v', c='C1')
book_plots.plot_filter(data3, label='g=0.9', c='C3')
plt.legend(loc=4)
plt.ylim([6, 20]);

image

在这里我们可以看到忽略信号的效果。我们不仅过滤掉噪声，还过滤掉信号中的有规则的变化。

也许我们需要一个恰到好处的先知滤波器，它的 不太大，也不太小。好吧，不完全是。如前所述，不同的滤波器根据问题的数学性质，以不同的方式选择 和 。例如，Benedict-Bordner 滤波器的发明是为了最大限度地减少本例中的瞬态误差（指一步中 变化很大）。我们不会在本书中讨论这个滤波器，但这里绘制出两个不同选择的 和 下的此滤波器。 这种设计最大限度地减少了阶跃跳变的瞬态误差 ，但代价就是 变化不适合其他类型。

zs = [5,6,7,8,9,9,9,9,9,10,11,12,13,14,
      15,16,16,16,16,16,16,16,16,16,16,16]
data1 = g_h_filter(data=zs, x0=4., dx=1., dt=1., g=.302, h=.054)
data2 = g_h_filter(data=zs, x0=4., dx=1., dt=1., g=.546, h=.205)

book_plots.plot_measurements(zs)
book_plots.plot_filter(data2, label='g=0.546, h=0.205', marker='s', c='C0')
book_plots.plot_filter(data1, label='g=0.302, h=0.054', marker='v', c='C1')
plt.legend(loc=4)
plt.ylim([6, 18]);

image

变化的

现在让我们保持 不变来研究 的变化效果。我们知道 影响我们对 测量相对预测的偏向程度。 但这意味着什么？如果我们的信号变化很大（相对于我们滤波器的时间步长变化很快），那么一个大的 将使我们对这些短暂的变化迅速做出反应。较小的 会让我们反应更慢。

我们将看三个例子。我们有一个无噪声测量，它以 50 步从 0 缓慢变为 1。我们的第一个滤波器使用几乎正确的初始值 和一个小 。你可以从输出中看到滤波器输出非常接近信号。第二个滤波器使用了非常不正确的猜测 。在这里，我们看到滤波器开始“震荡”，直到它稳定下来并找到信号。第三个滤波器使用相同的条件，但现在设置 . 如果你查看震荡的幅度，你会发现它比第二张图表小得多，但频率更高。它也比第二个滤波器更快地稳定下来，但幅度不大。

zs = np.linspace(0, 1, 50)

data1 = g_h_filter(data=zs, x0=0, dx=0., dt=1., g=.2, h=0.05)
data2 = g_h_filter(data=zs, x0=0, dx=2., dt=1., g=.2, h=0.05)
data3 = g_h_filter(data=zs, x0=0, dx=2., dt=1., g=.2, h=0.5)

book_plots.plot_measurements(zs)
book_plots.plot_filter(data1, label='dx=0, h=0.05', c='C0')
book_plots.plot_filter(data2, label='dx=2, h=0.05', marker='v', c='C1')
book_plots.plot_filter(data3, label='dx=2, h=0.5',  marker='s', c='C2')
plt.legend(loc=1);

image

交互示例

对于那些在 Jupyter Notebook 中运行它的人，我编写了滤波器的交互式版本，因此你可以看到更改的效果 , 和 实时。当你调整滑块时 ， 和 数据将被重新过滤并为你绘制结果。

如果你真的想测试自己，请阅读下一段并在移动滑块之前尝试预测结果。

要尝试的一些事情包括设置 和 到他们的最小值。看看滤波器如何完美地跟踪数据！这只是因为我们完美地预测了体重增加。调整 大于或小于 5。滤波器应该从数据中分离出来并且永远不会重新获取它。开始添加回去 和 并查看滤波器如何快速返回数据。仅添加时查看行中的差异 仅对比 . 你能解释一下差异的原因吗？然后尝试设置 大于 1。你能解释一下结果吗？放 回到一个合理的值（比如0.1），然后使 很大。你能解释一下这些结果吗？最后，设置两者 和 到他们的最大价值。

如果你想对此进行更多探索，请将数组zs的值更改为以上任何图表中使用的值，然后重新运行该单元格以查看结果。

from ipywidgets import interact

# my FloatSlider returns an ipywidgets.FloatSlider with
# continuous_update=False. Filtering code runs too slowly
# to instantly react to slider changes.
from kf_book.book_plots import FloatSlider

zs1 = gen_data(x0=5, dx=5., count=100, noise_factor=50)

fig = None
def interactive_gh(x, dx, g, h):
    global fig
    if fig is not None: plt.close(fig)
    fig = plt.figure()
    data = g_h_filter(data=zs1, x0=x, dx=dx, g=g, h=h)
    plt.scatter(range(len(zs1)), zs1, edgecolor='k', 
                facecolors='none', marker='o', lw=1)
    plt.plot(data, color='b')
    plt.show()

interact(interactive_gh,           
         x=FloatSlider(value=0, min=-200, max=200), 
         dx=FloatSlider(value=5, min=-50, max=50), 
         g=FloatSlider(value=.1, min=.01, max=2, step=.02), 
         h=FloatSlider(value=.02, min=.0, max=.5, step=.01));

不要对滤波器说谎

你可以自由设置 和 任何值。这是一个尽管存在极端噪声但仍能完美运行的滤波器。

zs = gen_data(x0=5., dx=.2, count=100, noise_factor=100)
data = g_h_filter(data=zs, x0=5., dx=.2, dt=1., g=0., h=0.)

book_plots.plot_measurements(zs)
book_plots.plot_filter(data, label='filter')
plt.legend(loc=1);

image

我出色地从非常嘈杂的数据中提取了一条直线！也许我现在不应该尝试去获得数学领域的菲尔兹奖。我通过设置 和 到 0做到了这一点 。为什么？它使滤波器忽略测量值，因此对于每次更新，它都会计算新位置 。当然，如果我们忽略测量值，结果是一条直线。

忽略测量的滤波器是无用的。我知道你永远不会同时设置两者 和 为零，因为这需要一种只有我拥有的特殊天才，但我保证，如果你不小心，你会把它们设置得比它们应该的低。你始终可以从测试数据中获得漂亮的结果。当你尝试对不同的数据进行过滤时，你会对结果感到失望，因为你对特定数据集的常量进行了微调。 和 必须反映你正在过滤的系统的真实世界行为，而不是某一特定数据集的行为。在后面的章节中，我们将学到很多关于如何做到这一点的知识。现在我只能说要小心，否则你会用你的测试数据得到完美的结果，但是一旦你切换到真实数据就会得到这样的结果：

zs = gen_data(x0=5, dx=-2, count=100, noise_factor=5)
data = g_h_filter(data=zs, x0=5., dx=2., dt=1., g=.005, h=0.001)
book_plots.plot_measurements(zs)
book_plots.plot_filter(data, label='filter')
plt.legend(loc=1);

image

跟踪火车

我们准备好一个实际的例子。在本章的前面，我们谈到了跟踪火车。火车又重又慢，因此不能快速变速。他们在轨道上，所以他们不能改变方向，除非减速到停下来然后倒车。因此，我们可以得出结论，如果我们已经知道火车的大致位置和速度，那么我们就可以非常准确地预测它在不久的将来的位置。火车不能在一两秒内改变它的速度。

所以让我们为火车写一个滤波器。它的位置表示为它在轨道上相对于某个固定点的位置，我们称之为 0 公里。即，位置为 1 表示火车距离固定点 1 公里。速度以米每秒表示。我们每秒进行一次位置测量，误差为±500 米。我们应该如何实现我们的滤波器？

首先，让我们模拟没有滤波器的情况。我们假设火车目前在 23 公里处，并以 15 m/s 的速度行驶。我们可以将其编码为

pos  =  23 * 1000
vel  =  15

现在我们可以计算火车在未来某个时间的位置，假设速度没有变化，

def compute_new_position(pos, vel, dt=1):
    return pos + (vel * dt)

我们可以通过向位置添加一些随机噪声来模拟测量。这里我们的错误是 500m，因此代码可能如下所示：

def measure_position(pos):
    return pos + random.randn()*500

让我们把它放在一个单元格中并绘制 100 秒模拟的结果。我将使用 NumPy 的asarray函数将数据转换为 NumPy 数组。这将允许我使用“/”运算符一次划分数组的所有元素。

from numpy.random import randn

def compute_new_position(pos, vel, dt=1.):
    """ dt is the time delta in seconds."""
    return pos + (vel * dt)

def measure_position(pos):
    return pos + randn()*500

def gen_train_data(pos, vel, count):
    zs = []
    for t in range(count):
        pos = compute_new_position(pos, vel)
        zs.append(measure_position(pos))
    return np.asarray(zs)
  
pos, vel = 23.*1000, 15.
zs = gen_train_data(pos, vel, 100)

plt.plot(zs / 1000.)  # convert to km
book_plots.set_labels('Train Position', 'time(sec)', 'km')

image

我们可以从图表中看出测量值有多差。真正的火车不可能那样移动。

那我们应该怎么设置 和 ，如果我们想过滤这些数据？我们还没有为此开发理论，但让我们尝试凭经验得到一个合理的答案。我们知道测量值非常不准确，所以我们根本不想给它们太大的权重。为此，我们需要选择一个非常小的 。我们也知道火车不能快速加速或减速，所以我们也想要一个非常小的 。例如：

zs = gen_train_data(pos=pos, vel=15., count=100)
data = g_h_filter(data=zs, x0=pos, dx=15., dt=1., g=.01, h=0.0001)
plot_g_h_results(zs/1000., data/1000., 'g=0.01, h=0.0001')

image

这对于初步猜测来说非常好。让我们做 放大看效果。

zs = gen_train_data(pos=pos, vel=15., count=100)
data = g_h_filter(data=zs, x0=pos, dx=15., dt=1., g=.2, h=0.0001)
plot_g_h_results(zs/1000., data/1000., 'g=0.2, h=0.0001')

image

我们做了g=0.2，我们可以看到，虽然火车的位置被平滑了，但估计的位置（以及速度）在一个非常小的框架内波动很大，远远超过真正的火车可以做到的。所以凭经验我们知道我们想要g<<0.2。

现在让我们看看错误选择的影响 。

zs = gen_train_data(pos=pos, vel=15., count=100)
data = g_h_filter(data=zs, x0=pos, dx=15., dt=1., g=0.01, h=0.1)
plot_g_h_results(zs/1000., data/1000., 'g=0.01, h=0.1')

image

由于小，这里的位置变化平稳 , 但大 使滤波器对测量非常敏感。发生这种情况是因为在几秒钟的过程中，快速变化的测量意味着非常大的速度变化，并且很大 告诉滤波器快速响应这些变化。火车不能快速改变速度，所以滤波器在过滤数据方面做得不好——滤波器改变速度的速度比火车快。

最后，让我们给火车增加一些加速度。我不知道火车实际上能加速多快，但假设它以 0.2 m/sec^2 的速度加速。

def gen_train_data_with_acc(pos, vel, count):
    zs = []
    for t in range(count):
        pos = compute_new_position(pos, vel)
        vel += 0.2
        zs.append(measure_position(pos))
    return np.asarray(zs) 

zs = gen_train_data_with_acc(pos=pos, vel=15., count=100)
data = g_h_filter(data=zs, x0=pos, dx=15., dt=1., g=.01, h=0.0001)
plot_g_h_results(zs/1000., data/1000., 'g=0.01, h=0.0001')

image

在这里，我们看到由于加速度，滤波器不再完全跟踪火车。我们可以调整 让它更好地跟踪，以不太平滑的过滤估计为代价。

zs = gen_train_data_with_acc(pos=pos, vel=15., count=100)
data = g_h_filter(data=zs, x0=pos, dx=15., dt=1., g=.01, h=0.001)
plot_g_h_results(zs/1000., data/1000., 'g=0.01, h=0.001')

image

这里有两个教训要吸取。首先，使用 响应你未建模的速度变化的术语。但是，更重要的是，在快速准确地响应行为变化和在系统处于稳定状态时产生理想输出之间存在权衡。如果火车从不改变速度，我们会 非常小，以避免过滤后的估计值受到测量噪声的过度影响。但是在一个有趣的问题中，状态几乎总是会发生变化，我们希望对它们做出快速反应。我们对它们的反应越快，我们就越容易受到传感器噪声的影响。

我可以继续，但我的目标不是在这里发展 g-h 滤波器理论，而是深入了解结合测量和预测如何导致过滤解决方案。有大量关于选择的文献 和 对于诸如此类的问题，并且有选择它们以实现各种目标的最佳方法。正如我之前解释的那样，在对这样的测试数据进行试验时，很容易对滤波器“说谎”。在随后的章节中，我们将学习卡尔曼滤波器如何以相同的基本方式解决这个问题，但数学要复杂得多。

使用 FilterPy 的 g-h 滤波器

FilterPy是我写的一个开源过滤库。它具有本书中的所有滤波器以及其他滤波器。编写自己的 g-h 滤波器相当容易，但随着我们的进步，我们将更多地依赖 FilterPy。作为快速介绍，让我们看一下 FilterPy 中的 g-h 滤波器。

如果你没有安装 FilterPy，只需从命令行发出以下命令。

pip install filterpy

阅读附录 A，了解有关从 GitHub 安装或下载 FilterPy 的更多信息。

要使用 g-h 滤波器，请导入它并从该类创建一个对象GHFilter。

from filterpy.gh import GHFilter
f = GHFilter(x=0., dx=0., dt=1., g=.8, h=.2)

要运行滤波器调用更新，将测量值传递给参数z，你会记得这是文献中测量值的标准名称。

f.update(z=1.2)

Out[48]:

(0.96, 0.24)

update()``x返回元组中和的新值dx，但你也可以从对象访问它们。

0.96 0.24

你可以动态更改g和h。

(1.965, 0.375)

你可以批量过滤一系列测量。

[[1.965 0.375]
 [2.868 0.507]
 [3.875 0.632]
 [4.901 0.731]]

你可以过滤多个自变量。如果你正在跟踪飞机，则需要在 3D 空间中跟踪它。使用 NumPy 数组进行x、dx和 测量。

x_0  = np.array([1., 10., 100.])
dx_0 = np.array([10., 12., .2])
               
f_air = GHFilter(x=x_0, dx=dx_0, dt=1., g=.8, h=.2)
f_air.update(z=np.array((2., 11., 102.)))
print(' x =', f_air.x)
print('dx =', f_air.dx)

x = [3.8  13.2  101.64]
dx = [8.2  9.8  0.56]

该类GHFilterOrder允许你创建阶数为 0、1 或 2 的滤波器。g-h 滤波器是阶数 1。我们尚未讨论的 g-h-k 滤波器也跟踪加速度。这两个类都具有实际应用程序所需的功能，例如计算方差缩减因子 (VRF)，我们在本章中没有讨论这些功能。我可以写一本关于 g-h 滤波器的理论和应用的书，但我们在本书中还有其他目标。如果你有兴趣，请浏览 FilterPy 代码并进一步阅读。

FilterPy 的文档位于https://filterpy.readthedocs.org/。

Summary

我鼓励你尝试使用此滤波器来加深你对它如何反应的理解。不需要太多尝试就可以意识到临时选择 和 表现不是很好。一个特定的选择可能在一种情况下表现良好，但在另一种情况下表现很差。即使当你了解的影响 和 但依然可能很难选择合适的值。事实上，你极不可能为任何给定的问题都选择最优的 和 。滤波器是专门设计的，不是特选的。

在某些方面，我不想在这里结束这一章，因为关于选择，我们可以说很多 和 。但是这种形式的 g-h滤波器不是本书的目的。设计卡尔曼滤波器需要你指定许多参数：它们确实与选择 和 间接相关，但在设计卡尔曼滤波器时永远不会直接引用它们。此外， 和 将以非常不明显的方式在每个时间步发生变化。

我们几乎没有触及这些滤波器的另一个特征：贝叶斯统计。你会注意到本书的标题中出现了“贝叶斯”一词；这不是巧合！我们暂时放下很大程度上未被探索的 和 ，去发展关于过滤的非常强大的概率推理形式。然而，同样的 g-h 滤波器算法会突然出现，届时有一个正式的数学大厦，允许我们从多个传感器创建滤波器，准确估计我们解决方案中的错误量，并控制机器人。