粒子滤波（PF）原理详解

背景介绍

在引入粒子滤波之前，有必要对其相关知识进行介绍。

动态模型

动态模型的一个主要特点是在概率模型中引入时间序列的概念，使得每个样本之间不再独立同分布，而变得有依赖关系了。

我们所看到的序列被称为观测序列，每个观测值背后都有一个隐变量，这个隐变量即系统状态，因此，动态模型又称为状态空间模型。

动态模型又分为三大类：

• 隐马尔科夫模型（隐变量必须离散，观测变量可以离散也可以连续）；
• 线性动态系统；
• 非线性非高斯的动态系统。

这三类模型都可以用一张图表示：

动态模型满足两个假设：

• 齐次马尔可夫假设：表示隐状态之间的关系，当给定$z_{t-1}$时，$z_t$只和$z_{t-1}$有关；
• 观测独立性假设：表示隐状态和观测值之间的关系，当给定$z_t$时，$x_t$只和$z_t$有关。

动态模型可以用两个方程定义：
$$\begin{gathered} z_t=g(z_{t-1},u,\epsilon ) \\ {x}_t=h(z_t,u,\delta ) \end{gathered}$$
第一个式子表示前一个状态和后一个状态之间的关系；第二个式子表示该时刻的状态和该时刻的观测值之间的关系。$u$是系统的一个输入信号，可有可无。

卡尔曼滤波

卡尔曼滤波是一个典型的线性动态系统，它可以被定义为：
$$\begin{gathered} z_t=A\cdot z_{t-1}+B+\epsilon \\ {x}_t=C\cdot z_t+D+\delta \end{gathered}$$
其中，$\epsilon \sim N(0,Q)$，$\delta \sim N(0,R)$，即$\epsilon$和$\delta$都是服从高斯分布的噪声，它们的方差矩阵分别为$Q$和$R$。

不管是线性动态系统还是非线性动态系统，它们所关注的问题通常都是filtering问题，即给定了第1时刻到第t时刻的观测值，求t时刻隐状态的概率，也就是$P(z_t|x_1,\cdots ,x_t)$。

在求解filtering问题时，通常有两个步骤：

• 1、预测
  $$P(z_t|x_1,\cdots ,x_{t-1})={\int }_{z_{t-1}}P(z_t|z_{t-1})\cdot P(z_{t-1}|x_1,\cdots ,x_{t-1})d{z}_{t-1}$$
• 2、更新
  $$P(z_t|x_1,\cdots ,x_t)\propto P(x_t|z_t)\cdot P(z_t|x_1,\cdots ,x_{t-1})$$

在这两个步骤中，$P(z_t|x_1,\cdots ,x_{t-1})$相当于对$z_t$的先验，因为此时$t$时刻的观测值是未知的；在知道$t$时刻的观测值$x_t$之后，对该先验概率进行更新，得到$z_t$的后验概率$P(z_t|x_1,\cdots ,x_t)$。

对预测公式的详细推导如下：
$$\begin{array}{rl}P(z_t|x_1,\cdots ,x_{t-1})& ={\int }_{z_{t-1}}P(z_{t-1},z_t|x_1,\cdots ,x_{t-1})d{z}_{t-1}\\ & ={\int }_{z_{t-1}}P(z_t|z_{t-1},x_1,\cdots ,x_{t-1})\cdot P(z_{t-1}|x_1,\cdots ,x_{t-1})d{z}_{t-1}\\ & ={\int }_{z_{t-1}}P(z_t|z_{t-1})\cdot P(z_{t-1}|x_1,\cdots ,x_{t-1})d{z}_{t-1}\end{array}$$
对更新公式的详细推导如下：
$$\begin{array}{rl}P(z_t|x_1,\cdots ,x_t)& =\frac{P(x_1,\cdots ,x_t,z_t)}{P(x_1,\cdots ,x_t)}\\ & =\frac{1}{P(x_1,\cdots ,x_t)}P(x_t|x_1,\cdots ,x_{t-1},z_t)\cdot P(x_1,\cdots ,x_{t-1},z_t)\\ & =\frac{1}{P(x_1,\cdots ,x_t)}P(x_t|z_t)\cdot P(z_t|x_1,\cdots ,x_{t-1})P(x_1,\cdots ,x_{t-1})\\ & =\frac{P(x_1,\cdots ,x_{t-1})}{P(x_1,\cdots ,x_t)}P(x_t|z_t)\cdot P(z_t|x_1,\cdots ,x_{t-1})\end{array}$$
此处的第一项由于概率中和隐变量没有关系，属于已知量，所以可作为常数看待。

卡尔曼滤波中的所有概率（条件概率、联合概率等）都是服从高斯分布的，因此我们可以得到filtering问题的解析解。但是对于非线性非高斯的动态系统，所有概率都有可能不是高斯分布的，因此是得不到解析解的，我们只能通过采样的方式（蒙特卡洛法）来近似求得所有概率的值。

重要性采样

蒙特卡洛采样法

本节主要介绍蒙特卡洛采样法，即以抽样的方法去近似求得后验概率$P(z|x)$。大多数情况下，当得到后验概率时，我们更关心的是与该后验概率相关得期望，如$E_{z|x}[f(x)]$。
$$E_{z|x}[f(z)]=\int f(z)P(z|x)dz\approx \frac{1}{N}\sum _{i=1}^{N}f(z^i)$$
这里从$P(z|x)$中随机采样了N个样本，记作$z^{(1)},z^{(2)},\cdots ,z^{(N)}$。

重要性采样基本原理

当$P(z|x)$很复杂，或维度特别高时，我们难以在$P(z|x)$中采样，此时需要应用重要性采样（importance sampling）来解决这一问题。

在重要性采样中，我们引入了一个简单的，易于采样的分布$q(z|x)$，则：
$$\begin{array}{rl}{E}_{z|x}[f(z)]& =\int f(z)P(z|x)dz\\ & =\int f(z)\frac{p(z|x)}{q(z|x)}q(z|x)dz\\ & =\frac{1}{N}\sum _{i=1}^{N}f(z^{(i)})\cdot \frac{p(z^{(i)}|x)}{q(z^{(i)}|x)}\end{array}$$
这里从$q(z|x)$中随机采样了N个样本，记作$z^{(1)},z^{(2)},\cdots ,z^{(N)}$。上式中的$q(z|x)$称为提议分布（proposal distribution），$\frac{p(z^{(i)}|x)}{q(z^{(i)}|x)}$表示权重（weight），可以写作$w^i$。

将重要性采样引入filtering问题

这里的filtering问题也就是之前在背景里讲过的$P(z_t|x_1,\cdots ,x_t)=P(z_t|x_{1:t})$，此时t时刻第i个样本的权重可写成$w_t^{(i)}=\frac{P(z_t^{(i)}|x_{1:t})}{q(z_t^{(i)}|x_{1:t})}$。

在迭代计算filtering问题过程中，我们需要在每个时刻都求N个权重（$w_1^{(1)},w_1^{(2)},\cdots ,w_1^{(N)}$），这是十分困难的（主要是因为$P(z_t^{(i)}|x_{1:t})$不好求）。因此我们希望找到一个递推公式使得通过前一个时刻求出来的权重得到后一个时刻的权重。

由此想法，引出另一个算法，即序列重要性采样（sequential importance sampling, SIS）。

序列重要性采样

序列重要性采样的思路是找到$w_t^{(i)}$和$w_{t-1}^{(i)}$这两个权重之间的递推关系。为简便（避免积分运算），SIS将关注点设在求概率$P(z_{1:t}|x_{1:t})$上，而不是$P(z_t|x_{1:t})$上，因此
$$w_t^{(i)}\propto \frac{P(z_{1:t}|x_{1:t})}{q(z_{1:t}|x_{1:t})}$$
首先看分子：
$$\begin{array}{rl}P(z_{1:t}|x_{1:t})& =\frac{P(z_{1:t},x_{1:t})}{P(x_{1:t})}\\ & =\frac{1}{P(x_{1:t})}P(z_{1:t},x_{1:t})\\ & =\frac{1}{P(x_{1:t})}P(x_t|z_{1:t},x_{1:t-1})P(z_{1:t},x_{1:t-1})\\ & =\frac{1}{P(x_{1:t})}P(x_t|z_t)P(z_{1:t,x_{1:t-1}})\\ & =\frac{1}{P(x_{1:t})}P(x_t|z_t)P(z_t|z_{1:t-1},x_{1:t-1})P(z_{1:t-1},x_{1:t-1})\\ & =\frac{1}{P(x_{1:t})}P(x_t|z_t)P(z_t|z_{t-1})P(z_{1:t-1},x_{1:t-1})\\ & =\frac{1}{P(x_{1:t})}P(x_t|z_t)P(z_t|z_{t-1})P(z_{1:t-1}|x_{1:t-1})P(x_{1:t-1})\\ & =\frac{P(x_{1:t-1})}{P(x_{1:t})}P(x_t|z_t)P(z_t|z_{t-1})P(z_{1:t-1}|x_{1:t-1})\end{array}$$
注意，这里的$P(x_{1:t-1})$和$P(x_{1:t})$都是已知的，视为常数。

然后看分母：
由于$q$是我们任意指定的，所以我们假定$q$为以下这种形式：
$q(z_{1:t}|x_{1:t})=q(z_t|z_{1:t-1},x_{1:t})q(z_{1:t-1}|x_{1:t-1})$

综合以上，我们得到：
$$w_t^{(i)}\propto \frac{P(z_{1:t}|x_{1:t})}{q(z_{1:t}|x_{1:t})}\propto \frac{P(x_t|z_t)P(z_t|z_{t-1})P(z_{1:t-1}|x_{1:t-1})}{q(z_t|z_{1:t-1},x_{1:t})q(z_{1:t-1}|x_{1:t-1})}=\frac{P(x_t|z_t)P(z_t|z_{t-1})}{q(z_t|z_{1:t-1},x_{1:t})}{w}_{t-1}^{(i)}$$
这样一来，就解决了从前一时刻的权重递推后一时刻权重的问题了。

基本粒子滤波算法

从上一节我们已知$w_t^{(i)}\propto \frac{P(x_t|z_t)P(z_t|z_{t-1})}{q(z_t|z_{1:t-1},x_{1:t})}{w}_{t-1}^{(i)}$，此时假设$z_t$只和$z_{t-1}$相关，则$q(z_t|z_{1:t-1},x_{1:t})=q(z_t|z_{t-1},x_{1:t})$。

总结来说该算法可以写成：

• 前提：t-1时刻的采样已经完成，则$w_{t-1}^{(i)}$已知；

• 在t时刻：

• for i=1: N

  • $z_t^{(i)}\sim q(z_t|z_{t-1}^{(i)},x_{1:t})$
  • $w_t^{(i)}\propto \frac{P(x_t|z_t^{(i)})P(z_t^{(i)}|z_{t-1}^{(i)})}{q(z_t^{(i)}|z_{t-1}^{(i)},x_{1:t})}{w}_{t-1}^{(i)}$

• end

• $w_t^{(i)}$归一化：${\sum }_{i=1}^N{w}_t^{(i)}=1$

在实际运行过程中，SIS会出现权值退化（权值衰减）问题，即在迭代过程中，$w_t^{(i)}$会变得越来越小，或越来越不平均。比如有100个样本，99个样本的权重都接近0，只有一个样本的权重接近1。这样的样本方差过大，不适合去逼近一个分布。这种问题是由高维空间所引起的，维数越高，需要的样本容量是呈指数级往上增的（维度灾难）。应对这个问题，通常有两个解决方法：

• 1、重采样
• 2、选择一个更好的提议分布$q(z|x)$

这里着重讲一下第一种方法，重采样，先用一张图来解释一下：

假设共采样了三个样本，它们的权重分别为0.1，0.1，0.8，显然第三个样本的权重远远大于另外两个样本。重采样的思想实际上就是将样本的权重视为概率，即有0.1的概率会抽取到样本1和样本2，有0.8的概率会抽取到样本3。因此，在重采样后，假设重新抽取到了10个样本，那么它们的分布很大可能会如上图所示，有80%的样本属于原来的样本3，有10%的样本属于原来的样本1和样本2。新采样得到的样本的权重都是相同的（0.1）。

实际上将序列重要性采样与重采样结合起来就是基本的粒子滤波算法。

SIR Filter

提议分布的选择与最终效果的好坏是息息相关的，一般来说，我们选择
$$q(z_t|z_{1:t-1},x_{1:t})=q(z_t|z_{t-1},x_{1:t})=P(z_t|z_{t-1})$$
在上一节中我们已经推导出了基本粒子滤波算法的步骤，那么可将其改进为：

• $z_t^{(i)}\sim q(z_t|z_{t-1}^{(i)},x_{1:t})=P(z_t|z_{t-1}^{(i)})$
• $w_t^{(i)}\propto \frac{P(x_t|z_t^{(i)})P(z_t^{(i)}|z_{t-1}^{(i)})}{q(z_t^{(i)}|z_{t-1}^{(i)},x_{1:t})}{w}_{t-1}^{(i)}=P(x_t|z_t^{(i)})w_{t-1}^{(i)}$

整个改进完的算法叫做Sampling importance resampling（SIR Filter），实际上就是SIS+Resampling+提议分布。