从贝叶斯滤波到粒子滤波

0 回顾与引言

在此前的文章《从概率到贝叶斯滤波》中，我们曾经提到，贝叶斯滤波框架下求解预测步中的先验概率密度函数 $f_{X_k}^{-}(x)$、更新步中的归一化常数 ${\eta }_k$、状态量最优估计 ${\hat{x}}_k$ 时均涉及到无穷积分，大多数情况无法得到解析解，使得贝叶斯滤波算法的直接应用十分困难。为了解决贝叶斯滤波积分难的问题，通常从两个角度出发：

(1) 作理想假设

• 线性高斯
  假设状态转移函数 $g(x)$ 和观测函数 $h(x)$ 均为线性函数，过程噪声随机变量 $Q_k$ 和观测噪声随机变量 $R_k$ 均服从均值为 0 的正态分布。线性高斯问题可通过卡尔曼滤波（Kalman Filter）进行解决，详细内容请参考此前文章《从贝叶斯滤波到卡尔曼滤波》。

• 非线性高斯
  假设状态转移函数 $g(x)$ 和（或）观测函数 $h(x)$ 为非线性函数，过程噪声随机变量 $Q_k$ 和观测噪声随机变量 $R_k$ 均服从均值为 0 的正态分布。非线性高斯问题可通过扩展卡尔曼滤波（Extended Kalman Filter）和无迹卡尔曼滤波（Unscented Kalman Filter）进行解决。扩展卡尔曼滤波通过一阶泰勒级数展开将非线性高斯系统的状态转移函数 $g(x)$ 和（或）观测函数 $h(x)$ 线性化，然后采用标准卡尔曼滤波框架实现状态量的滤波过程，详细内容请参考此前文章《从贝叶斯滤波到扩展卡尔曼滤波》；无迹卡尔曼滤波基于无迹变换，无迹变换研究的是如何通过确定的采样点捕获经非线性变换的高斯随机变量的后验分布的问题，通过无迹变换得到相应的统计特性后，再结合标准卡尔曼滤波框架，便得到无迹卡尔曼滤波，详细内容请参考此前文章《从贝叶斯滤波到无迹卡尔曼滤波》。

(2) 化连续为离散

对于强非线性非高斯问题，需要将无穷积分转化为离散的数值积分——即化整为零，由此可以引申出本文的主题——粒子滤波（Particle Filter，PF）。

粒子滤波是贝叶斯滤波的一种非参数实现，所谓非参数，即不对滤波状态量的后验概率密度作任何假设。粒子滤波的主要思想是用一系列从后验得到的带权重的随机采样表示后验。从采样的角度考虑，粒子滤波与无迹卡尔曼滤波相似，区别在于，无迹卡尔曼滤波使用 sigma 确定性采样，通过无迹变换计算 sigma 样本点的位置与权重；而粒子滤波使用蒙特卡罗随机采样从建议分布中得到样本（粒子），并通过观测值更新粒子权重，针对粒子的权值退化问题，还涉及粒子的重采样步骤。粒子滤波算法广泛用于解决无人车的定位问题。

关键词： 贝叶斯，大数定律，蒙特卡罗，重要性采样，序贯重要性采样，粒子退化，重采样，采样重要性重采样，无人车定位

1 蒙特卡罗方法

假设存在某一连续型随机变量 $X$，其概率密度函数为 $p(x)$，则 $X$ 的数学期望为：

$$\mathrm{E}(X)=\int x\cdot p(x)\mathrm{d}x\text{\hspace{1em}(1.1)}$$

若存在另一连续型随机变量 $Y$，满足 $Y=g(X)$，则 $Y$ 的数学期望为：

$$\mathrm{E}(Y)=\int g(x)\cdot p(x)\mathrm{d}x\text{\hspace{1em}(1.2)}$$

正如我们所知道的，上式中的 $\mathrm{E}(X)$ 和 $\mathrm{E}(Y)$ 可能很难得到解析解。蒙特卡罗（Monte Carlo）方法告诉我们，可以通过对随机变量的概率密度进行随机采样，并对样本进行加权求和来近似随机变量的期望，如此一来，积分问题便转化为有限样本点的求和问题。

假设 $\mathcal{X}$ 是从 $p(x)$ 中进行随机采样得到的样本集合，样本数量为 $N$，每一个样本（粒子）${\mathcal{X}}^{(i)}$ 代表 $X$ 的一种可能状态，即

$${\mathcal{X}}^{(i)}\sim p(x)\text{\hspace{1em}(1.3)}$$

根据辛钦大数定律，$\mathcal{X}$ 的样本均值依概率 1 收敛于数学期望，即对 $\forall ϵ>0$，有

$$\underset{N\to \infty }{\lim }P\left\{|\frac{1}{N}\sum _{i=0}^{N-1}{\mathcal{X}}^{(i)}-\mathrm{E}(X)|<ϵ\right\}=1\text{\hspace{1em}(1.4)}$$

$$\underset{N\to \infty }{\lim }P\left\{|\frac{1}{N}\sum _{i=0}^{N-1}g({\mathcal{X}}^{(i)})-\mathrm{E}(Y)|<ϵ\right\}=1\text{\hspace{1em}(1.5)}$$

式 (1.4) 和 (1.5) 意味着，当样本数量 $N$ 足够大时，有

$$\mathrm{E}(X)\approx \frac{1}{N}\sum _{i=0}^{N-1}{\mathcal{X}}^{(i)}\text{\hspace{1em}(1.6)}$$

$$\mathrm{E}(Y)\approx \frac{1}{N}\sum _{i=0}^{N-1}g({\mathcal{X}}^{(i)})\text{\hspace{1em}(1.7)}$$

2 重要性采样

2.1 什么是重要性采样

如上述，如果我们能够对概率密度函数 $p(x)$ 进行随机采样，便可通过对样本进行加权求和来近似随机变量 $X$ 和 $Y=g(X)$ 的数学期望，然而现实情况是，$p(x)$ 可能很难直接采样，甚至根本无法采样。此时，可以选择从一个更加容易采样的概率密度函数 $q(x)$ 中进行随机采样得到样本集合 $\mathcal{X}$，并通过 $\mathcal{X}$ 近似估计 $\mathrm{E}(Y)$

$$\begin{array}{rl}\mathrm{E}(Y)& =\int g(x)\cdot p(x)\mathrm{d}x\\ & =\int g(x)\frac{p(x)}{q(x)}\cdot q(x)\mathrm{d}x\\ & \approx \frac{1}{N}\sum _{i=0}^{N-1}g({\mathcal{X}}^{(i)})\frac{p({\mathcal{X}}^{(i)})}{q({\mathcal{X}}^{(i)})}\end{array}\text{\hspace{1em}(2.1)}$$

其中

$${\mathcal{X}}^{(i)}\sim q(x)\text{\hspace{1em}(2.2)}$$

上述的采样方式称为重要性采样（Importance Sampling），其中的更易采样的概率密度函数 $q(x)$ 称为建议分布（Proposal Distribution），也称重要性密度（Importance Density）或重要性函数（Importance Function）。

令

$$w(x)=\frac{p(x)}{q(x)}\text{\hspace{1em}(2.3)}$$

显然

$$\begin{array}{rl}\int p(x)\mathrm{d}x& =\int w(x)\cdot q(x)\mathrm{d}x\\ & \approx \frac{1}{N}\sum _{i=0}^{N-1}w({\mathcal{X}}^{(i)})\\ & =1\end{array}\text{\hspace{1em}(2.4)}$$

故，式 (2.1) 可改写为

$$\begin{array}{rl}\mathrm{E}(Y)& \approx \frac{1}{N}\sum _{i=0}^{N-1}g({\mathcal{X}}^{(i)})\frac{p({\mathcal{X}}^{(i)})}{q({\mathcal{X}}^{(i)})}\\ & \approx \frac{\frac{1}{N}\sum _{i=0}^{N-1}g({\mathcal{X}}^{(i)})w({\mathcal{X}}^{(i)})}{\frac{1}{N}\sum _{i=0}^{N-1}w({\mathcal{X}}^{(i)})}\\ & \approx \sum _{i=0}^{N-1}g({\mathcal{X}}^{(i)})\frac{w({\mathcal{X}}^{(i)})}{\sum _{i=0}^{N-1}w({\mathcal{X}}^{(i)})}\end{array}\text{\hspace{1em}(2.5)}$$

我们记 $w^{(i)}=w({\mathcal{X}}^{(i)})$，并记

$$\begin{array}{rl}{\tilde{w}}^{(i)}& =\frac{w({\mathcal{X}}^{(i)})}{\sum _{i=0}^{N-1}w({\mathcal{X}}^{(i)})}\\ & =\frac{w^{(i)}}{\sum _{i=0}^{N-1}{w}^{(i)}}\end{array}\text{\hspace{1em}(2.6)}$$

$w^{(i)}$ 我们称之为粒子 ${\mathcal{X}}^{(i)}$ 的非归一化的重要性权重（Unnormalized Importance Weight），${\tilde{w}}^{(i)}$ 我们称之为归一化的重要性权重（Normalized Importance Weight），则式 (2.5) 等价于

$$\mathrm{E}(Y)\approx \sum _{i=0}^{N-1}g({\mathcal{X}}^{(i)}){\tilde{w}}^{(i)}\text{\hspace{1em}(2.7)}$$

从式 (2.7) 中我们不难发现，归一化的重要性权重 ${\tilde{w}}^{(i)}$ 即对应粒子 ${\mathcal{X}}^{(i)}$ 的离散概率值（概率质量），只要得到了粒子 ${\mathcal{X}}^{(i)}$ 及其对应的归一化的重要性权重 ${\tilde{w}}^{(i)}$，便可通过式 (2.7) 近似估计期望 $\mathrm{E}(Y)$。简言之，重要性采样要解决的是原分布难以采样甚至无法采样的问题。

2.2 序贯重要性采样

上述的重要性采样过程针对的是单一随机变量的估计，而对于贝叶斯估计而言，我们需要处理的是从 $0$ 时刻到 $k$ 时刻的随机过程

X 0 : k = { X 0 , X 1 , X 2 , ⋯   , X k } (2.8) X_{0:k}=\{X_0, X_1, X_2, \dotsb, X_k\} \tag{2.8} X0:k​={ X0​,X1​,X2​,⋯,Xk​}(2.8)

意即，我们需要估计的是条件期望

$$\mathrm{E}[g(X_k)|Z_{1:k}]=\int g(x_k)\cdot p(x_k|z_{1:k})\mathrm{d}{x}_k\text{\hspace{1em}(2.9)}$$

类似 2.1 节中的推导过程，我们从一个更加容易采样的概率密度函数 $q(x_{0:k}|z_{1:k})$ 中进行随机采样得到样本集合 ${\mathcal{X}}_{0:k}$，并通过 ${\mathcal{X}}_{0:k}$ 近似估计条件期望 $\mathrm{E}[g(X_k)|Z_{1:k}]$

$$\begin{array}{rl}\mathrm{E}[g(X_k)|Z_{1:k}]& =\int g(x_k)\cdot p(x_k|z_{1:k})\mathrm{d}{x}_k\\ & =\int g(x_k)\cdot p(x_{0:k}|z_{1:k})\mathrm{d}{x}_{0:k}\\ & =\int g(x_k)\frac{p(x_{0:k}|z_{1:k})}{q(x_{0:k}|z_{1:k})}\cdot q(x_{0:k}|z_{1:k})\mathrm{d}{x}_{0:k}\\ & \approx \frac{1}{N}\sum _{i=0}^{N-1}g({\mathcal{X}}_k^{(i)})\frac{p({\mathcal{X}}_{0:k}^{(i)}|z_{1:k})}{q({\mathcal{X}}_{0:k}^{(i)}|z_{1:k})}\end{array}\text{\hspace{1em}(2.10)}$$

其中

$${\mathcal{X}}_{0:k}^{(i)}\sim q(x_{0:k}|z_{1:k})\text{\hspace{1em}(2.11)}$$

${\mathcal{X}}_{0:k}^{(i)}$ 是由每一时刻的第 $i$ 个样本粒子依次增广构成的粒子簇

X 0 : k ( i ) = { X 0 ( i ) , X 1 ( i ) , ⋯   , X k ( i ) } i = 0 , 1 , 2 , ⋯   , N − 1 (2.12) \mathcal{X}_{0:k}^{(i)}=\{\mathcal{X}_0^{(i)}, \mathcal{X}_1^{(i)}, \dotsb, \mathcal{X}_k^{(i)}\} \quad i=0, 1, 2, \dotsb, N-1 \tag{2.12} X0:k(i)​={ X0(i)​,X1(i)​,⋯,Xk(i)​}i=0,1,2,⋯,N−1(2.12)

令

$$w(x_{0:k})=\frac{p(x_{0:k}|z_{1:k})}{q(x_{0:k}|z_{1:k})}\text{\hspace{1em}(2.13)}$$

显然

$$\begin{array}{rl}\int p(x_{0:k}|z_{1:k})\mathrm{d}{x}_{0:k}& =\int w(x_{0:k})\cdot q(x_{0:k}|z_{1:k})\mathrm{d}{x}_{0:k}\\ & \approx \frac{1}{N}\sum _{i=0}^{N-1}w({\mathcal{X}}_{0:k}^{(i)})\\ & =1\end{array}\text{\hspace{1em}(2.14)}$$

故，式 (2.10) 可改写为

$$\begin{array}{rl}\mathrm{E}[g(X_k)|Z_{1:k}]& \approx \frac{1}{N}\sum _{i=0}^{N-1}g({\mathcal{X}}_k^{(i)})\frac{p({\mathcal{X}}_{0:k}^{(i)}|z_{1:k})}{q({\mathcal{X}}_{0:k}^{(i)}|z_{1:k})}\\ & \approx \frac{\frac{1}{N}\sum _{i=0}^{N-1}g({\mathcal{X}}_k^{(i)})w({\mathcal{X}}_{0:k}^{(i)})}{\frac{1}{N}\sum _{i=0}^{N-1}w({\mathcal{X}}_{0:k}^{(i)})}\\ & \approx \sum _{i=0}^{N-1}g({\mathcal{X}}_k^{(i)})\frac{w({\mathcal{X}}_{0:k}^{(i)})}{\sum _{i=0}^{N-1}w({\mathcal{X}}_{0:k}^{(i)})}\end{array}\text{\hspace{1em}(2.15)}$$

我们记 $w_{}$