Particle Filter Tutorial 粒子滤波：从推导到应用（一）

前言：

早几年前写博客的时候，编辑公式都是在线网站编辑的，有时候这个网站不稳定，贴的公式看不到。如果有需要，pdf版下载地址点击打开链接，给您阅读带来的不便表示抱歉，祝大家好运。

博主在自主学习粒子滤波的过程中，看了很多文献或博客，不知道是看文献时粗心大意还是悟性太低，看着那么多公式，总是无法把握住粒子滤波的思路，也无法将理论和实践对应起来。比如：理论推导过程中那么多概率公式，概率怎么和系统的状态变量对应上了？状态粒子 $x_{k}$ 是怎么一步步采样出来的，为什么程序里面都是直接用状态方程来计算？粒子的权重是怎么来的？经过一段时间的理解，总算理清了它的脉络。同时也觉得，只有对理论的推导心中有数了，才能知道什么样的地方可以用这个算法，以及这个算法有什么不足。因此，本文将结合实际程序给出粒子滤波的详细推导，在推导过程中加入博主自己的理解，如有不妥，请指出，谢谢。

文章架构：

由最基础的贝叶斯估计开始介绍，再引出蒙特卡罗采样，重要性采样，SIS粒子滤波，重采样，基本粒子滤波Generic Particle Filter，SIR粒子滤波，这些概念的引进，都是为了解决上一个概念中出现的问题而环环相扣的。最后给出几个在matlab和python中的应用，例程包括图像跟踪，滤波，机器人定位。

再往下看之前，也可以看看《卡尔曼滤波:从推导到应用》，好对这种通过状态方程来滤波的思路有所了解。

一、贝叶斯滤波

假设有一个系统，我们知道它的状态方程，和测量方程如下：

$x_{k} = f_{k}\left ( x_{k-1},v_{k-1} \right )$ 如： $x_{k} = \frac{x_{k-1}}{2}+\frac{25x_{k-1}}{1+x_{k-1}^{2}}+8cos\left ( 1.2\left ( k-1 \right ) \right )+v_{k}$ (1)

$y_{k} = h_{k}\left ( x_{k},n_{k} \right )$ 如： $y_{k} = \frac{x_{k}^{2}}{20}+n_{k}$ (2)

其中x为系统状态，y为测量到的数据，f,h是状态转移函数和测量函数，v,n为过程噪声和测量噪声，噪声都是独立同分布的。上面对应的那个例子将会出现在程序中。

从贝叶斯理论的观点来看，状态估计问题（目标跟踪、信号滤波）就是根据之前一系列的已有数据 $y_{1:k}$ （后验知识）递推的计算出当前状态 $x_{k}$ 的可信度。这个可信度就是概率公式 $p\left ( x_{k}|y_{1:k} \right )$ ，它需要通过预测和更新两个步奏来递推的计算。

预测过程是利用系统模型(状态方程1)预测状态的先验概率密度，也就是通过已有的先验知识对未来的状态进行猜测，即p( x(k)|x(k-1) )。更新过程则利用最新的测量值对先验概率密度进行修正，得到后验概率密度，也就是对之前的猜测进行修正。

在处理这些问题时，一般都先假设系统的状态转移服从一阶马尔科夫模型，即当前时刻的状态x(k)只与上一个时刻的状态x(k-1)有关。这是很自然的一种假设，就像小时候玩飞行棋，下一时刻的飞机跳到的位置只由当前时刻的位置和骰子决定。同时，假设k时刻测量到的数据y(k)只与当前的状态x(k)有关，如上面的状态方程2。

为了进行递推，不妨假设已知k-1时刻的概率密度函数 $p\left ( x_{k-1}|y_{1:k-1} \right )$

预测：由上一时刻的概率密度 $p\left ( x_{k-1}|y_{1:k-1} \right )$ 得到 $p\left ( x_{k}|y_{1:k-1} \right )$ ，这个公式的含义是既然有了前面1：k-1时刻的测量数据，那就可以预测一下状态x(k)出现的概率。

计算推导如下：

$p\left ( x_{k}|y_{1:k-1} \right ) = \int p\left ( x_{k},x_{k-1}|y_{1:k-1} \right )d x_{k-1}$

$=\int p\left ( x_{k}|x_{k-1},y_{1:k-1} \right )p\left ( x_{k-1}|y_{1:k-1} \right )d x_{k-1}$

$=\int p\left ( x_{k}|x_{k-1} \right )p\left ( x_{k-1}|y_{1:k-1} \right )d x_{k-1}$

等式的第一行到第二行纯粹是贝叶斯公式的应用。第二行得到第三行是由于一阶马尔科夫过程的假设，状态x(k)只由x(k-1)决定。

楼主看到这里的时候想到两个问题：

第一：既然 $p\left ( x_{k-1}|x_{k-1},y_{1:k-1} \right )=p\left ( x_{k}|x_{k-1} \right )$ ，x(k)都只由x(k-1)决定了，即 $p\left ( x_{k}|x_{k-1} \right )$ ，在这里弄一个 $p\left ( x_{k}|y_{1:k-1} \right )$ 是什么意思？

这两个概率公式含义是不一样的，前一个是纯粹根据模型进行预测，x(k)实实在在的由x(k-1)决定，后一个是既然测到的数据和状态是有关系的，现在已经有了很多测量数据 y 了，那么我可以根据已有的经验对你进行预测，只是猜测x(k)，而不能决定x(k)。

第二：上面公式的最后一行 $p\left ( x_{k-1}|y_{1:k-1} \right )$ 是假设已知的，但是 $p\left ( x_{k}|x_{k-1} \right )$ 怎么得到呢？

其实 $p\left ( x_{k}|x_{k-1} \right )$ 它由系统状态方程决定，它的概率分布形状和系统的过程噪声 $v_{k-1}$ 形状一模一样。如何理解呢?观察状态方程(1)式我们知道，x(k) = Constant( x(k-1) ) + v(k-1) 也就是x(k)由一个通过x(k-1)计算出的常数叠加一个噪声得到。看下面的图：

如果没有噪声，x(k)完全由x(k-1)计算得到，也就没由概率分布这个概念了，由于出现了噪声，所以x(k)不好确定，他的分布就如同图中的阴影部分，实际上形状和噪声是一样的，只是进行了一些平移。理解了这一点，对粒子滤波程序中，状态x(k)的采样的计算很有帮助，要采样x(k)，直接采样一个过程噪声，再叠加上 f(x(k-1)) 这个常数就行了。

更新：由 $p\left ( x_{k}|y_{1:k-1} \right )$ 得到后验概率 $p\left ( x_{k}|y_{1:k} \right )$ 。这个后验概率才是真正有用的东西，上一步还只是预测，这里又多了k时刻的测量，对上面的预测再进行修正，就是滤波了。这里的后验概率也将是代入到下次的预测，形成递推。

推导：

$p\left ( x_{k}|y_{1:k} \right )=\frac{p\left ( y_{k}|x_{k},y_{1:k-1} \right )p\left ( x_{k}|y_{1:k-1} \right )}{p\left ( y_{k}|y_{1:k-1} \right )}$

$= \frac{p\left ( y_{k}|x_{k}\right )p\left ( x_{k}|y_{1:k-1} \right )}{p\left ( y_{k}|y_{1:k-1} \right )}$

其中归一化常数：

$p\left ( y_{k}|y_{1:k-1} \right ) = \int p\left ( y_{k}|x_{k} \right )p\left ( x_{k}|y_{1:k-1} \right )d x_{k}$

等式第一行到第二行是因为测量方程知道, y(k)只与x(k)有关， $p\left ( y_{k}|x_{k} \right )$ 也称之为似然函数，由量测方程决定。也和上面的推理一样， $y_{k}= h\left ( x_{k} \right )+n_{k}$ , x(k)部分是常数， $p\left ( y_{k}|x_{k} \right )$ 也是只和量测噪声n(k)的概率分布有关，注意这个也将为SIR粒子滤波里权重的采样提供编程依据。

贝叶斯滤波到这里就告一段落了。但是，请注意上面的推导过程中需要用到积分，这对于一般的非线性，非高斯系统，很难得到后验概率的解析解。为了解决这个问题，就得引进蒙特卡洛采样。关于它的具体推导请见下一篇博文。

（转载请注明作者和出处：http://blog.csdn.net/heyijia0327 未经允许请勿用于商业用途）

reference：

1.M. Sanjeev Arulampalam 《A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking》

2.ZHE CHEN 《Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond》

3.Sebastian THRUN 《Probabilistic Robotics》

3.百度文库《粒子滤波理论》

Particle Filter Tutorial 粒子滤波：从推导到应用（一）相关推荐

Particle Filter Tutorial 粒子滤波：从推导到应用（四）
六.Sampling Importance Resampling Filter (SIR) SIR滤波器很容易由前面的基本粒子滤波推导出来,只要对粒子的重要性概率密度函数做出特定的选择即可.在SIR中 ...
Particle Filter Tutorial 粒子滤波：从推导到应用（二）
二.蒙特卡洛采样假设我们能从一个目标概率分布p(x)中采样到一系列的样本(粒子),(至于怎么生成服从p(x)分布的样本,这个问题先放一放),那么就能利用这些样本去估计这个分布的某些函数的期望值.譬如 ...
Particle Filter Tutorial 粒子滤波：从推导到应用（三）
五.重采样在应用SIS 滤波的过程中,存在一个退化的问题.就是经过几次迭代以后,很多粒子的权重都变得很小,可以忽略了,只有少数粒子的权重比较大.并且粒子权值的方差随着时间增大,状态空间中的有效粒子数 ...
Particle Filter Tutorial 粒子滤波：从推导到应用
转载自作者白巧克力亦唯心的文章:粒子滤波文章目录 1.贝叶斯滤波 2.蒙特卡洛采样 3.重要性采样 4.SIS Filter(Sequential Importance Sampling) 5.重采 ...
粒子滤波的推导到应用
本文转自 http://blog.csdn.net/heyijia0327, Particle Filter Tutorial 粒子滤波:从推导到应用系列一.贝叶斯滤波假设有一个系统,我们知道它的 ...
学习笔记：粒子滤波的推导过程
前面的文章我们了解了到对于一个线性动态系统(比如说卡尔曼滤波)中的filtering问题可以用预测和跟新两个步骤求解,因为它们的后验概率和条件概率都服从高斯分布,所以比较容易求出解析解,但是对于非线性 ...
视频跟踪算法之粒子滤波
1. 写在前面最近在看视频跟踪方面的一些硕博士毕业论文,几乎看到的每一篇都会涉及到粒子滤波算法,所以这段时间花了很多时间在看相关的内容. 浏览了大量的博客和文章,跟着不停推导公式,感觉还是无法完全掌 ...
双通道粒子滤波(PF)原理详解
1.粒子滤波是动态系统非线性非高斯的情况 2.粒子滤波无法像卡尔曼滤波求得解析解,需要用蒙特卡洛采样 3.采取重要性采样方法近似计算也无法直接求解粒子滤波问题 4.序列重要性采样得到t时刻与t-1时刻 ...
[转]粒子滤波（particle filtering）的思路发展过程及应用（详细深度好文）
粒子滤波作为视觉SLAM中后端进行状态估计的主要算法之一,很好的完成了扩展卡尔曼滤波无法有效处理的复杂状态方程下的状态估计任务.这篇文章详细地描述了粒子滤波的思想历程,即如何一步步从简单的状态估计.采 ...

Particle Filter Tutorial 粒子滤波：从推导到应用（一）

（转载请注明作者和出处：http://blog.csdn.net/heyijia0327 未经允许请勿用于商业用途）

Particle Filter Tutorial 粒子滤波：从推导到应用（一）相关推荐

最新文章

热门文章