Particle Filter
Table of Contents
1. 粒子滤波简介
阅读本文前,需要先理解“贝叶斯滤波(Bayes Filter)”,如果不了解它,可以参考:http://aandds.com/blog/bayes-filter.html ,还需要理解 Sampling Importance Resampling(SIR)算法,如果不了解它,可以参考:http://aandds.com/blog/statistics-simulation.html
这里简单回顾一下贝叶斯滤波相关结论。
贝叶斯滤波用于估计 已知系统控制量(即
贝叶斯滤波需要知道下面三个概率分布(通过建模可得到这些概率分布):
(1) 传感器模型(Measurements Model)
(2) 系统控制量模型(Motion Model)
(3) 系统的初始状态概率分布
这里有一个对“有限状态的离散系统”直接用贝叶斯滤波的递推式进行状态估计的例子: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. 粒子滤波算法
粒子滤波的主要思想是 用后验分布(
在粒子滤波中,后验概率密度函数的样本点称为粒子(Particle)。假设共有
粒子滤波算法描述如图 1 所示。
Figure 1: The particle filter algorithm(图片改编自:Probabilistic Robotics, by Sebastian Thrun, 4.2 The Particle Filter)
说明 1:上面算法中第 4 行,是从 proposal distribution 中抽样的过程。粒子滤波中,SIR 算法中的 proposal distribution 选择的是
说明 2:上面算法中第 5 行,是计算权重
说明 3:上面算法中第 8-11 行,是 SIR 算法的重抽样过程,重抽样结束后
2.1. 粒子滤波算法推导
粒子滤波算法是 SIR 算法的应用。推导粒子滤波只需要证明
可以认为,粒子是下面状态序列的样本:
记
证明使用粒子滤波算法时
所以,我们可以通过证明
说明:上面的推荐过程和贝叶斯滤波中后验概率的推导基本上是一样的,只是贝叶斯滤波的
利用前面的结论,继续推导:
证毕。
参考: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)可以用于非线性系统, 但卡尔曼滤波器和扩展卡尔曼滤波器均要求状态变量和噪声(过程噪声和测试噪声)都服从正态分布。
粒子滤波没有上面这些限制。