粒子滤波器

为了实现机器人的自主运动，它需要不断地根据传感器的数据来估计自身和环境的状态，这个估计的过程通常被称为滤波。有些状态可以直接通过传感器测得，比如说编码器可以直接测得电机的转速。有些状态只能通过传感器的数据推导出来，比如机器人在工作空间中的位置。传感器的数据往往伴随着噪声，"滤波"有过滤信息还原状态本来面目的意思。

在二战期间N. Wiener研究火炮控制系统时提出了经典的维纳滤波。它是一种频率的滤波器，需要用到过去的所有数据，对计算机的存储和计算实时性都有很高的要求，工程实现比较困难。在上个世纪60年代出现了卡尔曼滤波，这是一种时域的方法，通过状态空间方程来进行最优估计，可以递推的形式进行，能够方便的在计算机上实现。但是经典的卡尔曼滤波器处理的都是线性高斯系统，适用范围有限。后续，人们对卡尔曼滤波器进行了扩展，提出了EKF、UKF能够处理一定程度的非线性系统。尽管EKF、UKF有着各种各样的限制，它们仍然是目前广泛使用的滤波器。

粒子滤波器是一种非参数形式的滤波器，它使用序列蒙特卡洛的方法，以大量的样本来描述状态的概率密度，结合递推贝叶斯理论实现状态估计。这种滤波器不需要对状态空间建立精确的模型，适用于非线性非高斯的系统。在我看来它是一种万能的暴力方法，只要计算资源够用不计较计算时间，能够对任何系统进行估计。

在GMapping中使用Rao-Blackwellized粒子滤波器来估计机器人的轨迹，每个粒子对应着一条运动轨迹，然后在机器人的运动轨迹下构建地图。本文中介绍我们介绍GMapping运行的基础粒子滤波器。

1. 最优贝叶斯估计

我们在描述和分析一个动态系统的过程中，往往需要两个方程。一个用于描述系统状态随时间演变，我们称之为状态方程，可以写成如下的形式：

$$ \begin{equation}\label{f1} x_t = g_t(u_t, x_{t-1}) + \varepsilon_t \end{equation} $$

其中，$x_t$表示系统状态向量。$u_t$为控制向量，相互之间是独立的，在不存在控制的场景下该值为0。$g(·)$是状态转移函数，描述了系统在$t-1$时刻下的状态，经控制量$u_t$作用后，转变为$t$时刻的状态。$\{\varepsilon_t, t \in \mathbb{N}\}$是服从独立同分布的系统噪声序列。第二个方程用于描述在当前状态下能够观测到的数据，称之为观测方程，具有如下的形式:

$$ \begin{equation}\label{f2} z_t = h_t(x_t) + \delta_t \end{equation} $$

其中，$z_t$为观测向量，$h(·)$是观测函数，描述了在状态$x_t$下能够观测到的数据。$\{\delta_t, t \in \mathbb{N}\}$是服从独立同分布的测量噪声序列。在以后的分析过程中，除了要掌握函数$g(·)$和$h(·)$，以及系统噪声和测量噪声的分布外，还需要了解初始状态$x_0$及其分布$p(x_0)$。

滤波器的任务就是，根据观测序列$z_{1:t}$来估计系统的状态$x_t$。在有控制的场景中，估计系统状态时还要用到控制序列$u_{1:t}$。我们可以将之写成后验概率的形式

$$ \begin{equation}\label{f3} p(x_t | z_{1:t}, u_{1:t}) \end{equation} $$

我们假设系统状态满足一阶马尔可夫过程。也就是说状态向量$x$是完备的，$x_{t-1}$涵盖了之前所有的控制量$u_{1:t-1}$和观测值$z_{1:t-1}$的信息。在$x_{t-1}$的状态下，只有控制量$u_t$能够影响系统状态，其概率分布可认为由系统噪声$\varepsilon_t$决定。

$$ \begin{equation}\label{f4} p(x_t | x_{0:t-1}, z_{1:t-1}, u_{1:t}) = p(x_t | x_{t-1}, u_t) \end{equation} $$

此外根据式($\ref{f2}$)，假设观测量只与当前系统状态有关，分布由测量噪声$\delta_t$决定。

$$ \begin{equation}\label{f5} p(z_t | x_{0:t}, z_{1:t-1}, u_{1:t}) = p(z_t | x_t) \end{equation} $$

为了计算后验概率$p(x_t | z_{1:t}, u_{1:t})$，一般分为预测和更新两个部分来实现。若已知$t-1$时刻的后验概率$p(x_{t-1} | z_{1:t-1}, u_{1:t-1})$，根据Chapman-Kolmogorov公式我们有:

$$ \begin{equation}\label{f6} p(x_t | z_{1:t-1}, u_{1: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} $$

上式($\ref{f6}$)就是预测过程。接着根据贝叶斯公式实现更新过程，最终计算后验概率值:

$$ \begin{equation}\label{f7} p(x_t | z_{1:t}, u_{1:t}) = \frac{p(z_t | x_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)p(x_t | z_{1:t-1}, u_{1:t}) \end{equation} $$

上式中的分母$p(z_t | z_{1:t-1}, u_{1:t})$对所有的$x_t$都一样，用归一化因子$\eta$来代替。式($\ref{f6},\ref{f7}$)构成了最优贝叶斯估计。在最小方差意义下，对于线性高斯系统而言，卡尔曼滤波器就是一种最优贝叶斯估计的实现。

2. 序列重要性采样算法(Sequential Importance Sampling, SIS)

序列重要性采样算法(SIS)是一种使用蒙特卡洛仿真实现的递推贝叶斯滤波器，它通过大量带有权重的样本来近似表示后验概率。

$$ \begin{equation}\label{f8} p(x_{0:t} | z_{1:t}, u_{1:t}) \approx \sum_{k=0}^{N-1} w_t^k \delta(x_{0:t} - x_{0:t}^k) \end{equation} $$

其中，$\delta(·)$是一个狄拉克函数，在除了零以外的点上取值都为0，并且整个定义域上的积分为1。$\{x_{0:t}^k, k = 0, \cdots, N-1\}$为状态序列的样本集合，有些资料里称他们为支撑点。 $w_t^k$为归一化之后的权重，即$\sum_k w_t^k = 1$。只要样本足够多，或者说$N \to \infty$，这种求和形式的表示方式将收敛于真实的分布，其中的关键在于权重$w_t^k$的选择。

SIS采用一种重要性采样(Importance Sampling)的方法来计算权重。由于我们不清楚后验概率$p(x_{0:t} | z_{1:t}, u_{1:t})$的分布，所以不能直接对其进行采样。但是根据控制量$u_{1:t}$和观测值$z_{1:t}$我们可以统计得到一个建议分布$q(x_{0:t} | z_{1:t}, u_{1:t})$(proposal distribution)。那么我们就可以通过加权的建议分布样本来近似目标分布，此时权重就是目标分布与建议分布的比值。

$$ \begin{equation}\label{f9} w_t^k \propto \frac{p(x_{0:t}^k | z_{1:t}, u_{1:t})}{q(x_{0:t}^k | z_{1:t}, u_{1:t})} \end{equation} $$

对于上式，我们需要保证目标分布的概率值不为0的时候，建议分布的概率值也不能为0，否则权值没有意义。如果建议分布能够因式分解为如下的形式， $$ \begin{equation}\label{f10} q(x_{0:t}^k | z_{1:t}, u_{1:t}) = q(x_t^k | x_{t-1}^k, u_t)q(x_{0:t-1}^k | z_{1:t-1}, u_{1:t-1}) \end{equation} $$ 那么我们就可以根据系统的状态方程，通过$t-1$时刻的样本$x_{0:t-1}^k$，直接推演得到的$t$时刻的样本$x_{0:t}^k$。并且新的状态$x_t^k$服从分布$q(x_t^k | x_{t-1}^k, u_t)$。为了写出权重的递推公式，根据贝叶斯公式，我们将目标分布写成如下的形式:

$$ \begin{align}\label{f11} p(x_{0:t}^k | z_{1:t}, u_{1:t}) & = \frac{p(z_t | x_{0:t}^k, z_{1:t-1}, u_{1:t}) p(x_{0:t}^k | z_{1:t-1}, u_{1:t})}{p(z_t | z_{1:t-1}, u_{1:t})} \\ & = \frac{p(z_t | x_{0:t}^k, z_{1:t-1}, u_{1:t}) p(x_t^k | x_{0:t-1}^k z_{1:t-1}, u_{1:t}) p(x_{0:t-1}^k | z_{1:t-1}, u_{1:t})} {p(z_t | z_{1:t-1}, u_{1:t})} \\ & = \frac{p(z_t | x_t^k) p(x_t^k | x_{t-1}^k, u_t) p(x_{0:t-1}^k | z_{1:t-1}, u_{1:t-1})} {p(z_t | z_{1:t-1}, u_{1:t})} \\ & \propto p(z_t | x_t^k) p(x_t^k | x_{t-1}^k, u_t) p(x_{0:t-1}^k | z_{1:t-1}, u_{1:t-1}) \end{align} $$

将上式与式($\ref{f9}, \ref{f10}$)结合，有:

$$ \begin{align}\label{f12} w_t^k & \propto \frac{p(z_t | x_t^k) p(x_t^k | x_{t-1}^k, u_t) p(x_{0:t-1}^k | z_{1:t-1}, u_{1:t-1})}{q(x_t^k | x_{t-1}^k, u_t)q(x_{0:t-1}^k | z_{1:t-1}, u_{1:t-1})} \\ & = w_{t-1}^k \frac{p(z_t | x_t^k) p(x_t^k | x_{t-1}^k, u_t)}{q(x_t^k | x_{t-1}^k, u_t)} \end{align} $$

如果初始时刻$t=0$我们有一组对先验概率$p(x_0)$采样的样本$\{x_0^k\}$，并为之赋予初始权重$w_0^k = \frac{1}{N}$。以后各个时刻的样本都是根据系统状态方程，从上一时刻的样本中推演生成，其权值则根据式($\ref{f12}$)更新。如此，我们只需维护最新的状态，而不必记录历史的状态、控制量和观测值了。因此，我们可以通过下式进行贝叶斯估计:

$$ \begin{equation}\label{f13} p(x_t | z_{1:t}, u_{1:t}) \approx \sum_{k=0}^{N-1} w_t^k \delta(x_t - x_t^k) \end{equation} $$

下面是SIS粒子滤波器的伪代码。它以上一时刻的加权粒子集合$\mathcal{X}_{t-1}$、当前时刻的控制量$u_t$和观测值$z_t$为输入，输出的是当前时刻以加权粒子集合$\mathcal{X}_t$表示的状态。

故事讲到这里就差不多该结束了，但是人们发现SIS存在一个巨大的缺陷。随着迭代次数的增加，权重将集中在少数的粒子身上，而其余粒子的权重则几乎为零。这就是所谓的退化问题(Degeneracy Problem)。这一粒子退化问题使得大量的计算资源都浪费在了对后验分布几乎没有贡献的粒子上，降低了算法的精度。要解决这一问题需要从两个方面入手，其一找一个与目标分布尽可能接近的建议分布，其二对粒子集合进行重采样。下面我们只介绍第二种选择。

3. 重采样

重采样是一种解决粒子退化问题的主要方法。其基本思想就是对迭代之后的粒子集合重新采样，减少权重较低的粒子，增加权重较大的粒子。右边是Probabilistic Robotics中提到的一种重采样算法，这里根据本文的符号稍微做了一些调整。

该算法的输入是一个加权的粒子集合$\mathcal{X}_t = \{ x_t^i, w_t^i \}_{k=0}^{N-1}$，每个元素都是一个二值对，其中$x_t^i$描述了样本值，$w_t^i$则是样本的权重。 $N$则是样本数量，输出的是重采样后的粒子集合$\bar{\mathcal{X}}_t$，其各个粒子的权重都是$N^{-1}$。在该采样算法中，权重较大的粒子将被多次选中，而权重小于$N^{-1}$的粒子则会被抛弃掉。

这种采样方法应该源自于一种所谓的逆变换采样(inverse transform sampling)。对于随机变量$X$，其概率密度函数为$p(x)$，可以通过积分得到其累积分布函数$P(x) = \int_{-\infty}^{+\infty} p(x) dx$。对在区间$[0, 1]$上均匀分布的变量$Y$，有随机变量$P^{-1}(Y)$与$X$具有相同的分布。其中$P^{-1}(·)$是累积分布函数$P(·)$的反函数。如果我们对随机变量$Y$进行采样，得到的样本$y$，那么$x=P^{-1}(y)$就是对随机变量$X$的采样。

在右边的伪代码中，第4行和第10行中对原粒子集合的权重累加，相当于求取累积分布函数P(·)。在第3行中在区间$[0, N^{-1}]$中随机生成了一个均匀分布的样本，在第7行中依次累加$N^{-1}$。这两个操作相当于将区间$[0, 1]$划分为$N$个小区间，并在每个小区间中采集了一个样本。由于我们不能写出粒子集合所描述的概率密度函数和累积分布函数的具体形式，所以这里通过一个while循环来进行筛选，并将通过筛选的粒子添加到新的粒子集合中。这个过程就相当于求$x=P^{-1}(y)$。

将这一重采样的过程放置在SIS算法之后，就得到了一个典型的SIR粒子滤波器。如下面的伪代码所示，需要注意的是在调用Resampling的算法之前，必须保证粒子集合中的权重是归一化了的，即$\sum_{i=0}^{N-1} w_t^i = 1$。

4. 完

在本文中，我们介绍了粒子滤波器的基本思想。人们一开始通过序贯重要性采样算法(SIS)，来用大量的加权随机样本近似后验概率$p(x_{0:t} | z_{1:t}, u_{1:t})$。但是人们发现SIS算法存在一种称为粒子退化的问题。为了解决该问题，又提出了重采样的技术。SIR就是这样一种典型的粒子滤波器。

虽然重采样技术解决了粒子退化问题，但是它又带来了一种所谓的粒子匮乏问题。即随着迭代次数的增加，因为重采样操作的存在，降低了粒子的多样性，以至于虽然有大量的粒子，但是它们并不能完全覆盖所有高概率的区域。而该问题仍然没有很好的解决方案，人们通常是额外的增加几个样本来缓解这一问题。在我看来粒子匮乏问题和粒子退化问题的根源是同一个——样本的数量不够大。假如我们有无穷多个样本，就一定可以很好的结果，但这是不切实际的。

此外，粒子滤波器还留下了一个开放问题，就是用于估计目标分布的建议分布的选择。合理的建议分布选择可以在一定程度上缓解粒子退化或者粒子匮乏问题，而且取得较高的近似精度。

粒子滤波器

1. 最优贝叶斯估计

2. 序列重要性采样算法(Sequential Importance Sampling, SIS)

3. 重采样

4. 完

5. 参考文献