【滤波】离散贝叶斯滤波器

分手后的思念是犯贱 2023-01-10 11:51 139阅读 0赞

本文主要翻译自[rlabbe/Kalman-and-Bayesian-Filters-in-Python][rlabbe_Kalman-and-Bayesian-Filters-in-Python]的第2章节[02-Discrete-Bayes（离散贝叶斯）][02-Discrete-Bayes]。

**本文实例源码github地址**：[https://github.com/yngzMiao/yngzmiao-blogs/tree/master/2021Q1/20210125][https_github.com_yngzMiao_yngzmiao-blogs_tree_master_2021Q1_20210125]。

## 前言 ##

卡尔曼滤波器属于贝叶斯滤波器的一种。大多数教科书中对卡尔曼滤波的讲解几乎都是直接上贝叶斯公式，然后一大堆公式推导。这种方式也许说明了贝叶斯公式是如何影响到卡尔曼滤波方程的，但大多数的讨论都停留在一个非常抽象的层次上。

这种讲解方法需要对某些数学领域有一个深刻的理解，比如概率统计、矩阵运算等，对于初学者来说还是稍许晦涩难懂。

我将用另一种方式来开启这个话题，这种方式借鉴了Dieter Fox和Sebastian Thrun的想法。利用通过走廊追踪物体来建立贝叶斯滤波器原理的直觉——他们用机器人，我用狗。我喜欢狗，它们比机器人更难预测，这会给滤波带来了有趣的困难。

现在让我们用一个简单的跟踪狗的实验，来看看我们如何使用概率进行滤波和跟踪。

## 跟踪狗 ##

### 初始化 ###

让我们从一个简单的问题开始。现在我们有一个走廊，走廊的两边是墙或者是敞开的门，而狗可以在该走廊中自由行动，我们想追踪他。所以在一次黑客竞赛中，有人发明了一种声纳传感器，它可以连接到狗的项圈上。通过判断该传感器从发出信号到监听到回声的时间间隔，我们可以判断狗是否在一个敞开的门口前面（如果是门，那么时间间隔肯定比墙的时间长）。它还能感知狗什么时候走，并报告狗朝哪个方向移动（比如，可以感知到狗向左或者向右移动1个单位）。它通过wifi连接到网络，并每秒发送一次更新。

我想跟踪我的狗Simon，所以我把这个设备连接到他的项圈上，然后启动Python，准备编写代码跟踪他。乍一看，这似乎是不可能的。如果我开始监听Simon项圈上的传感器，我可能会读到门、墙、墙、门等等。我如何利用这些信息来确定Simon在哪里？

为了使问题小到可以很容易地讲解并编写代码，我们假设走廊上只有10个位置，我们将把这些位置编号为0到9，其中1在0的右边。出于可以重复地进行多次跟踪，我们还假定走廊是圆形的。如果你从位置9向右移动，你将在位置0。

当我开始监听传感器时，我没有办法确定Simon在走廊里的任何特定位置。在我看来，他在任何位置上都有同样的可能。有10个位置，所以他处于任何给定位置的概率都是1/10。

让我们用NumPy表示我们觉得的他在每个位置的概率。我可以使用Python list，但是NumPy数组提供了更强大的功能。

import numpy as np
    belief = np.array([1/10]*10)
    print(belief)

[0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1]

在[贝叶斯统计][Link 1]中，这称为[先验][Link 2]。**它是在融入测量值或其他信息之前的概率**。更完整地说，这被称为先验概率分布（prior）。[概率分布][Link 3]是一个事件的所有可能性的集合，因此**概率分布的总和总是1**，因为这些可能性中至少有一个必须发生。简而言之，概率分布列出了所有可能的事件和每个事件的概率。

贝叶斯统计是概率论的一次革命，这是区别于经典统计学派的。

**经典统计学派又叫做频率学派，所研究的对象是能大量重复的随机现象**，不是这类随机现象不能用频率的方法去确定有关事件的概率。例如掷硬币试验就是可以大量重复的随机现象，结果包括正面和反面。通过进行大量的掷硬币试验，发现正、反面朝上的概率分别稳定在50%左右。

但是对于有一些随机现象，不能重复或者不能大量重复，例如中心气象台预报：明日降水的概率是80%，这里的概率就不能作出频率的解释，因为2020年3月16日只有一次，不可能大量的重复出现，但明日是否下雨是随机现象，这里的80%是气象专家对明日降水的**一种看法或一种信念**，信不信由你，但大多数市民对这句话是理解其含义的，明日降水的可能性较大，叫人们多作预防为好。

也就是说，**贝叶斯统计概率看作是对单个事件的一种信念，经典统计是基于事件发生的频率**。

比如：我再掷一次硬币，让它落地。我该相信它是从哪个方向向上的呢？经典统计概率论对此没有什么可说的，它只会说50%的概率硬币是以正面向上形式掷到地上的。从某些方面上来说，给硬币的当前状态指定一个概率是没有意义的。不是正面就是反面，我们只是不知道是哪个。贝叶斯把这看作是对单一事件的信念——以我的信念或知识的力量，这个特定的硬币投掷情况是50%。有些人反对信念一词，信念意味着可以在没有证据的情况下认为某事是真实的。事实上，**信念的正确与否总是在衡量我们知识的力量**。我们将在接下来的过程中了解更多。

贝叶斯统计考虑了过去的信息（先验）。如果从经典统计上看，我们观察到每100天下雨4次。由此我可以说明天下雨的可能性是1/25。但真实的天气预报不是这样做的：如果我知道今天下雨，风暴锋还停留在当前区域，那么明天很可能也会下雨。因此天气预报是贝叶斯统计的。

**如果一个事件的概率是人们根据经验对该事件发生可能性所给出的个人信念，这种概率称为主观概率，也可以叫做信念**。主观概率反映了人们对某种随机现象的认识，尽管带有主观性（这也是不被频率学派统计学家承认的原因），但是在人类发展的历史长河中，在面对重大决策时，我们利用自身经验作出了许多正确的重大历史决策。因此，我们不应忽略这一略带主观性的信息，应该将其运用到统计推断中。

在实践中，统计学家综合运用了经典统计和贝叶斯统计。有时发现先验是困难的或不可能的，那么就只能进行经典统计。但是，在这书中的例子我们都可以找到先验。因而当我谈论某件事的可能性或概率时，我一般指的都是主观概率，即信念。

现在让我们创建一个走廊的地图。我们先把前两扇门放在一起，然后再把另一扇门放远一点。我们将使用1表示门，0表示墙：

hallway = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0])

我开始监听Simon在走廊的位置，我从传感器得到的第一个数据就是门。目前，假设传感器返回的都是正确答案。由此我断定他在一扇门前，但那扇门是哪一扇呢？我没有理由相信他在第一、第二或第三扇门前。我能做的就是给每个门平均分配一个概率。所有的门都有相同的可能性，并且有三个门，所以我给每个门分配了1/3的概率。

import numpy as np
    import matplotlib.pyplot as plt
    
    def bar_plot(pos, x=None, ylim=(0,1), title=None, c='#30a2da',
            **kwargs):
      ax = plt.gca()
      if x is None:
        x = np.arange(len(pos))
      ax.bar(x, pos, color=c, **kwargs)
      if ylim:
        plt.ylim(ylim)
      plt.xticks(np.asarray(x), x)
      if title is not None:
        plt.title(title)
      plt.show()
    
    belief = np.array([1 / 3, 1 / 3, 0, 0, 0, 0, 0, 0, 1 / 3, 0])
    bar_plot(belief)

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center]

这种分布被称为[分类分布][Link 4]，这是一个描述观察到的n个结果的概率的离散分布。这是一个[多峰分布][Link 5]，因为我们对狗的位置有多种看法。当然，我们并不是说他同时在三个不同的地方，只是我们把我们的知识缩小到这三个地方中的一个。贝叶斯的观点是，有33.3%的概率在0号门，33.3%的概率在1号门，33.3%的概率在8号门。

这是两方面的改进。我拒绝了一些不可能的走廊位置，我对其余位置的概率从10%增加到33%。那么可以想象，**随着更多的传感器数据，我们的信息越多**，最终总有一个位置的概率将接近100%。

简单说一下`分布的峰`。给定一个数字列表，例如 \[ 1 , 2 , 2 , 2 , 3 , 3 , 4 \] \[1, 2, 2, 2, 3, 3, 4\] \[1,2,2,2,3,3,4\]，**峰是最常出现的数字**。对于此list，峰为2。一个分布也可以包含多个峰。列表 \[ 1 , 2 , 2 , 2 , 3 , 3 , 4 , 4 , 4 \] \[1, 2, 2, 2, 3, 3, 4, 4, 4\] \[1,2,2,2,3,3,4,4,4\]包含2和4两个峰，因为它们都出现了三次。我们说前者是单峰的，后者是多峰的。

我在上面的代码中手动定义了概率数组，我们如何在代码中更优雅地实现这一点？如下所示：

belief = hallway * (1/3)
    print(belief)

[0.333 0.333 0.    0.    0.    0.    0.    0.    0.333 0.   ]

### 从传感器读数中提取信息 ###

让我们把Python放在一边，考虑一下这个问题。假设我们从Simon的传感器上读到以下信息：

*  门
 *  向右移动1个单位
 *  门

我们能推断出Simon的位置吗？当然！考虑到走廊的布局，只有一个地方可以得到这个序列，那就是最左端。因此，我们可以自信地说，现在Simon在第2个门口前面。如果不清楚的话，假设Simon是从第2个或第3个门开始的，向右移动1个单位后，他的传感器会返回墙。与传感器读数不符，所以我们知道他不是从那里开始的。那么他是从第1个门开始的，现在在第2个门前面。我们的概率变成：

belief = np.array([0., 1., 0., 0., 0., 0., 0., 0., 0., 0.])

我设计了走廊布局和传感器读数，以便快速给出准确答案。真正的问题并不一定那么清楚，但这应该会触发你的直觉————第一个传感器读数对Simon的位置只给了我们很低的概率（0.333），但在位置更新和另一个传感器读数后，我们知道更多关于他在哪里的信息。你可能会怀疑，如果有一个很长的走廊，有大量的门，经过几次传感器读数和位置更新，我们还可以正确地知道Simon在哪里，或者把可能性缩小到少数可能性吗？当一组传感器读数仅匹配一到几个起始位置时，这就是可能的。

我们现在可以实现这个解决方案，但是让我们考虑一下这个问题的实际复杂性。

### 传感器噪声 ###

完美的传感器结果很少见。如果Simon坐在门前挠自己，脑袋或者爪子遮住了传感器，可能检测不到门；如果他没有直面正前方，传感器可能会误读（明明位置在门的前方，但是身体却朝着旁边的墙）。因此，当我得到传感器的结果为门时，我不能用1/3作为概率。我必须为每个门分配不到1/3的概率，并且为每个空白墙位置分配一个小概率。像这样的：

[.31, .31, .01, .01, .01, .01, .01, .01, .31, .01]

乍看起来，这种误差似乎是无法克服的。如果传感器有噪音，它会对每一条数据产生怀疑。既然我们为每个位置都要分配一个概率，那么现在我们必须考虑由传感器噪声引起的额外不确定性。

假设我们得到一个门的传感器结果，并且假设传感器正确的可能性是错误的3倍。在有门的地方，我们应该把概率分布按3的比例放大。如果我们这样做，结果将不再是一个概率分布，因为各种可能性的总和不为1了。但我们将学习如何解决这一问题。

让我们看看Python代码。这里我用变量z来表示测量值。z或y是文献中测量的惯用选择。作为一名程序员，我更喜欢有意义的变量名，但我希望您能够阅读其他的文献或者代码，所以我现在就开始介绍这些缩写名。

def update_belief(hall, belief, z, correct_scale):
      for i, val in enumerate(hall):
        if val == z:
          belief[i] *= correct_scale
    
    hallway = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0])
    belief = np.array([0.1] * 10)
    reading = 1                 # 1 is 'door'
    update_belief(hallway, belief, z=reading, correct_scale=3.)
    print('belief:', belief)
    print('sum =', sum(belief))
    plt.figure()
    bar_plot(belief)

belief: [0.3 0.3 0.1 0.1 0.1 0.1 0.1 0.1 0.3 0.1]
    sum = 1.6000000000000003

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 1]

这不是一个概率分布，因为它的总和不是1.0。但是代码做的大部分是正确的——门被分配了一个是墙3倍的概率（0.3）。接下去我们所要做的就是对结果进行`归一化`，使概率总和正确地等于1.0。通过将每个元素除以列表中所有元素的总和来实现归一化。NumPy很容易做到：

belief / sum(belief)

array([0.188, 0.188, 0.062, 0.062, 0.062, 0.062, 0.062, 0.062, 0.188,
        0.062])

FilterPy通过`normalize()`函数实现这一点：

from filterpy.discrete_bayes import normalize
    normalize(belief)

上文提到的，对的可能性是错的可能性的三倍，这种说法有点奇怪。我们既然使用概率来解决问题，所以让我们直接指定传感器正确的概率，并从中计算两者的比例因子。这个方程是：

s c a l e = p r o b c o r r e c t p r o b i n c o r r e c t = p r o b c o r r e c t 1 − p r o b c o r r e c t scale = \\frac\{prob\_\{correct\} \}\{prob\_\{incorrect\} \} = \\frac\{prob\_\{correct\} \}\{1 - prob\_\{correct\} \} scale=probincorrectprobcorrect=1−probcorrectprobcorrect

另外，上述代码的for循环也很麻烦。一般来说，我希望避免在NumPy代码中使用for循环。NumPy是用C和Fortran实现的，因此如果避免for循环，效率通常比等效循环快100倍。

我们如何摆脱这个循环？NumPy允许使用布尔数组对数组进行索引。使用逻辑运算符创建布尔数组。我们可以在走廊里找到所有的门：

hallway = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0])
    hallway == 1

array([ True,  True, False, False, False, False, False, False,  True,
        False])

当使用布尔数组作为另一个数组的索引时，它只返回索引为真的元素。因此，我们可以用

belief[hall==z] *= scale

这句代码的意思是，只有等于z的元素才会乘以scale比例。

教你如何使用NumPy不在本书的范围之内。我比较惯用NumPy结构，并只在第一次呈现它们时解释它们。如果你刚接触到NumPy，有许多博客文章和视频讲解如何使用高效地使用。

这是我们的改进版本：

from filterpy.discrete_bayes import normalize
    
    def scaled_update(hall, belief, z, z_prob):
      scale = z_prob / (1. - z_prob)
      belief[hall==z] *= scale
      normalize(belief)
    
    hallway = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0])
    belief = np.array([0.1] * 10)
    scaled_update(hallway, belief, z=1, z_prob=.75)
    
    print('sum =', sum(belief))
    print('probability of door =', belief[0])
    print('probability of wall =', belief[2])
    bar_plot(belief, ylim=(0, .3))

sum = 1.0
    probability of door = 0.1875
    probability of wall = 0.06249999999999999

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 2]

我们可以从输出中看到，现在所有概率的总和是1.0，门对墙的概率仍然是原来的三倍。这个结果也符合我们的直觉，门的概率必须小于0.333，墙的概率必须大于0.0。最后，它应该符合我们的直觉，我们还没有得到任何信息，使我们能够区分任何给定的门或墙的位置，所以所有的门的位置应该有相同的价值，这同样适用于墙的位置。

这个结果叫做[后验概率分布][Link 6]（posterior），这是**纳入测量信息后的概率分布（后验代表测量后的意思）**。回顾一下，先验是包含测量信息之前的概率分布。

另一个术语是[似然概率][Link 7]（likelihood）。当我们计算`belief[hall==z] *= scale`时，我们实际上在计算每个位置和测量值有多相似。似然概率不是一个概率分布，因为它的和不是1，甚至似然的值可能都大于1。因为**似然只是为了描述先验和测量值的相似度**，至于最终结果归一化，有单独的模块进行。

先验和后验的结合关系，可以给出一个方程式：

p o s t e r i o r = l i k e l i h o o d × p r i o r n o r m a l i z a t i o n posterior = \\frac\{likelihood × prior\}\{normalization\} posterior=normalizationlikelihood×prior

当我们谈论滤波器的输出时，我们通常将执行预测（predict）操作后的状态称为`prior`或`prediction`，并将更新（update）操作后的状态称为`posterior`或`estimated state`。

学习并理解这些术语是非常重要的，因为大多数文献都广泛使用它们。

`scaled_update()`是否也按这个逻辑执行的？是的。让我把它改写成这样：

def scaled_update(hall, belief, z, z_prob): 
      scale = z_prob / (1. - z_prob)
      likelihood = np.ones(len(hall))
      likelihood[hall==z] *= scale
      return normalize(likelihood * belief)

此函数不是完全通用的。它包含关于走廊信息的参数，以及我们如何将测量值与之匹配。我们总是努力编写更通用性的函数。在这里，我们将从函数中删除似然的计算，并要求调用者自己计算似然。

以下是算法的完整实现：

def update(likelihood, prior):
      return normalize(likelihood * prior)

似然概率的计算因问题而异。例如，传感器可能不只是返回1或0，而是一个介于0和1之间的浮点数，表示在门前的概率。它可能使用计算机视觉并返回视野中是一个门的可能性，可能使用声纳并返回一个距离读数。在每种情况下，似然的计算都是不同的。我们将在本书中看到许多这样的例子，并学习如何执行这些计算。

FilterPy实现了上述`update()`的内容，可以直接调用。以下是前一个示例完全通用的代码：

from filterpy.discrete_bayes import update
    
    def lh_hallway(hall, z, z_prob):
      try:
        scale = z_prob / (1. - z_prob)
      except ZeroDivisionError:
        scale = 1e8
      likelihood = np.ones(len(hall))
      likelihood[hall == z] *= scale
      return likelihood
    
    hallway = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0])
    belief = np.array([0.1] * 10)
    likelihood = lh_hallway(hallway, z=1, z_prob=.75)
    update(likelihood, belief)

array([0.188, 0.188, 0.062, 0.062, 0.062, 0.062, 0.062, 0.062, 0.188,
        0.062])

### 考虑狗开始运动 ###

回想一下，当我们融合了一系列的测量和运动更新后，我们很迅速地找到了一个精确的结果（比如门、右移1个单位、门，我们会迅速锁定现在狗的位置在第2个门前）。然而，这发生在一个完美的传感器虚构的世界。我们能找到一个有噪声传感器的精确解决方案吗？

不幸的是，答案是否定的。即使传感器读数与极其复杂的走廊地图完全吻合，我们也不能100%确定狗在一个特定的位置——毕竟，每个传感器的读数都有很小的可能性是错误的！当然，在更典型的情况下，大多数传感器读数都是正确的，我们可能接近100%确定我们的答案，但从来不是100%确定。这听起来可能很复杂，但让我们继续编写代码。

首先让我们来处理一个简单的情况——假设传感器对狗的位置移动判断是完美的，它报告狗已经向右移动了一个空间。我们将如何改变我们的概率值呢？

经过一番思考之后，我觉得我们应该把所有的值向右移动一个空格。例如：如果我们之前认为Simon有50%的概率在第3位，那么在他移到右边一个位置后，我们应该相信他有50%的可能性在第4位。由于走廊是圆形的，所以我们将使用模运算来执行移位。

def perfect_predict(belief, move):
        n = len(belief)
        result = np.zeros(n)
        for i in range(n):
            result[i] = belief[(i - move) % n]
        return result
    
    belief = np.array([.35, .1, .2, .3, 0, 0, 0, 0, 0, .05])
    plt.subplot(121)
    bar_plot(belief, title='Before prediction', ylim=(0, .4))
    
    belief = perfect_predict(belief, 1)
    plt.subplot(122)
    bar_plot(belief, title='After prediction', ylim=(0, .4))

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 3]

我们可以看到，我们正确地将所有值向右移动了一个位置，数组的末尾也换到了数组的开头。

下面的代码通过设置交互式控件，以便你可以看到它的实际变化。使用滑块在时间上前后移动。这模拟了Simon在走廊里走来走去。它还没有纳入新的测量，所以概率分布不改变值，只改变位置。

from ipywidgets import interact, IntSlider
    
    belief = np.array([.35, .1, .2, .3, 0, 0, 0, 0, 0, .05])
    perfect_beliefs = []
    
    for _ in range(20):
      belief = perfect_predict(belief, 1)
      perfect_beliefs.append(belief)
    
    def simulate(time_step):
      bar_plot(perfect_beliefs[time_step], ylim=(0, .4))
    
    interact(simulate, time_step=IntSlider(value=0, max=len(perfect_beliefs) - 1))

interactive(children=(IntSlider(value=0, description='time_step', max=19), Output()), _dom_classes=('widget-interact',))

### 增加预测的不确定性 ###

`perfect_predict()`假设传感器对狗的位置移动判断是完美地，但所有传感器都有噪声。如果传感器报告说我们的狗移动了一个空间，但它实际上移动了两个空间，或者说是零呢？这听起来也像是一个无法克服的问题，但让我们对它进行建模，看看会发生什么。

假设传感器的移动测量值80%可能是正确的，10%可能是向右偏离一个位置，10%可能是向左偏离一个位置。也就是说，如果移动度量为4（即向右移动4个空格），则狗有80%可能向右移动4个空格，10%可能向右移动3个空格，10%可能向右移动5个空格。

这样，数组中的每个结果现在需要包含3种不同情况的概率。例如，如果我们100%确定狗是从位置3开始的，那么它有80%的概率是5，4或6的概率是10%；而在仅考虑一次向右移动1个单位的情况下，狗的位置最终在3的概率有可能是2向右的80%，1向右的10%，3向右的10%。让我们试着编写代码：

def plot_belief_vs_prior(belief, prior, **kwargs):
      plt.subplot(121)
      bar_plot(belief, title='belief', **kwargs)
      plt.subplot(122)
      bar_plot(prior, title='prior', **kwargs)
      plt.show()
    
    def predict_move(belief, move, p_under, p_correct, p_over):
      n = len(belief)
      prior = np.zeros(n)
      for i in range(n):
        prior[i] = (
          belief[(i-move) % n]   * p_correct +
          belief[(i-move-1) % n] * p_over +
          belief[(i-move+1) % n] * p_under)
      return prior
    
    belief = [0., 0., 0., 1., 0., 0., 0., 0., 0., 0.]
    prior = predict_move(belief, 2, .1, .8, .1)
    plot_belief_vs_prior(belief, prior)

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 4]

它似乎工作正常。当我们的起始概率不是100%确定时会发生什么？

belief = [0, 0, .4, .6, 0, 0, 0, 0, 0, 0]
    prior = predict_move(belief, 2, .1, .8, .1)
    plot_belief_vs_prior(belief, prior)

array([0.  , 0.  , 0.  , 0.04, 0.38, 0.52, 0.06, 0.  , 0.  , 0.  ])

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 5]

这里的结果更复杂，但你应该仍然能够在你的头脑中解决它。0.04是因为0.4可能移动1个单位。0.38是由于以下原因：80%的概率0.4移动了2个单位（ 0.4 × 0.8 0.4\\times 0.8 0.4×0.8），10%的概率0.6移动1个单位（ 0.6 × 0.1 0.6\\times 0.1 0.6×0.1）。以此类推。我强烈建议这个示例的每个值的计算过程都要都非常清楚，因为接下来的大部分内容取决于对这一步骤的理解。

如果你查看执行后的概率，你可能会感到沮丧。在上面的例子中，我们从两个位置的概率0.4和0.6开始；在**执行预测操作之后，概率不仅降低了，而且分布在地图上**（产生了更多的可能）。

这不是巧合，也不是精心挑选的例子的结果——预测的结果是没有问题的。

也就是说，如果传感器没有噪声，直接移动位置就可以了，信息没有丢失；**如果传感器是有噪声的，我们对于每个预测都会丢失一些信息，导致概率稍微平均一些**。假设我们进行无限次的预测，结果会是什么？如果我们在每一步都丢失了信息，那么我们最终将一点信息都没有，我们的概率将平均分布在整个概率数组中。让我们尝试100次迭代。绘图已设置交互式控件，使用滑块来更改步数。

belief = np.array([1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
    predict_beliefs = []
    
    for i in range(100):
      belief = predict_move(belief, 1, .1, .8, .1)
      predict_beliefs.append(belief)
    
    print('Final Belief:', belief)
    
    def show_prior(step):
      bar_plot(predict_beliefs[step-1])
      plt.title('Step {}'.format(step))
    
    interact(show_prior, step=IntSlider(value=1, max=len(predict_beliefs)))

Final Belief: [0.104 0.103 0.101 0.099 0.097 0.096 0.097 0.099 0.101 0.103]
    interactive(children=(IntSlider(value=1, description='step'), Output()), _dom_classes=('widget-interact',))

尽管我们100%确定我们从位置0开始，但经过100次预测迭代之后，我们几乎丢失了所有的信息。你可以随意查看不同更新次数的效果。

如果不想输入代码调试，这里有一个输出的动画。

![在这里插入图片描述][20210124051632694.gif_pic_center]

### 利用卷积推广 ###

我们之前假设传感器的运动误差值最多为一个位置，但实际上误差可能是两个、三个或更多的位置。作为程序员，我们总是希望对代码进行泛化，使其适用于所有情况。

这个问题用[卷积][Link 8]很容易解决，卷积是用一个函数修改另一个函数。在我们的例子中，我们用传感器的误差函数来修正概率分布。predict\_move()的实现是一个卷积，只是我们没有这样称呼它。形式上，卷积定义为：

( f ∗ g ) ( t ) = ∫ 0 t ! f ( τ ) g ( t − τ ) d ( τ ) (f \* g)(t) = \\int\_\{0\}^\{t\} !f(\\tau ) g(t - \\tau )d(\\tau ) (f∗g)(t)=∫0t!f(τ)g(t−τ)d(τ)

其中， f ∗ g f \* g f∗g是将f与g进行卷积的表示法，并不表示乘法。

积分用于连续函数，但我们使用离散函数。我们用求和代替积分，用数组括号代替圆括号。

( f ∗ g ) \[ t \] = ∑ τ = 0 t ! f \[ τ \] g \[ t − τ \] (f \* g)\[t\] = \\sum\_\{\\tau = 0\}^\{t\} !f\[\\tau \] g\[t - \\tau \] (f∗g)\[t\]=τ=0∑t!f\[τ\]g\[t−τ\]

比较之后发现，predict\_move()就是使用这个方程计算的————它计算了一系列乘法的和。

Khan Academy对卷积有很好的介绍，Wikipedia有一些关于卷积的优秀动画。但总体思路已经很清楚了。**将一个名为kernel的数组滑动到另一个数组上，将当前单元格的邻居与kernel数组的值相乘**。在上面的例子中，我们使用0.8表示移动到正确位置的概率，0.1表示正确位置上一个位置，0.1表示正确位置下一个位置。我们用数组 \[ 0.1 , 0.8 , 0.1 \] \[0.1, 0.8, 0.1\] \[0.1,0.8,0.1\]来做一个kernel内核。我们所要做的就是写一个循环，遍历数组的每个元素，乘以内核，然后对结果求和。为了强调这个概率是一个概率分布，我把它命名为pdf。

def predict_move_convolution(pdf, offset, kernel):
      N = len(pdf)
      kN = len(kernel)
      width = int((kN - 1) / 2)
    
      prior = np.zeros(N)
      for i in range(N):
        for k in range(kN):
          index = (i + (width-k) - offset) % N
          prior[i] += pdf[index] * kernel[k]
      return prior

这段代码实现了算法的逻辑，但运行速度非常慢。SciPy在`ndimage.filters`文件模块提供了卷积程序`convolve()`。我们还需要在卷积之前将pdf偏移，`np.roll()`可以实现。预测算法可以用一行实现：

from scipy.ndimage.filters import convolve
    
    convolve(np.roll(pdf, offset), kernel, mode='wrap')

FilterPy使用`discrete_bayes`的`predict()`函数来实现这一点。

from filterpy.discrete_bayes import predict
    
    belief = [.05, .05, .05, .05, .55, .05, .05, .05, .05, .05]
    prior = predict(belief, offset=1, kernel=[.1, .8, .1])
    plot_belief_vs_prior(belief, prior, ylim=(0,0.6))

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 6]

除了中间的元素外，其他的元素都没有变化。位置4和6中的值应为：

( 0.1 × 0.05 ) + ( 0.8 × 0.05 ) + ( 0.1 × 0.55 ) = 0.1 (0.1 \\times 0.05) + (0.8 \\times 0.05) + (0.1 \\times 0.55) = 0.1 (0.1×0.05)\+(0.8×0.05)\+(0.1×0.55)=0.1

位置5应为:

( 0.1 × 0.05 ) + ( 0.8 × 0.55 ) + ( 0.1 × 0.05 ) = 0.45 (0.1 \\times 0.05) + (0.8 \\times 0.55) + (0.1 \\times 0.05) = 0.45 (0.1×0.05)\+(0.8×0.55)\+(0.1×0.05)=0.45

如果是大于1个单位的移动，并且kernel内核不对称的话，也能正确地移动位置吗？

prior = predict(belief, offset=3, kernel=[.05, .05, .6, .2, .1])
    plot_belief_vs_prior(belief, prior, ylim=(0,0.6))

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 7]

位置被正确地移动了3个位置，所以这看起来是正确的。

确保你明白我们在做什么：我们正在预测狗的移动位置，并对概率进行卷积以获得先验信息。

如果我们不使用概率，我们会使用我之前给出的等式：

x ˉ k + 1 = f x ( ∙ ) + x k \\bar\{x\} \_\{k + 1\} = f\_\{\\mathbf \{x\}\}(\\bullet ) + x\_\{k\} xˉk\+1=fx(∙)\+xk

我们预测狗会在哪里，是狗移动的距离加上它当前的位置。那只狗当时位置是2，他移动的距离是5，所以现在他的位置是7了，这再简单不过了。但是我们用概率来建模，所以我们的等式是：

X ˉ k + 1 = X k ∗ f x ( ∙ ) \\bar \{\\mathbf \{X\}\} \_\{k+1\} = \\mathbf \{X\} \_\{k\} \*f\\mathbf \{x\}(\\bullet ) Xˉk\+1=Xk∗fx(∙)

这和我们之前的做法是一样的：将当前的位置的概率与我们认为狗移动了多少的概率进行卷积。

两个等式实际上是同一个概念，只是数学表示上略有不同。 X \\mathbf \{X\} X粗体表示它是一个数字数组。

### 融合测量更新 ###

**在预测过程中丢失信息的问题，可能会使我们的系统看起来好像很快就会退化为没有信息。然而，每个预测之后都会有一个更新，我们会将测量值融合到估计值中，更新过程会增加我们的信息。更新步骤的输出被输入到下一个预测中，预测过程降低了我们的确定性，但传递到更新过程后，确定性再次增加**。

让我们直观地考虑一下。举个简单的例子————你在跟踪一只狗，而它却一动不动。在每次预测中，你都预测他不会动。你的过滤器很快就收敛到一个准确的位置估计。然后厨房里的微波炉开了，他就跑了。你不知道这一点，所以在下一次预测中，你预测他还在同一地点。但测量结果却说明了一个不同的情况。当你考虑测量值时，你的概率会沿着走廊一直延伸到厨房。在下一个滤波周期中，他坐着不动的概率会变小，你会发现他开始向厨房运动。

这就是直觉告诉我们的。

我们已经对更新和预测步骤进行了编程。我们所要做的就是把一个结果输入到另一个，我们就实现了一个狗跟踪器！！！让我们看看它的表现。我们将输入测量值，就像狗从位置0开始，每个单位时间向右移动一个位置一样。在实际应用中，我们将从不知道他的位置开始，为所有位置分配相等的概率。

def plot_prior_vs_posterior(prior, posterior, reverse=False, **kwargs):
      if reverse:
        plt.subplot(121)
        bar_plot(posterior, title='posterior', **kwargs)
        plt.subplot(122)
        bar_plot(prior, title='prior', **kwargs)
      else:
        plt.subplot(121)
        bar_plot(prior, title='prior', **kwargs)
        plt.subplot(122)
        bar_plot(posterior, title='posterior', **kwargs)
    
    hallway = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0])
    prior = np.array([.1] * 10)
    likelihood = lh_hallway(hallway, z=1, z_prob=.75)
    posterior = update(likelihood, prior)
    plot_prior_vs_posterior(prior, posterior, ylim=(0,.5))

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 8]

在第一次更新之后，我们为每个门位置分配了高概率，为每个墙位置分配了低概率。

kernel = (.1, .8, .1)
    prior = predict(posterior, 1, kernel)
    plot_prior_vs_posterior(prior, posterior, True, ylim=(0,.5))

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 9]

预测一步将这些概率右移，并使它们之间的区别变得稍微模糊。现在让我们看看下一个更新操作发生了什么。

likelihood = lh_hallway(hallway, z=1, z_prob=.75)
    posterior = update(likelihood, prior)
    plot_prior_vs_posterior(prior, posterior, ylim=(0,.5))

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 10]

注意位置1的高条，这与从位置0开始（正确的）情况相对应。感应一个门，向右移动1，然后感应另一个门。没有其他位置能使这组观察结果成为可能。现在我们再添加一个更新操作，测量结果是检测到墙。

likelihood = lh_hallway(hallway, z=0, z_prob=.75)
    posterior = update(likelihood, prior)
    plot_prior_vs_posterior(prior, posterior, ylim=(0,.5))

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 11]

这太让人激动了！我们有一个非常突出的概率值在位置2，约35%。它是图中任何其他概率值的两倍多，比我们上一个图的最高概率值（约为31%）大4%。让我们再看一个滤波周期。

prior = predict(posterior, 1, kernel)
    likelihood = lh_hallway(hallway, z=0, z_prob=.75)
    posterior = update(likelihood, prior)
    plot_prior_vs_posterior(prior, posterior, ylim=(0,.5))

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 12]

但在这个过程中，我忽略了一个重要的问题。早些时候，我假设我们有一个预测步骤的运动传感器；然后，当谈到狗和微波炉时，我假设你不知道他突然开始跑了。我提到过你认为狗在跑会随着时间的推移而增加，但是我没有提供任何代码。简而言之，如果我们没有运动传感器，我们如何预测呢？

现在我想忽略这个问题。在后面的章节中，我们将学习这个预测背后的数学逻辑，但现在仅仅学习这个算法就足够了。解决这个问题是非常重要的，但是我们还没有足够的背景来解决。因此在本章的剩余部分，我们将通过假设我们有一个感知运动的传感器来忽略这个问题。

## 离散贝叶斯算法 ##

此图说明了算法：

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 13]

过滤方程为：

\{ x ˉ = x ∗ f x ( ∙ ) 预 测 步 x = ∣ L ⋅ x ˉ ∣ 更 新 步 \\left\\\{\\begin\{matrix\} \\bar\{\\mathbf \{x\}\} = \\mathbf\{x\} \* f\_\{\\mathbf\{x\} \}(\\bullet ) & 预测步\\\\ \\mathbf\{x\} = \\left | \\mathcal\{L\} \\cdot \\bar\{\\mathbf\{x\}\} \\right | & 更新步 \\end\{matrix\}\\right. \{ xˉ=x∗fx(∙)x=∣L⋅xˉ∣预测步更新步

其中， L \\mathcal\{L\} L是似然函数的常用表示符号， ∣ ∣ \\left | \\right | ∣∣符号表示取范数。我们需要将似然与先验的乘积归一化，以确保 x \\mathbf\{x\} x是一个概率分布，其总和为1。

我们也可以用伪代码来表示。

初始化设置

1.  初始化各个状态的值

预测

1.  根据系统行为，预测下一时间的状态
2.  调整状态的值以解释预测中的不确定性

更新

1.  得到一个测量值和它的准确度
2.  计算测量值与每个状态匹配的相似度
3.  通过似然更新状态的值

当我们讨论卡尔曼滤波器时，我们将使用完全相同的算法，只是计算的细节不同。

这种形式的算法有时被称为`预测——更新算法`。我们做一个预测，然后修正它们。

让我们设置交互式控件。首先，让我们编写函数来执行滤波并在任何步骤绘制结果。我用黑色标出了门口的位置。先验是橙色标出的，后验是蓝色标出的。我画了一条粗的垂直线来表示Simon到底在哪里。这不是过滤器的输出——我们知道Simon在哪里只是因为我们在模拟他的运动。

def discrete_bayes_sim(prior, kernel, measurements, z_prob, hallway):
      posterior = np.array([.1] * 10)
      priors, posteriors = [], []
      for i, z in enumerate(measurements):
        prior = predict(posterior, 1, kernel)
        priors.append(prior)
    
        likelihood = lh_hallway(hallway, z, z_prob)
        posterior = update(likelihood, prior)
        posteriors.append(posterior)
      return priors, posteriors
    
    def plot_posterior(hallway, posteriors, i):
      plt.title('Posterior')
      bar_plot(hallway, c='k')
      bar_plot(posteriors[i], ylim=(0, 1.0))
      plt.axvline(i % len(hallway), lw=5)
    
    def plot_prior(hallway, priors, i):
      plt.title('Prior')
      bar_plot(hallway, c='k')
      bar_plot(priors[i], ylim=(0, 1.0), c='#ff8015')
      plt.axvline(i % len(hallway), lw=5)
    
    def animate_discrete_bayes(hallway, priors, posteriors):
      def animate(step):
        step -= 1
        i = step // 2
        if step % 2 == 0:
          plot_prior(hallway, priors, i)
        else:
          plot_posterior(hallway, posteriors, i)
      return animate

让我们运行滤波器并设置交互式控件，以便获得更深刻的理解。

kernel = (.1, .8, .1)
    z_prob = 1.0
    hallway = np.array([1, 1, 0, 0, 0, 0, 0, 0, 1, 0])
    prior = np.array([.1] * 10)
    
    # measurements with no noise
    zs = [hallway[i % len(hallway)] for i in range(50)]
    
    priors, posteriors = discrete_bayes_sim(prior, kernel, zs, z_prob, hallway)
    interact(animate_discrete_bayes(hallway, priors, posteriors), step=IntSlider(value=1, max=len(zs)*2))

interactive(children=(IntSlider(value=1, description='step'), Output()), _dom_classes=('widget-interact',))

现在我们可以看到结果了。你可以看到**先验是如何改变位置并减少确定性，后验是如何保持位置并增加确定性**，因为它包含了测量的信息。我已经使用`z_prob = 1.0`使测量噪声减少，使用`zs = hallway[..]`使测量与真实值一致。下面我们将探讨不良测量值的影响。

另一个需要注意的是，当我们站在门前时，我们的估计是多么准确；而当我们站在墙前时，我们的估计又是如何下降的，这应该是直观的。由于只有几个门口，所以当传感器告诉我们我们在门前时，这就增强了我们在位置上的确定性。一段很长的无门地带减少了我们的确定性。

## 不良传感器数据的影响 ##

你可能会对上述结果产生怀疑，因为我总是将正确的传感器数据传递到函数中。然而，我们声称这段代码实现了一个滤波器——它应该过滤掉坏的传感器测量。它能做到吗？

为了便于编程和可视化，我将把走廊的布局修改一下，并在6个正确的测量值上运行算法：

kernel = (.1, .8, .1)
    z_prob = 0.75
    hallway = np.array([1, 0, 1, 0, 0] * 2)
    prior = np.array([.1] * 10)
    
    zs = [1, 0, 1, 0, 0, 1]
    
    priors, posteriors = discrete_bayes_sim(prior, kernel, zs, z_prob, hallway)
    interact(animate_discrete_bayes(hallway, priors, posteriors), step=IntSlider(value=12, max=len(zs)*2))

interactive(children=(IntSlider(value=12, description='step', max=12), Output()), _dom_classes=('widget-interact',))

我们已经确定了从位置0或5开始的可能情况，因为我们看到了门和墙的顺序：1, 0, 1, 0, 0, 1。现在我注入了一个错误的测量，下一个测量值应该是0，但是我们得到的是1：

measurements = [1, 0, 1, 0, 0, 1, 1]
    priors, posteriors = discrete_bayes_sim(prior, kernel, measurements, z_prob, hallway)
    plot_posterior(hallway, posteriors, 6)

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 14]

那一次糟糕的测量大大影响了我们的结果，现在让我们继续进行一系列正确的测量。

from contextlib import contextmanager
    import matplotlib.pylab as pylab
    import matplotlib as mpl
    
    _default_size = (9, 4)
    
    def set_figsize(x=_default_size[0], y=_default_size[1]):
        """ set the figure size of the plot to the specified size in inches"""
        mpl.rcParams['figure.figsize'] = x, y
    
    @contextmanager
    def figsize(x=8, y=3):
        """Temporarily set the figure size using 'with figsize(a, b):'"""
        size = pylab.rcParams['figure.figsize']
        set_figsize(x, y)
        yield
        pylab.rcParams['figure.figsize'] = size
    
    kernel = (.1, .8, .1)
    z_prob = 0.75
    hallway = np.array([1, 0, 1, 0, 0] * 2)
    prior = np.array([.1] * 10)
    
    with figsize(y=5.5):
        measurements = [1, 0, 1, 0, 0, 1, 1, 1, 0, 0]
        for i, m in enumerate(measurements):
            likelihood = lh_hallway(hallway, z=m, z_prob=.75)
            posterior = update(likelihood, prior)
            prior = predict(posterior, 1, kernel)
            plt.subplot(5, 2, i+1)
            bar_plot(posterior, ylim=(0, .4), title='step {}'.format(i+1))
        plt.tight_layout()

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 15]

可以看到，我们很快过滤掉坏的传感器读数，并集中在我们的狗最有可能的位置。

## 缺点和局限性 ##

不要被我选择的例子的简单性所误导，这是一个健壮且完整的滤波器，你可以在实际解决方案中使用该代码。如果你需要一个多峰的离散滤波器，那么这个滤波器可以胜任。

也就是说，这个过滤器不经常使用，因为它有几个限制。绕过这些限制是本书其余章节背后的动机。

第一个问题是**滤波器的伸缩性**。我们的狗跟踪问题只使用了一个变量pos来表示狗的位置。最有趣的问题是在一个大空间里跟踪多个东西。实际上，我们至少要跟踪狗的(x, y)坐标，可能还有它的速度 ( x ˙ , y ˙ ) (\\dot\{x\} , \\dot\{y\}) (x˙,y˙)。我们没有讨论多维情况，但是我们可以使用多维网格代替数组来存储每个离散位置的概率。每个update()和predict()步骤都需要更新网格中的所有值，所以一个简单的四变量问题需要 O ( n 4 ) O(n^\{4\}) O(n4)时间复杂度。现实的滤波器可以有10个甚至更多的变量需要跟踪，导致过高的计算要求；

第二个问题是**滤波器是离散的**，但是我们生活在一个连续的世界里。直方图要求你将滤波器的输出建模为一组离散点。一个100米的走廊需要10000个位置来模拟走廊，精确到1厘米。因此，每次预测和更新操作都需要对10000个不同的概率进行计算。随着维数的增加，情况会变得更糟。一个 100 × 100 m 2 100\\times 100 m^\{2\} 100×100m2的庭院需要100000000个小块才能获得1cm的精度；

第三个问题是**滤波器是多峰的**。在上一个例子中，我们最终坚信狗处于4或9的位置。我们将在后面研究的粒子滤波器也是多峰的，就是因为多峰性而经常被使用。但是想象一下，如果你车里的GPS告诉你，40%确定你在D街，30%确定你在E街，这是多么崩溃；

第四个问题是，它需要**对状态的变化进行测量**。我们需要一个运动传感器来检测狗的运动。有一些方法可以解决这个问题，但这会使本章的论述复杂化。因此，鉴于上述原因不作进一步讨论。

话虽如此，但如果我有一个该滤波器就足够处理的小问题，我会选择使用它；因为它的理解、实现、调试都很简单。

## 跟踪与控制 ##

我们一直被动地跟踪一个自主运动的物体，但是考虑一个非常相似的问题。我们想搭建一个自动化仓库，希望使用机器人来收集客户订单的所有项目。

也许最简单的方法就是让机器人在轨道上行走，当我们想把机器人送到一个某个目的地，直接让它通过轨道去那里。但是轨道和机器人马达并不完美，车轮打滑和不完美的电机控制意味着机器人不太可能准确地行驶到我们所命令的位置。现在有不止一个机器人，我们需要知道每一个的位置，这样整个机器人群体的调度才会顺畅。

所以我们增加传感器，在轨道上每隔几英尺就安装一块磁铁，然后用霍尔传感器计算通过的磁铁数量。如果我们数到10块磁铁，那么机器人应该在第10块磁铁上。当然，漏磁或计数两次是可能的，所以我们必须考虑一定程度的误差。我们可以使用上文的代码来跟踪我们的机器人，因为磁铁计数与门口感应非常相似。

但我们不这样做，我们需要学会永远不要丢弃信息。如果你有信息，你应该用它来提高你的估计。我们遗漏了什么信息？因为我们知道每一时刻给机器人的轮子提供的控制输入。例如，假设我们每秒钟向机器人发送一次移动命令————向左移动1个单位，向右移动1个单位，或静止不动。如果我发出向左移动1个单位的命令，我希望从现在起一秒钟内机器人将向它现在所在位置的左侧移动1个单位。但轮子和马达不完美，机器人最终可能会离开0.9个单位，或者1.2个单位。

现在整个解决方案都清楚了。

### 模拟列车行为 ###

我们需要模拟一列不完美的火车。当我们命令它移动时，它有时会犯一个小错误，它的传感器有时会返回不正确的值。

class Train(object):
      def __init__(self, track_len, kernel=[1.], sensor_accuracy=.9):
        self.track_len = track_len
        self.pos = 0
        self.kernel = kernel
        self.sensor_accuracy = sensor_accuracy
    
      def move(self, distance=1):
        """ move in the specified direction with some small chance of error"""
        self.pos += distance
        # insert random movement error according to kernel
        r = random.random()
        s = 0
        offset = -(len(self.kernel) - 1) / 2
        for k in self.kernel:
          s += k
          if r <= s:
            break
          offset += 1
        self.pos = int((self.pos + offset) % self.track_len)
        return self.pos
    
      def sense(self):
        pos = self.pos
        # insert random sensor error
        if random.random() > self.sensor_accuracy:
          if random.random() > 0.5:
            pos += 1
          else:
            pos -= 1
        return pos

这样我们就可以编写滤波器了。我们将把它放在一个函数中，这样我们就可以用不同的假设来运行它。我假设机器人总是从轨道的起点开始。磁道的长度是10个单位，同时该磁道也是一个循环磁道，长度为10是为了绘图和检查更容易。

def train_filter(iterations, kernel, sensor_accuracy,
          move_distance, do_print=True):
      track = np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9])
      prior = np.array([.9] + [0.01] * 9)
      posterior = prior[:]
      normalize(prior)
    
      robot = Train(len(track), kernel, sensor_accuracy)
      for i in range(iterations):
        # move the robot and
        robot.move(distance=move_distance)
    
        # peform prediction
        prior = predict(posterior, move_distance, kernel)
    
        # and update the filter
        m = robot.sense()
        likelihood = lh_hallway(track, m, sensor_accuracy)
        posterior = update(likelihood, prior)
        index = np.argmax(posterior)
    
        if do_print:
          print('''time {}: pos {}, sensed {}, '''
              '''at position {}'''.format(
            i, robot.pos, m, track[robot.pos]))
    
          print(''' estimated position is {}'''
              ''' with confidence {:.4f}%:'''.format(
            index, posterior[index] * 100))
    
      bar_plot(posterior)
      if do_print:
        print()
        print('final position is', robot.pos)
        index = np.argmax(posterior)
        print('''Estimated position is {} with '''
            '''confidence {:.4f}%:'''.format(
          index, posterior[index] * 100))

阅读代码并确保你理解它，现在让我们在没有传感器或移动错误的情况下运行。如果代码是正确的，它应该能够定位机器人没有错误。阅读输出有点乏味，但是如果你完全不确定预测/更新周期是如何工作的，请确保仔细阅读它以巩固您的理解。

import random
    
    random.seed(3)
    np.set_printoptions(precision=2, suppress=True, linewidth=60)
    train_filter(4, kernel=[1.], sensor_accuracy=.999,
          move_distance=4, do_print=True)

time 0: pos 4, sensed 4, at position 4
            estimated position is 4 with confidence 99.9900%:
    time 1: pos 8, sensed 8, at position 8
            estimated position is 8 with confidence 100.0000%:
    time 2: pos 2, sensed 2, at position 2
            estimated position is 2 with confidence 100.0000%:
    time 3: pos 6, sensed 6, at position 6
            estimated position is 6 with confidence 100.0000%:
    
    final position is 6
    Estimated position is 6 with confidence 100.0000%:

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 16]

我们可以看到，代码能够完美地跟踪机器人，所以我们应该有足够的信心，代码正在工作。现在让我们看看它是如何处理一些错误的。

random.seed(5)
    train_filter(4, kernel=[.1, .8, .1], sensor_accuracy=.9,
          move_distance=4, do_print=True)

time 0: pos 4, sensed 4, at position 4
            estimated position is 4 with confidence 96.0390%:
    time 1: pos 8, sensed 9, at position 8
            estimated position is 9 with confidence 52.1180%:
    time 2: pos 3, sensed 3, at position 3
            estimated position is 3 with confidence 88.3993%:
    time 3: pos 7, sensed 8, at position 7
            estimated position is 8 with confidence 49.3174%:
    
    final position is 7
    Estimated position is 8 with confidence 49.3174%:

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 17]

在时间1处有一个感应错误，但是我们仍然对我们的位置很有信心。

现在让我们运行一个较长时间的模拟，看看滤波器如何响应错误。

with figsize(y=5.5):
      for i in range (4):
        random.seed(3)
        plt.subplot(221+i)
        train_filter(148+i, kernel=[.1, .8, .1],
              sensor_accuracy=.8,
              move_distance=4, do_print=False)
        plt.title ('iteration {}'.format(148+i))

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 18]

我们可以看到，当置信度降低时，迭代149出现了一个问题。但在几次迭代后，滤波器能够自我校正并重新获得估计位置的置信度。

## 贝叶斯定理与全概率定理 ##

在这一章中，我们可以发现[贝叶斯定理][Link 9]和[全概率定理][Link 10]的影子。

**贝叶斯定理告诉我们如何在给定的先验信息下计算事件发生的概率**。我们使用以下公式实现update()函数：

p o s t e r i o r = l i k e l i h o o d × p r i o r n o r m a l i z a t i o n \_ f a c t o r posterior = \\frac\{likelihood \\times prior\}\{normalization\\\_factor\} posterior=normalization\_factorlikelihood×prior

本书中的每一个滤波器实际上都是贝叶斯定理的表达式。在下一章中，我们将讨论贝叶斯公式背后的数学理论，但这也掩盖不了贝叶斯定理的简单想法：

u p d a t e d , k n o w l e d g e = ∣ l i k e l i h o o d \_ o f \_ n e w \_ k n o w l e d g e × p r i o r \_ k n o w l e d g e ∣ updated,knowledge = \\left | likelihood\\\_of\\\_new\\\_knowledge \\times prior\\\_knowledge\\right | updated,knowledge=∣∣likelihood\_of\_new\_knowledge×prior\_knowledge∣∣

其中， ∣ ∣ \\left | \\right | ∣∣表示归一化。

同样，predict()步骤计算多个可能事件的总概率，这就是统计学中的全概率定理。我们将在下一章数学理论讲解中讨论这个问题。

## 总结 ##

代码很短，但结果令人印象深刻！我们已经实现了一种形式的贝叶斯滤波器。

我们已经学会了如何从没有信息开始，从有噪声的传感器中获取信息。尽管本章中的传感器非常嘈杂（例如，大多数传感器的准确率超过80%），但我们很快就会收敛到最可能的位置。我们已经了解了预测步骤总是如何降低我们的概率，但是添加另一个测量，即使其中可能有噪声，也会提高我们的概率，使我们能够收敛到最可能的结果。

本书主要是关于卡尔曼滤波器的。它使用的数学公式是不同的，但逻辑和本章中使用的是完全相同的。

如果你能理解这一章，你将能够理解和实施卡尔曼滤波器。我怎么强调都不过分。如果有什么不清楚的地方，请重读本章并跟随代码理解。本书的其余部分也以我们在这里使用的算法为基础。如果你不明白为什么这个滤波器可以工作，剩下来的章节可能也很难理解。

## 相关阅读 ##

*  [Kalman-and-Bayesian-Filters-in-Python/02-Discrete-Bayes][02-Discrete-Bayes]
 *  [你对贝叶斯统计都有怎样的理解？][Link 11]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly95bmd6bWlhby5ibG9nLmNzZG4ubmV0_size_16_color_FFFFFF_t_70][]

[rlabbe_Kalman-and-Bayesian-Filters-in-Python]: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python
[02-Discrete-Bayes]: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/02-Discrete-Bayes.ipynb
[https_github.com_yngzMiao_yngzmiao-blogs_tree_master_2021Q1_20210125]: https://github.com/yngzMiao/yngzmiao-blogs/tree/master/2021Q1/20210125
[Link 1]: https://en.wikipedia.org/wiki/Bayesian_probability
[Link 2]: https://en.wikipedia.org/wiki/Prior_probability
[Link 3]: https://en.wikipedia.org/wiki/Probability_distribution
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center]: /images/20221119/74301172113b4479b0193e569fbdb184.png
[Link 4]: https://en.wikipedia.org/wiki/Categorical_distribution
[Link 5]: https://en.wikipedia.org/wiki/Multimodal_distribution
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 1]: /images/20221119/7df8a237c10044829ca16e4b005bfcd8.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 2]: https://img-blog.csdnimg.cn/20210124051509218.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[Link 6]: https://en.wikipedia.org/wiki/Posterior_probability
[Link 7]: https://en.wikipedia.org/wiki/Likelihood_function
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 3]: https://img-blog.csdnimg.cn/2021012405145695.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 4]: https://img-blog.csdnimg.cn/20210124051429127.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 5]: https://img-blog.csdnimg.cn/20210124051351165.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[20210124051632694.gif_pic_center]: https://img-blog.csdnimg.cn/20210124051632694.gif#pic_center
[Link 8]: https://en.wikipedia.org/wiki/Convolution
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 6]: https://img-blog.csdnimg.cn/20210124051700861.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 7]: https://img-blog.csdnimg.cn/20210124051731554.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 8]: https://img-blog.csdnimg.cn/20210124051809555.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 9]: https://img-blog.csdnimg.cn/20210124051828816.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 10]: https://img-blog.csdnimg.cn/20210124051845314.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 11]: https://img-blog.csdnimg.cn/20210124051857263.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 12]: https://img-blog.csdnimg.cn/20210124051911614.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 13]: https://img-blog.csdnimg.cn/20210124051929110.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 14]: https://img-blog.csdnimg.cn/20210124051953869.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 15]: https://img-blog.csdnimg.cn/20210124052006632.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 16]: https://img-blog.csdnimg.cn/20210124052024374.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 17]: https://img-blog.csdnimg.cn/20210124052037770.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw_size_16_color_FFFFFF_t_70_pic_center 18]: https://img-blog.csdnimg.cn/2021012405205063.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM4NDEwNzMw,size_16,color_FFFFFF,t_70#pic_center
[Link 9]: https://en.wikipedia.org/wiki/Bayes%27_theorem
[Link 10]: https://en.wikipedia.org/wiki/Law_of_total_probability
[Link 11]: https://www.zhihu.com/question/21134457
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly95bmd6bWlhby5ibG9nLmNzZG4ubmV0_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20190309211249199.jpg?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly95bmd6bWlhby5ibG9nLmNzZG4ubmV0,size_16,color_FFFFFF,t_70