第二章：迭代状态估计
2.1 介绍
概率机器人学的核心理念就是根据传感器的数据估计状态。所谓的状态估计就是要得到一些传感器数据中不能直接得出，却可以推导出的一些定量描述。 在很多机器人应用中，只有明确了特定的定量属性才可以相对容易的做出决策。比如说，对于移动机器人而言，如果我们知道了它的位置以及周围的障碍物，就能够轻松的控制它了。 不幸的是，这些变量都不能直接获得。实际上，机器人依靠它的传感器来收集信息，而传感器只能采集到关于这些变量的部分信息，而且其测量值还存在一定的噪声。 状态估计就是要从这些数据中寻找并恢复状态变量。概率学的状态估计算法计算整个状态空间下的置信度分布(belief distribution)。 在本书的介绍章节中就已经看到了一个概率学状态估计的例子：移动机器人的定位。
本章是要介绍从传感器数据中估计状态需要用到的基本名词和数学工具。
- 2.2节介绍本书中用到的基本的概率论概念。
- 2.3节描述机器人环境交互(robot environment interaction)的形式化模型, 对于本书中用到的一些关键术语做出解释。
- 2.4节介绍贝叶斯滤波器Bayes filters，它是一个用于状态估计的迭代算法，基本上是本书所提到的所有技术的基石。
- 2.5节讨论贝叶斯滤波器在应用过程中面临的表示(representational)和计算(computational)问题。
2.2 概率论的基本概念
本节介绍一些概率论的基本概念和定理。在概率机器人学中，传感器的测量值、控制量、机器人和环境的状态等变量都被认为是随机变量random variables。 它们可以有多个取值，并且满足特定的概率学定律。所谓概率学推理就是根据概率学定律，从其他随机变量以及观测数据中，计算目标随机变量的过程。
令\(X\)表示一个随机变量，\(x\)为\(X\)的一个可能取值。一个典型的随机变量的例子就是硬币的正反面，\(X\)只有正面和反面两个可能的取值。 如果变量\(X\)像这里的硬币正反面一样，取值空间是离散的，那么我们可以用如下符号表示\(X\)取值为\(x\)的概率: $$ \begin{equation}\label{f1} p(X=x) \end{equation} $$ 对于一个均匀的硬币，有\(p(X=正面) = p(X=反面) = \frac{1}{2}\)。取值空间中所有的概率之和和为1，即 $$ \begin{equation}\label{f2} \sum_x{p(X=x)} = 1 \end{equation} $$ 当然，概率值总是非负数，即\(p(X=x) ≥ 0\)。为了简化符号，我们通常省略随机变量的显式表示，用\(p(x)\)代替\(p(X=x)\)。
本书中的大部分技术都假设估计空间和决策空间是连续的。连续的空间意味着随机变量可以取连续值。 除非特别声明，我们假设所有的连续随机变量都有一个概率密度函数(Probability Density Function, PDF)。一维正态分布的概率密度函数可以用均值\(μ\)和方差\(σ^2\)来表示， 也就是所谓的高斯函数Gaussian，其形式为如下: $$ \begin{equation}\label{f3} p(x) = (2\pi σ^2)^{-\frac{1}{2}} \exp\left\{-\frac{1}{2}\frac{(x-μ)^2}{σ^2}\right\} \end{equation} $$ 正态分布在本书中有着重要的作用。我们通常都将之简写为\(\mathcal{N}(x;μ,σ^2)\)，其中\(x,μ,σ^2\)分别表示随机变量、均值和方差。 公式(\(\ref{f3}\))中的随机变量\(x\)是一个标量。更多情况下，\(x\)是一个多维的向量。针对向量的正态分布称为多变量multivariable正态分布， 其概率密度函数为： $$ \begin{equation}\label{f4} p(x) = \det(2\pi \Sigma)^{-\frac{1}{2}} \exp\left\{-\frac{1}{2}(x-μ)^T\Sigma^{-1}(x-μ)\right\} \end{equation} $$ 其中，\(μ\)为均值向量，\(\Sigma\)是一个半正定的对称矩阵称为协方差矩阵covariance matrix。 上角标\(T\)表示向量的转置。这个概率密度函数的指数部分的参数是\(x\)的二次型。 读者应该可以发现公式(\(\ref{f4}\))实际上就是公式(\(\ref{f3}\))的扩展。如果\(x\)是一个一维的向量(也就是一个标量)， 那么两个公式就是完全相同的。
公式(\(\ref{f3},\ref{f4}\))都是PDF的例子。与离散概率分布的求和为1类似的，它们的积分总是为1： $$ \begin{equation}\label{f5} \int{p(x)dx} = 1 \end{equation} $$
两个随机变量\(X,Y\)的联合分布joint distribution由下式给出： $$ \begin{equation}\label{f6} p(x,y) = p(X = x, Y = y) \end{equation} $$ 该式描述的是随机变量\(X\)取值为\(x\)并且\(Y\)取值为\(y\)的概率。如果\(X,Y\)是独立的，我们就有 $$ \begin{equation}\label{f7} p(x,y) = p(x)p(y) \end{equation} $$ 通常随机变量还会携带其它变量的信息。假设我们已经知道\(Y\)的取值为\(y\)，在此条件下我们就有\(X=x\)的概率： $$ \begin{equation}\label{f8} p(x|y) = p(x=x | Y=y) \end{equation} $$ 称之为条件概率(conditional probability)。如果\(p(y) > 0\)，那么其条件概率定义为： $$ \begin{equation}\label{f9} p(x|y) = \frac{p(x,y)}{p(y)} \end{equation} $$ 如果\(X,Y\)相互独立，我们有 $$ \begin{equation}\label{f10} p(x|y) = \frac{p(x)p(y)}{p(y)} = p(x) \end{equation} $$ 也就是说，如果\(X,Y\)相互独立，那么\(Y\)将不能为我们带来任何关于\(X\)的信息。根据条件概率的定义和概率测量公理(axioms)，可以推出全概率公式： $$ \begin{equation}\label{f11} p(x) = \sum_y{p(x|y)p(y)} \end{equation} $$ $$ \begin{equation}\label{f12} p(x) = \int{p(x|y)p(y)dy} \end{equation} $$ 进而可以得到贝叶斯公式(Bayes rule)： $$ \begin{equation}\label{f13} p(x|y) = \frac{p(y|x)p(x)}{p(y)} = \frac{p(y|x)p(x)}{\sum_{x'}{p(y|x')p(x')}} \end{equation} $$ $$ \begin{equation}\label{f14} p(x|y) = \frac{p(y|x)p(x)}{p(y)} = \frac{p(y|x)p(x)}{\int{p(y|x')p(x')dx'}} \end{equation} $$ 如果我们需要从\(y\)中窥测\(x\)，那么概率\(p(x)\)就被称为先验概率分布(prior probability distribution)，\(y\)称为数据(data,比如说是传感器的测量值)。 概率\(p(x|y)\)称为\(X\)的后验概率密度(posterior probability distribution)。正如公式(\(\ref{f14}\))所描述的那样， 贝叶斯公式建立了后验概率\(p(x|y)\)与其"反"(译者按.\(p(y|x)\)在一些资料中称之为似然概率)和先验概率之间的关系。 也就是说，贝叶斯公式为我们提供了通过传感器数据\(y\)获得状态\(x\)的定量表述的方法。 在机器人学中，\(p(y|x)\)通常称为生成模型(generative model)，因为它在一定的抽象程度上描述了状态变量\(X\)是如何影响传感器测量值\(Y\)的。
贝叶斯公式中的分母\(p(y)\)的取值于\(x\)无关。因为，无论\(x\)取什么值，公式(\(\ref{f13},\ref{f14}\))中的因子\(p(y)^{-1}\)都是一样的。 因此，\(p(y)^{-1}\)通常都被写为一个归一化(normalizer)的变量，用符号\(\eta\)表示： $$ \begin{equation}\label{f15} p(x|y) = \eta p(y|x)p(x) \end{equation} $$ 这种表达形式很简单，如果要写出归一化算子的完整数学形式，它将变得很复杂。这里仅用一个符号\(\eta\)来表示这种归一化的操作。 在本书中，归一化操作都是用类似的符号来表示，可以是\(\eta\)或者是\(\eta ', \eta '', \dots\)。即使实际的取值不一样，我们也在不同的公式中用\(\eta\)表示归一化操作。
我们可以在条件中组合上任意一个随机变量\(Z=z\)，此时条件贝叶斯公式的形式如下: $$ \begin{equation}\label{f16} p(x|y,z) = \frac{p(y|x, z)p(x|z)}{p(y|z)} \end{equation} $$ 其中，\(p(y|z) > 0\)。类似的，在以\(z\)为条件的情况下，如果变量\(x,y\)相互独立，有联合概率密度： $$ \begin{equation}\label{f17} p(x,y|z) = p(x|z)p(y|z) \end{equation} $$ 这种关系称为条件独立性(conditional independence)。我们也可以很容易证明公式(\(\ref{f17}\))和下式等价： $$ \begin{equation}\label{f18} \begin{matrix} p(x|z) & = p(x|z, y) \\ p(y|z) & = p(y|z, x) \end{matrix} \end{equation} $$ 在概率机器人学中，条件独立性扮演者一个很重要的角色。其物理意义在于，在已知其他变量的取值\(z\)的时候，变量\(y\)是否携带有\(x\)的信息。 条件独立性并不意味着绝对的独立，也就是说： $$ \begin{equation}\label{f20} p(x,y|z) = p(x|z)p(y|z) \not\Rightarrow p(x,y) = p(x)p(y) \end{equation} $$ 反过来也不成立：绝对的独立并不意味着条件独立： $$ \begin{equation}\label{f21} p(x,y) = p(x)p(y) \not\Rightarrow p(x,y|z) = p(x|z)p(y|z) \end{equation} $$ 当然，在有些特殊的场合两者会很巧合的一致。
很多概率学算法都需要计算概率分布的一些特征或者说是统计量。变量\(X\)的期望就是一种常见的统计量，其定义如下： $$ \begin{equation}\label{f22} E[X] = \sum_x{xp(x)} \end{equation} $$ $$ \begin{equation}\label{f23} E[X] = \int{xp(x)dx} \end{equation} $$ 并不是所有的变量都有有限的期望，但这些不在本书要讨论的范畴中。期望是随机变量的线性函数，对于任意的两个数值\(a,b\)我们有： $$ \begin{equation}\label{f24} E[aX + b] = aE[X] + b \end{equation} $$
我们还可以通过如下的形式获得变量\(X\)的协方差covariance： $$ \begin{equation}\label{f25} Cov[X] = E[X - E[X]^2] = E[X^2] - E[X]^2 \end{equation} $$ 协方差描述了数据相对于均值的二阶偏差程度。对于一个正态分布\(\mathcal{N}(x;μ,\Sigma)\)，其均值为\(μ\)，协方差为\(\Sigma\)。
随机变量的另一个重要特性是它的熵(entropy)。其定义为概率值的负对数的期望: $$ \begin{equation}\label{f26} H_p(x) = E[-\log_2{p(x)}] \end{equation} $$ 展开有: $$ \begin{equation}\label{f27} H_p(x) = -\sum_x{p(x)\log_2{p(x)}} \end{equation} $$ $$ \begin{equation}\label{f28} H_p(x) = -\int_x{p(x)\log_2{p(x)dx}} \end{equation} $$ 熵的概念是从信息论中引入的，描述的是\(x\)携带的信息量。对于离散形式的熵，\(p(x)\)是观测到\(x\)的概率，那么\(-\log_2{p(x)}\)说明了编码\(x\)需要多少Bit。 在本书中，熵被用作机器人的信息收集，进而表述机器人执行特定动作后可能获得信息。
2.3 机器人环境交互
下图2.1就是一个机器人与其周围环境之间交互的例子。机器人所在的环境environment或者世界world是一个动态的系统，具有内部的状态。 机器人可以通过传感器获取环境的状态。但是，传感器是有噪声的，而且通常很多信息都是不能直接获得的。 正如图2.1中左侧所示的那样，机器人根据环境的状态维护着一个内在的置信度信息(internal belief)。
 |
| 图 2.1. 机器人环境交互 |
机器人还可以通过执行器改变它的环境。但执行后的状态通常是不确定的。因此，每个控制动作不仅会影响环境状态，也会改变机器人关于这些状态的置信度。
下面我们将更形式化的描述交互。
2.3.1 状态
环境是通过状态来描述的。在本书中为了方便，我们把状态看作是机器人和它的环境中对未来有影响的所有要素的集合。状态可能随着时间而改变，比如说机器人附近的人的位置。 有时环境是静态的，比如说建筑中的墙大多都是不能移动的。变化的状态称为动态状态dynamic state，不变的状态称为静止状态static state。 状态也用于描述一些机器人的变量，比如它的位置、速度、传感器工作状态等等。
在本书中，我们将一直用符号\(x\)表示状态，它的取值与上下文有关。在\(t\)时刻的状态记为\(x_t\)。本书中涉及的典型变量有：
- 机器人的位置。它由相对于一个全局坐标系的位置和方向两个部分构成。刚性的移动机器人有6个变量，其中三个表示它的笛卡尔坐标， 另外三个则是它的姿态角度，或者说是欧拉角(俯仰角pitch、滚转角roll和偏航角yaw)。一个约束在平面上的移动机器人，通常只有三个变量就可以描述其位置了， 其中两个表示笛卡尔坐标，剩下一个表示方向角(yaw)。
- 在机器人的操作过程中，位置变量还包括机器人执行器的配置configuration of the robot's actuators。比如说转动关节的转角。 机械臂的每个自由度都要用一个变量来描述，它们是机器人运动学状态的一部分。机器人配置通常被称为运动学状态Kinematic state。
- 机器人的速度和关节的速度通常称为动力学状态dynamic state。一个刚性的机器人在空间中的运动是用6个速度变量来描述的，这点和机器人的位置一样。 在本书中动力学状态不是研究的重点。
- 环境中周围物体的位置和特征也是状态变量。这个物体可以是一棵树、一堵墙、或者是一个很大的面上的一个点。 这些物体的特征可能是它们的视觉效果(颜色、纹理等)。根据建模粒度的不同，机器人环境的变量从几十个到几百万甚至更多都是有可能的。 本书中研究的很多问题，环境中物体的位置都是静态的。在一些问题中，会有一些称为路标landmarks的物体，它们在环境中是固定不动的， 总是可以被识别的。
- 移动地物体和人类的位置和速度也可以看作是变量。通常机器人不是环境中的唯一的移动物体。其它的移动物体都有自己的运动学和动力学状态。
- 还有很多其它状态变量可能影响机器人的操控。比如说，传感器是否坏掉了，机器人的电池电量，等等，类似这样的特征还有很多，它们都可以当作变量。
状态的完备性对于理论分析而言是一个比较重要的概念。实际上不可能在任何机器人系统都找到一个完备的状态。 一个完备的状态不仅仅要包含环境中所有可能影响未来的因素，还要有机器人自身的状态，计算机内存中存储的数据， 周围人类嘈杂的内心活动等等。有些信息是很难获取的。实际上我们只是选用了所有状态变量的一个很小的子集，它是一个不完备状态incomplete state。
在大多数机器人应用中，状态\(x_t\)都是连续的。机器人的位姿，相对于外部坐标系的坐标和方向，就是一个典型的连续状态。 有些情况下，状态是离散的。比如说传感器是否坏了，这一变量的状态空间就是二值的。既包含连续的状态也有离散状态的状态空间被称为混合hybrid状态空间。
在大多数情况下，状态都是随着时间改变的。在本书中时间都是离散的，也就是说事件总是发生在离散的时间点\(0,1,2,\dots\)上。 如果一个机器人在一个特定的时间开始动作，我们将这一时刻记为\(t = 0\)。
2.3.2 环境交互
机器人与环境之间有两种基本的交互类型：机器人可以通过它的执行器改变环境的状态，还可以通过传感器收集信息。 这两种类型的交互可能同时发生，但为了方便，在本书中我们将两者区别对待。图2.1示例了机器人与环境的交互：
- 传感器测量值。感知是机器人通过传感器收集周围环境状态的过程。比如说，一个机器人可能用拍照、距离扫描、或者通过触觉传感器收集环境的信息。 得到的结果就是所谓的测量值measurement，有时我们称之为observation或percept。通常传感器测量值都有一定的延迟，因此它们提供的都是一段时间之前的信息。
- 改变世界状态的控制操作。它们通过作用在机器人环境的作用力实现。机器人的移动、操作物体都是控制操作。即使机器人自身没有动作，它的状态也会改变。 因此出于一致性的考虑，即使不让任何一个电机转动，我们也认为机器人是在执行控制操作。实际上机器人总是在不断的在执行动作并做出测量。
理想情况下，机器人会记录过去所有的传感器测量值和控制操作。我们称这些记录为数据data(无论它们是否还在内存中)。 考虑到两种不同类型的环境交互，机器人有如下两种不同的数据流：
- 测量数据(Measurement data)提供了某一瞬间环境的信息。典型的例子有图像、距离扫描等。大多数情况下，
我们都忽略了很小的时间因素，比如说，雷达传感器以一个较高的频率扫描环境，我们就认为所有的这些扫描值都是在一个时刻获得的。\(t\)时刻的测量数据将被记为\(z_t\)。
在本书中的大部分内容中，都假设机器人在一个时刻只进行一次测量，这主要是为了符号上的一致性。经过简单的扩展， 本书中几乎所有的算法都能够用于在一个时间段里进行多次测量的机器人上。符号 $$ \begin{equation}\label{f29} z_{t_1 : t_2} = z_{t_1}, z_{t_1 + 1}, z_{t_1 + 2}, \cdots, z_{t_2} \end{equation} $$ 用来标记从\(t_1\)到\(t_2\)时刻的测量数据流，并且\(t_1 ≤ t_2\)。 - 控制数据(Control data)中携带了环境状态改变的信息。对于一个移动机器人，它的速度就是一个典型的控制数据的例子。
执行一个操作，让机器人在5秒的时间里以10cm每秒的速度运动，机器人的位置基本上向前移动了50cm。因此控制蕴含了状态改变的信息。
另一种控制数据的来源就是里程计odometers。里程计是测量机器人轮子转动的传感器，它们也携带者状态改变的信息。 虽然里程计是一种传感器，但是我们仍将之视为控制数据，因为它主要是测量控制操作的效果的。
控制数据用符号\(u_t\)来表示。变量\(u_t\)表示一段时间\((t-1;t]\)内的状态改变。按照惯例，我们用符号\(u_{t_1:t_2}\)表示\(t_1 ≤ t_2\)一段时间的控制数据序列： $$ \begin{equation}\label{f30} u_{t_1 : t_2} = u_{t_1}, u_{t_1 + 1}, u_{t_1 + 2}, \cdots, u_{t_2} \end{equation} $$ 即使机器人没有执行任何操作，环境都是会变的。可以说时间总是在流逝，控制数据也一直在流淌~.~ 因此我们假设在每一个时刻\(t\)都有一个控制数据，即使机器人什么也没做，也认为是一个合法的控制操作。
测量数据流和控制数据流之间存在着很大的差别，因为它们将扮演不同的角色。环境感知提供了关于环境状态的信息，因此它倾向于增加机器人的知识。 另一方面，因为机器人存在着执行误差，并且其所在环境具有一定随机性，所以即使控制能够使得机器人的状态更加稳定，运动也还意味着机器人的知识的降低。 在时间上把这两个数据流区分开是没有意义的，做测量的时候也没有要求机器人不能移动，很多时候感知和控制是同时发生的。这里将它们分开纯粹是为了方便。
2.3.3 概率生成法则
状态和测量值的发展是由概率规律决定的。一般当前状态\(x_t\)都是由状态\(x_{t-1}\)随机生成的。因此我们有必要研究生成\(x_t\)的概率分布。 乍一看，状态\(x_t\)的出现可能与过去所有的状态、测量、和控制都有关系，所以我们可以用\( p(x_t | x_{0:t-1}, z_{1:t-1}, u_{1:t})\)描述状态演变的概率密度。 在没有特别说明的情况下，我们都认为机器人是先执行动作\(u_1\)，再测量\(z_1\)的。
如果状态\(x\)是完备的，那么它就足以涵盖过去所有的操作了。具体来说就是，\(x_{t-1}\)是之前所有的控制量\(u_{1:t-1}\)和测量值\(z_{1:t-1}\)的充分统计量。 在已知状态\(x_{t-1}\)的情况下，上式中所有的变量里只有控制量\(u_t\)对当前状态有影响。
上述特性可以用如下的等式来描述: $$ \begin{equation}\label{f31} p(x_t | x_{0:t-1}, z_{1:t-1}, u_{1:t}) = p(x_t | x_{t-1}, u_t) \end{equation} $$ 这个等式就是条件独立性的一个应用示例。当我们知道了第三组变量(也就是条件变量)的值，那么其他变量之间就是相对独立的。条件独立性将贯穿本书始末，它也是本书中算法都很好理解的原因。
有时，我们可能想要对生成测量值的过程建模。如果\(x_t\)是完备的，我们将有如下重要的条件独立性： $$ \begin{equation}\label{f32} p(z_t | x_{0:t-1}, z_{1:t-1}, u_{1:t}) = p(z_t | x_t) \end{equation} $$ 换句话说，状态\(x_t\)就已经足够预测测量值\(z_t\)了。如果\(x_t\)是完备的，那么其它的变量，比如说历史的测量值、控制量、甚至是历史的状态都与\(z_t\)无关。
下面，我们将讨论这两个条件概率\(p(x_t|x_{t-1},u_t), p(z_t|x_t)\)。概率\(p(x_t|x_{t-1},u_t)\)就是状态转移概率state transition probability。它说明了环境状态是如何因为控制量\(u_t\)而随时间迁移。同时概率分布\(p(x_t|x_{t-1},u_t)\)还说明环境状态是随机的，并不是确定的。 有些时候状态转移概率分布并不依赖于时间索引\(t\)，此时我们就可以将之写成\(p(x'|u,x)\)，其中\(x'\)是\(x\)后继时刻的状态。
概率\(p(z_t|x_t)\)称为测量概率measurement probability，它也可能与时间索引\(t\)无关，可以简写为\(p(z|x)\)。测量概率说明了在状态\(x\)下观测到\(z\)的概率。 我们可以将测量值看作是状态的一种带有噪声的映射。
状态转移概率和测量概率一起描述了机器人及其环境所构成的动态随机系统。下图2.2描述了由这些概率建模的状态与测量值的变化过程。 \(t\)时刻的状态只与\(t-1\)时刻的状态和控制量\(u_t\)有关。而测量值\(z_t\)依赖于\(t\)时刻的状态。这样一个概率生成模型(generative model)就是所谓的隐马尔科夫模型(Hidden Markov Model, HMM)或者动态贝叶斯网络(Dynamic Bayes Network, DBN)。
 |
| 图 2.2. 描述控制量、状态和测量值演化的动态贝叶斯网络 |
2.3.4 置信度分布(Belief Distributions)
概率机器人学中的另一个核心概念就是置信度belief。置信度反映了机器人对环境状态的掌握程度。我们已经讨论过有些状态是不能够直接测量的。 例如，机器人在全局坐标系中的位置可能是\(x = \langle 14.12, 12.7, 45° \rangle\)，但机器人通常不知道自己的位置， 因为位置是不能够直接测量的(即使是GPS也是不可以的!)。实际上，机器人必须通过数据来推算自己的位置。因此，我们根据机器人维护的置信度来识别真实的状态。 在一些文献中置信度也称为state of knowledge和information state，需要注意不要与后续的信息向量和信息矩阵的概念搞混了。
概率机器人学通过条件概率分布描述置信度。一个置信度分布根据真实的状态为每一个可能的值赋予一个概率(或者是密度值, density value)。 置信度分布是状态变量在已知数据上的后验概率。我们用\(bel(x_t)\)描述状态变量\(x_t\)的置信度： $$ \begin{equation}\label{f33} bel(x_t) = p(x_t | z_{1:t}, u_{1:t}) \end{equation} $$
读者可能注意到置信度是在进行了测量\(z_t\)之后计算的。有时在执行了控制量\(u_t\)后，进行测量\(z_t\)之前，使用上次测量值计算的置信度也是很有用的。 这样的后验概率表示为： $$ \begin{equation}\label{f34} \overline{bel}(x_t) = p(x_t | z_{1:t-1}, u_{1:t}) \end{equation} $$ 在基于概率的滤波器中，它通常被称为预测值prediction。\(\overline{bel}(x_t)\)是根据之前的数据和最新的控制量来预测\(t\)时刻的状态。 从\(\overline{bel}(x_t)\)计算得出\(bel(x_t)\)的过程被称为校正correction或者测量更新measurement update。
2.4 贝叶斯滤波器
2.4.1 贝叶斯滤波器算法
贝叶斯滤波器算法是最一般的计算置信度的算法。它从测量和控制数据中计算置信度分布\(bel\)。我们将先简单陈述一下基本算法，再用一个数值的例子具体说明。 最后，我们将给出数学推导。
算法2.1以伪代码的形式描述了贝叶斯滤波器。它是一个迭代的算法，用\(t-1\)时刻的置信度\(bel(x_{t-1})\)计算\(t\)时刻的置信度\(bel(x_{t})\)。 它的输入是\(t-1\)时刻的置信度，以及最近一次的控制量\(u_t\)和测量值\(z_t\)。输出是\(t\)时刻的置信度\(bel(x_t)\)。 算法2.1中只描述了贝叶斯滤波器的更新规则update rule。这一更新规则是一个迭代的过程， 用上一次的置信度\(bel_{x_{t-1}}\)来计算下一次的置信度\(bel_{x_t}\)。
 |
| 算法2.1. 贝叶斯滤波器的一般形式 |
贝叶斯滤波器算法有两个基本的步骤。在第三行中根据\(x_{t-1}\)和\(u_t\)预测状态\(x_t\)的置信度\(\overline{bel}(x_t)\)。其积分项是两个概率密度的乘积， 其中第一项描述的是在状态\(x_{t-1}\)下，控制量\(u_t\)的作用使得状态从\(x_{t-1}\)转移到\(x_t\)的概率。读者可能会注意到这一步与公式(\(\ref{f12}\))神似， 这一过程被称为控制更新(control update)或者是预测prediction。
贝叶斯滤波器的第二步称为测量更新(measurement update)。在第4行中，贝叶斯滤波器用观测到\(z_t\)的条件概率乘上预测值\(\overline{bel}(x_t)\)， 并做归一化处理，得到置信度\(bel(x_t)\)。之所以需要归一化处理，是因为直接将两项相乘后得到的结果可能不是一个概率值(对所有的\(x_t\)积分或者求和不等于1)。
为了让算法正常运行，需要有一个\(t=0\)时刻的初始置信度\(bel{x_0}\)作为边界条件。如果我们确切的知道初始状态\(x_0\)， \(bel{x_0}\)就应该初始化为一个中心为\(x_0\)的点概率密度(point mass distribution)。如果我们对于\(x_0\)的初始值一无所知， 那么通常就用均匀分布或者Dirichlet类的分布来描述\(bel{x_0}\)。了解了\(x_0\)的部分信息，我们就可以用一个非均匀分布的概率模型来描述。 但更多的情况是，我们掌握了完整的信息，或者一脸懵逼。
贝叶斯滤波器算法只适用于这里所讲的简单的估计问题。我们不仅需要循环地计算第三行的积分和第四行的乘积，还需要把状态限定在一个有限的空间中， 这样第三行的积分才可以写成有限的求和形式。
2.4.2 例子
我们以图2.3中所示的场景为例，以数值的形式说明贝叶斯滤波器，该图中显示了机器人根据照相机估计门的状态的场景。 为了简化问题，我们假定门只有开着或者关着两种不同的状态，而且只有机器人可以改变门的状态。更进一步的，假设机器人在一开始并不知道门的状态。
 |
| 图 2.3. 一个移动机器人估计门的状态 |
我们为两个状态赋予一个相同的概率值： $$ bel(X_0 = \boldsymbol{open}) = 0.5 $$ $$ bel(X_0 = \boldsymbol{close}) = 0.5 $$ 假设机器人的传感器存在噪声，我们可以通过如下的条件概率来描述这一特性，检测到门是关着的状态具有相对较高的可靠性。 $$ \begin{cases} p(Z_t = \boldsymbol{sense\_open} | X_t = \boldsymbol{is\_open}) & = 0.6 \\ p(Z_t = \boldsymbol{sense\_close} | X_t = \boldsymbol{is\_open}) & = 0.4 \end{cases} $$ $$ \begin{cases} p(Z_t = \boldsymbol{sense\_open} | X_t = \boldsymbol{is\_close}) & = 0.2 \\ p(Z_t = \boldsymbol{sense\_close} | X_t = \boldsymbol{is\_close}) & = 0.8 \end{cases} $$ 最后，我们让机器人把门推开。如果门本来就是开着的，那动作之后仍然开着。如果关着，机器人有80%的可能打开它。 $$ \begin{cases} p(X_t = \boldsymbol{is\_open} | U_t = \boldsymbol{push}, X_{t-1} = \boldsymbol{is\_open}) & = 1 \\ p(X_t = \boldsymbol{is\_closed} | U_t = \boldsymbol{push}, X_{t-1} = \boldsymbol{is\_open}) & = 0 \\ p(X_t = \boldsymbol{is\_open} | U_t = \boldsymbol{push}, X_{t-1} = \boldsymbol{is\_close}) & = 0.8 \\ p(X_t = \boldsymbol{is\_closed} | U_t = \boldsymbol{push}, X_{t-1} = \boldsymbol{is\_close}) & = 0.2 \end{cases} $$ 机器人也可能什么都不做，那么门的状态就不会有变化。因此有如下的条件概率： $$ \begin{cases} p(X_t = \boldsymbol{is\_open} | U_t = \boldsymbol{do\_nothing}, X_{t-1} = \boldsymbol{is\_open}) & = 1 \\ p(X_t = \boldsymbol{is\_closed} | U_t = \boldsymbol{do\_nothing}, X_{t-1} = \boldsymbol{is\_open}) & = 0 \\ p(X_t = \boldsymbol{is\_open} | U_t = \boldsymbol{do\_nothing}, X_{t-1} = \boldsymbol{is\_close}) & = 0 \\ p(X_t = \boldsymbol{is\_closed} | U_t = \boldsymbol{do\_nothing}, X_{t-1} = \boldsymbol{is\_close}) & = 1 \end{cases} $$ 假设在\(t\)时刻，机器人并没有动作但是检测到门打开了。将\(bel(X_0)\)和控制量\(u_1 = \boldsymbol{do\_nothing}\)代入贝叶斯滤波器算法的第三行，计算积分项。 $$ \begin{array} {lcl} \overline{bel}(X_1) & = & \int{p(X_1 | u_1, X_0)bel(x_0)dX_0} \\ & = & \sum_{x_0}p(X_1 | u_1, X_0)bel(X_0) \\ & = & p(x_1 | u_1 = \boldsymbol{do\_nothing}， X_0 = \boldsymbol{is\_open})bel(X_0 = \boldsymbol{is\_open}) \\ & + & p(x_1 | u_1 = \boldsymbol{do\_nothing}， X_0 = \boldsymbol{is\_closed})bel(X_0 = \boldsymbol{is\_closed}) \end{array} $$ 这样我们就可以计算出\(X_1\)的两种状态的置信度。对于\(X_1 = \boldsymbol{is\_open}\)，有： $$ \begin{array} {lcl} \overline{bel}(X_1 = \boldsymbol{is\_open}) & = & p(X_1 = \boldsymbol{is\_open} | u_1 = \boldsymbol{do\_nothing}， x_0 = \boldsymbol{is\_open})bel(X_0 = \boldsymbol{is\_open}) \\ & + & p(X_1 = \boldsymbol{is\_open} | u_1 = \boldsymbol{do\_nothing}， x_0 = \boldsymbol{is\_closed})bel(X_0 = \boldsymbol{is\_closed}) \\ & = & 1 · 0.5 + 0 · 0.5 = 0.5 \end{array} $$ 对\(X_1 = \boldsymbol{is\_closed}\)，有： $$ \begin{array} {lcl} \overline{bel}(X_1 = \boldsymbol{is\_closed}) & = & p(X_1 = \boldsymbol{is\_closed} | u_1 = \boldsymbol{do\_nothing}， x_0 = \boldsymbol{is\_open})bel(X_0 = \boldsymbol{is\_open}) \\ & + & p(X_1 = \boldsymbol{is\_closed} | u_1 = \boldsymbol{do\_nothing}， x_0 = \boldsymbol{is\_closed})bel(X_0 = \boldsymbol{is\_closed}) \\ & = & 0 · 0.5 + 1 · 0.5 = 0.5 \end{array} $$ 我们得出\(\overline{bel}(x_1)\)与\(bel(x_0)\)的值相等的结论。这是因为动作do_nothing并不会改变世界的状态。
但是进行了测量之后，置信度就会发生改变。贝叶斯滤波器的第四行意味着如下的公式：
$$ bel(x_1) = \eta p(Z_1 = \boldsymbol{sense\_open} | x_1) \overline{bel}(x_1) $$
对于可能的两个状态\(X_1 = \boldsymbol{is\_open}\)和\(X_1 = \boldsymbol{is\_closed}\)，我们有：
$$ \begin{array} {lcl}
bel(X_1 = \boldsymbol{is\_open})
& = & \eta p(Z_1 = \boldsymbol{sense\_open} | X_1 = \boldsymbol{is\_open})\overline{bel}(X_1 = \boldsymbol{is\_open}) \\
& = & \eta 0.6 · 0.5 = \eta 0.3
\end{array} $$
和
$$ \begin{array} {lcl}
bel(X_1 = \boldsymbol{is\_closed})
& = & \eta p(Z_1 = \boldsymbol{sense\_open} | X_1 = \boldsymbol{is\_closed})\overline{bel}(X_1 = \boldsymbol{is\_closed}) \\
& = & \eta 0.2 · 0.5 = \eta 0.1
\end{array} $$
归一化算子\(\eta = (0.3 + 0.1)^{-1} = 2.5\)因此有：
$$ \begin{array} {lcl}
bel(X_1 = \boldsymbol{is\_open}) & = & 0.75 \\
bel(X_1 = \boldsymbol{is\_closed}) & = & 0.25
\end{array} $$
接下来，机器人执行的
2.4.3 贝叶斯滤波器的数学推导
我们可以证明贝叶斯滤波器的正确性。下面将证明从上一时刻的置信度\(p(x_{t-1} | z_{1:t-1},u_{1:t-1})\)计算后验分布\(p(x_t|z_{1:t},u{1:t})\)的正确性。 整个证明过程是建立在正确初始化初始时刻\(t=0\)的置信度\(bel(x_0)\)的基础之上的。
推导过程要求状态\(x_t\)如2.3.1节所说的那样是完备的，并且控制量的选择是随机的。首先用贝叶斯公式来计算后验概率： $$ \begin{align}\label{f35} p(x_t | z_{1:t}, u_{1:t}) & = & \frac{p(z_t | x_t, z_{1:t-1}, u_{1:t})p(x_t | z_{1:t-1}, u_{1:t})}{p(z_t | z_{1:t-1}, u_{1:t})} \\ & = & \eta p(z_t | x_t, z_{1:t-1}, u_{1:t})p(x_t | z_{1:t-1}, u_{1:t}) \end{align} $$
现在，我们来研究一下状态完备的假设。在第2.3.1中，如果除了\(x_t\)没有变量可以影响整个环境在未来的随机过程，我们说状态\(x_t\)是完备的。 也就是说，如果我们知道状态\(x_t\)并据此预测可能的测量值\(z_t\)，那么过去的测量和控制量都不能为我们提供任何附加的信息。数学上，可以用如下的条件独立性来表示这一特性： $$ \begin{equation}\label{f36} p(z_t | x_t, z_{1:t-1}, u_{1:t}) = p(z_t | x_t) \end{equation} $$ 这又是一个条件独立性conditional independence的例子。我们可以将公式(\(\ref{f35}\))化简为： $$ \begin{equation}\label{f37} p(x_t | z_{1:t}, u_{1:t}) = \eta p(z_t | x_t)p(x_t | z_{1:t-1}, u_{1:t}) \end{equation} $$ 因此有: $$ \begin{equation}\label{f38} bel(x_t) = \eta p(z_t | x_t)\overline{bel}(x_t) \end{equation} $$ 这个公式就是我们在贝叶斯滤波器中第四行提到的伪代码。接下来我们用公式(\(\ref{f12}\))展开\(\overline{bel}(x_t)\): $$ \begin{equation}\label{f39} \begin{array} ?\overline{bel}(x_t) & = & p(x_t | z_{1:t-1}, u_{1:t}) \\ & = & \int{p(x_t| x_{t-1}, z_{1:t-1}, u_{1:t})p(x_{t-1} | z_{1:t-1},u_{1:t})dx_{t-1}} \end{array} \end{equation} $$ 状态完备假设意味着，如果我们知道了\(x_{t-1}\)，过去的测量和控制量将不携带关于状态\(x_t\)的信息。因此，有： $$ \begin{equation}\label{f40} p(x_t | x_{t-1}, z_{1:t-1}, u_{1:t}) = p(x_t | x_{t-1}, u_t) \end{equation} $$ 这里我们保留了控制量\(u_t\)，因为它并不用于预测状态\(x_{t-1}\)。实际上\(p(x_t | x_{t-1}, u_t) \neq p(x_t | x_{t - 1})\)。 我们注意到通过随机选择控制，就可以把控制量\(u_t\)从\(p(x_{t-1}|z_{1:t-1}, u_{1:t})\)的条件中移除，得到迭代更新公式： $$ \begin{equation}\label{f41} \overline{bel}(x_t) = \int{p(x_t | x_{t-1}, u_t)p(x_{t-1} | z_{1:t-1}, u_{1:t-1})dx_{t-1}} \end{equation} $$ 读者可以很容易验证，这就是算法2.1中第三行提到的伪代码。总之，贝叶斯滤波器算法根据\(t\)时刻的测量值和控制量来计算状态\(x_t\)的后验概率。 这个推导过程中的假设被称为马尔可夫(Markov)特性，也就是说状态是完备的。
任何该算法的具体实现都需要三个概率分布：初始置信度\(p(x_0)\)，测量概率分布\(p(z_t|x_t)\)，和状态转移概率\(p(x_t|u_t, x_{t-1})\)。 我们将在第五章中关注\(p(x_t | u_t, x_{t-1})\)， 在第六章中考察\(p(z_t | x_t)\)。 此外，我们也需要一个置信度\(bel(x_t)\)的表达形式，这将是第三章和 第四章要讨论的内容。
2.4.4 马尔可夫假设
马尔可夫假设或者说是完备状态假设是本书的基石。马尔可夫假设中，如果我们知道了当前状态\(x_t\)那么过去和未来的数据就是相互独立的。 至于说这个假设到底有多强呢？让我们再重新考虑一下移动机器人定位的例子。在移动机器人定位问题中，\(x_t\)是机器人的位置， 并使用贝叶斯滤波器来估计机器人在一个固定地图中的位置。下面的这些因素都可能对传感器的读数有影响，因此它们都会打破马尔可夫假设。
- 环境中未建模的动态特性并不包含在\(x_t\)中，比如说，移动的人类，以及他们对传感器测量值的影响。
- 概率模型\(p(z_t | x_t)\)和\(p(x_t | u_t, x_{t-1})\)的不精确性。
- 表达置信度函数的近似计算误差。比如说，我们以后会提到的网格(grid)或者高斯函数(Gaussians)。
- 机器人控制软件中的变量，这些变量影响多个控制选择。(比如说，变量“目标位置”通常会影响整个控制命令流)
理论上，这些变量都可以包含在状态中。但是为了降低贝叶斯滤波器算法的计算复杂度，通常用不完备的状态替代之。 虽然有这些不利的因素，但是实际的贝叶斯滤波器也具有很强的鲁棒性。根据经验来说，我们在设计状态\(x_t\)的时候应当谨慎考虑，这样才能够使未建模的状态变量表现出近似随机的效果。
2.5 表达和计算
在概率机器人学中，贝叶斯滤波器有几种不同的实现方式。我们将在后续的两章中会发现，还有很多从贝叶斯滤波器衍生出来的算法和技术。 这些技术对于测量概率、状态转移概率和初始置信度都有着各种不同的假设。这些假设带来了不同的后验概率分布和具有不同特性的算法。 根据一般经验, 计算置信度的技术都是针对高度特质化的案例的，在一般的机器人问题中，置信度一定是近似计算的。 近似计算对于算法复杂度有重要的影响。寻找一个合适的近似通常是一个有挑战的问题，并不存在一个唯一的最优方案适用于所有的机器人问题。
当选择一个近似，我们必须在如下的特性中做出妥协：
- 计算效率。有些近似，比如后面要讨论线性高斯近似，可以在关于状态空间维度的多项式时间复杂度内计算出置信度。 而其他的算法的复杂度确实指数级的。后续我们还要讨论基于粒子的技术，它有一个any-time的特性，可以牺牲精度来换取速度。
- 近似计算的准确度。有些近似可以在更宽的区域内比其他近似方法更贴近真值。比如说，线性高斯近似都只适用于单峰分布， 而直方图表达方式就可以近似多峰分布，但是准确度有限。粒子表达方式可以近似更广的分布，但是为了获得需要的准确度，通常需要很大的粒子的数量。
- 容易实现。概率算法实现的难度与很多因素有关，比如说测量概率\(p(z_t | x_t)\)和状态转移概率\(p(x_t | u_t, x_{t-1}\)的形式。 粒子的表达方法对于复杂的非线性系统来书通常实现起来很简单，这也是它们进来很流行的原因。
接下来的两章将介绍具体的算法实现，它们在上述的特性中做出了不同的取舍。
2.6 小结
在本节中，我们介绍了机器人学中的贝叶斯滤波器的基本思想。它是一种估计环境状态(也可能包含机器人自身的状态)的方法。
- 机器人与它的环境的交互是一个耦合的动态系统模型，机器人可以通过选择控制量来操控环境，同时可以通过传感器测量来感知环境。
- 在概率机器人学中，用状态转移概率分布和测量概率分布来描述机器人及其环境的动态特性。状态转移概率分布描述的是状态如何在机器人的控制下随着时间而变化。 而测量概率分布则反映了状态是如何影响测量值的。这两个概率分布，都考虑了状态演变和感知与生俱来的不确定性。
- 机器人置信度是在所有历史测量值和控制量下的环境状态(包含了机器人状态)的后验概率。在机器人学中，贝叶斯滤波器是计算置信度的基础算法。 它是一种递归算法，\(t\)时刻的计算依赖于\(t-1\)时刻的置信度。
- 贝叶斯滤波器建立在马尔可夫假设上，认定状态是完备的。这个假设意味着当前状态足以涵盖历史状态的所有信息。在机器人学中，马尔可夫假设通常是一种近似。 我们也讨论了在什么情况下假设不成立。
- 因为贝叶斯滤波器不是一个实际的算法，所以它不能够在一个数字计算机上实现，需要对其做出一些近似处理。 其近似处理可能考虑不同的因素，包括它们的准确度、效率以及实现的难易程度。
在接下来的两章中讨论两种流行的迭代状态估计算法，它们都是从贝叶斯滤波器中衍生出来的。
2.7 参考文献
本章中的基础统计学知识在很多概率和统计学的入门教材中都有涉及。一些早期的经典教材DeGroot(1975), Subrahmaniam (1979)和Thorp (1966)提供了高度精炼的介绍。 更多的内容可以参考Feller 1968; Casella and Berger 1990; Tanner 1996; Devroye et al. 1996; Duda et al. 2000。机器人环境交互凡是是机器人学中广泛研究的主题， 在Russell and Norvig (2002)中提供了AI角度的解读。
2.8 练习
暂略