Particle Filter

Table of Contents

1 粒子滤波简介

阅读本文前,需要先理解(1)“贝叶斯滤波(Bayes Filter)”,如果不了解它,可以参考:http://aandds.com/blog/bayes-filter.html ,(2)Sampling Importance Resampling(SIR)算法,如果不了解它,可以参考:http://aandds.com/blog/statistics-simulation.html

这里简单回顾一下贝叶斯滤波相关结论。
贝叶斯滤波用于估计 已知系统控制量(即 \(u_{1:t}\) )和状态测量值(即 \(z_{1:t}\) )时系统的状态,即后验证概率密度 \(p(x_t \mid u_{1:t}, z_{1:t})\) ,一般我们把它记为 \(bel(x_t) = p(x_t \mid u_{1:t}, z_{1:t})\) 。 贝叶斯滤波的递推公式为:
\[{\color{red}{bel(x_t)}} = \eta \, p(z_t \mid x_t) \int p(x_t \mid x_{t-1}, u_t) {\color{red}{bel(x_{t-1})}} \, \mathrm{d} x_{t-1}\]

贝叶斯滤波需要知道下面三个概率分布(通过建模可得到这些概率分布):
(1) 传感器模型(Measurements Model) \(p(z_t \mid x_t)\)
(2) 系统控制量模型(Motion Model) \(p(x_t \mid u_t, x_{t-1})\)
(3) 系统的初始状态概率分布 \(bel(x_0) = p(x_0)\)

这里有一个对“有限状态的离散系统”直接用贝叶斯滤波的递推式进行状态估计的例子:http://aandds.com/blog/bayes-filter.html 。但大多数情况下,我们无法直接用贝叶斯滤波。

贝叶斯滤波需要进行积分运算,除了一些特殊的系统模型(如线性高斯系统,有限状态的离散系统)之外,对于一般的非线性、非高斯系统,贝叶斯滤波很难得到后验概率的封闭解析式。因此,现有的非线性滤波器多采用近似的计算方法解决积分问题,以此来获取估计的次优解。在系统的非线性模型可由在当前状态展开的线性模型有限近似的前提下,基于一阶或二阶Taylor级数展开的扩展Kalman滤波得到广泛应用。在一般情况下,逼近概率密度函数比逼近非线性函数容易实现。据此,Julier与Uhlmann提出一种Unscented Kalman滤波器(UKF),通过选定的sigma点来精确估计随机变量经非线性变换后的均值和方差,从而更好的近似状态的概率密度函数,其理论估计精度优于扩展Kalman滤波(EKF)。获取次优解的另外一中方案便是基于蒙特卡洛模拟的粒子滤波器。

摘自:http://wenku.baidu.com/view/88896d2b453610661ed9f4b4.html

2 粒子滤波算法

粒子滤波的主要思想是 用后验分布(\(p(x_t \mid u_{1:t}, z_{1:t})\))的样本来表达后验分布。 粒子滤波中,对后验分布抽样的采用的具体方法是Sampling Importance Resampling(SIR)算法。可以简单地认为 粒子滤波就是把SIR算法应用到贝叶斯滤波中。

在粒子滤波中,后验概率密度函数的样本点称为粒子(Particle)。假设共有 \(M\) 个粒子,把这些粒子记为:
\[\mathcal{X}_t := x_t^{[1]}, x_t^{[2]}, \cdots, x_t^{[M]}\]

粒子滤波算法描述如图 1 所示。

particle_filter.png

Figure 1: The particle filter algorithm(图片改编自:Probabilistic Robotics, by Sebastian Thrun, 4.2 The Particle Filter)

说明1:上面算法中第4行,是从proposal distribution中抽样的过程。粒子滤波中,SIR算法中的proposal distribution选择的是 \(p(x_t \mid u_t, x_{t-1}) bel(x_{t-1})\) 。 \(bel(x_{t-1})\) 是上一次的后验分布,用粒子表示就是 \(\mathcal{X}_{t-1}\) ,即这一步抽样用到了 \(\mathcal{X}_{t-1}\) 。
说明2:上面算法中第5行,是计算权重 \(w_t = \frac{\text{target distribution}}{\text{proposal distribution}} = \frac{bel(x_{t})}{p(x_t \mid u_t, x_{t-1}) bel(x_{t-1})}\) 的过程(后文将证明为什么 \(\frac{bel(x_{t})}{p(x_t \mid u_t, x_{t-1}) bel(x_{t-1})} \propto p(z_t \mid x_t)\) )。
说明3:上面算法中第8-11行,是SIR算法的重抽样过程,重抽样结束后 \(\mathcal{X}_t\) 中会有重复的粒子。

2.1 粒子滤波算法推导

粒子滤波算法是SIR算法的应用。推导粒子滤波只需要证明 \(\frac{\text{target distribution}}{\text{proposal distribution}} = \frac{bel(x_{t})}{p(x_t \mid u_t, x_{t-1}) bel(x_{t-1})} \propto p(z_t \mid x_t)\) 即可。

可以认为,粒子是下面状态序列的样本:
\[x_{0:t}^{[m]} = x_{0}^{[m]},x_{1}^{[m]}, \cdots, x_{t}^{[m]}\]

记 \(bel(x_{0:t}) = p(x_{0:t} \mid u_{1:t}, z_{1:t})\) ,如果我们证明了使用粒子滤波算法时 \(x_{0:t}^{[m]}\) 是 \(bel(x_{0:t})\) 的样本,那么使用同样算法时显然 \(x_{t}^{[m]}\) 会是 \(bel(x_{t})\) 的样本。

证明使用粒子滤波算法时 \(x_{0:t}^{[m]}\) 是 \(bel(x_{0:t})\) 的样本,那么对应的target distribution为 \(bel(x_{0:t})\) ,proposal distribution为 \(p(x_t \mid u_t, x_{t-1}) bel(x_{0:t-1})\) 。

所以,我们可以通过证明 \(\frac{bel(x_{0:t})}{p(x_t \mid u_t, x_{t-1}) bel(x_{0:t-1})} \propto p(z_t \mid x_t)\) ,来间接证明 \(\frac{bel(x_{t})}{p(x_t \mid u_t, x_{t-1}) bel(x_{t-1})} \propto p(z_t \mid x_t)\) 。

\[\begin{aligned} bel(x_{0:t}) & = p(x_{0:t} \mid u_{1:t}, z_{1:t}) \\ & \stackrel{\text{Bayes}}{=} \frac{p(z_t \mid x_{0:t}, u_{1:t}, z_{1:t-1})p(x_{0:t} \mid u_{1:t}, z_{1:t-1})}{p(z_t \mid u_{1:t}, z_{1:t-1})} \\ & = \eta \, p(z_t \mid x_{0:t}, u_{1:t}, z_{1:t-1})p(x_{0:t} \mid u_{1:t}, z_{1:t-1}) \\ & \stackrel{\text{Markov}}{=} \eta \, p(z_t \mid x_{t})p(x_{0:t} \mid u_{1:t}, z_{1:t-1}) \\ & = \eta \, p(z_t \mid x_{t}) p(x_t \mid x_{0:t-1}, u_{1:t}, z_{1:t-1}) p(x_{0:t-1} \mid u_{1:t}, z_{1:t-1}) \\ & \stackrel{\text{Markov}}{=} \eta \, p(z_t \mid x_{t}) p(x_t \mid x_{t-1}, u_{t}) p(x_{0:t-1} \mid u_{1:t-1}, z_{1:t-1}) \\ \end{aligned}\]

说明:上面的推荐过程和贝叶斯滤波中后验概率的推导基本上是一样的,只是贝叶斯滤波的 \(x_t\) 变成了这里的 \(x_{0:t}\) ,就是这个不同,使得贝叶斯估计里需要积分,而这里后验概率的分解形式却没有积分。

利用前面的结论,继续推导:
\[\begin{aligned} \frac{\text{target distribution}}{\text{proposal distribution}} & = \frac{bel(x_{0:t})}{p(x_t \mid u_t, x_{t-1}) bel(x_{0:t-1})} \\ &= \frac{\eta \, p(z_t \mid x_{t}) p(x_t \mid x_{t-1}, u_{t}) p(x_{0:t-1} \mid u_{1:t-1}, z_{1:t-1})}{p(x_t \mid u_t, x_{t-1}) p(x_{0:t-1} \mid u_{1:t-1}, z_{1:t-1})} \\ & = \eta \, p(z_t \mid x_{t}) \\ & \propto p(z_t \mid x_{t}) \\ \end{aligned}\]

证毕。

参考:Probabilistic Robotics, by Sebastian Thrun, 4.2.3 Mathematical Derivation of the PF

2.2 粒子滤波的不足及改进

粒子滤波中,重抽样过程后,权重大的粒子会重复多个,而权重小的粒子很可能消失。这称为Particle Deprivation Problem。
其解决办法是:A popular solution to this problem is to add a small number of randomly generated particles into the set after each resampling process.

粒子滤波还有其它不足,可参考:Probabilistic Robotics, by Sebastian Thrun, 4.2.4 Properties of the Particle Filter

2.3 OpenCV中粒子滤波实现

粒子滤波又叫 Condensation algorithm

OpenCV中实现了Condensation算法。参考:Learning OpenCV, Chapter 10 Tracking and Motion. The Condensation Algorithm

3 卡尔曼滤波 vs. 粒子滤波

可以认为,卡尔曼滤波和粒子滤波都是贝叶斯滤波的近似实现。

卡尔曼滤波器(Kalman Filter)只能用于线性系统。扩展卡尔曼滤波器(Extended Kalman Filter, EKF)可以用于非线性系统, 但卡尔曼滤波器和扩展卡尔曼滤波器均要求状态变量和噪声(过程噪声和测试噪声)都服从正态分布。

粒子滤波没有上面这些限制。


Author: cig01

Created: <2016-06-12 Sun 00:00>

Last updated: <2017-05-16 Tue 23:30>

Creator: Emacs 25.1.1 (Org mode 9.0.7)