Particle Filter Tutorial 粒子滤波:从推导到应用(一)

版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/heyijia0327/article/details/40899819

前言

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

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

     文章架构:

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

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

一、贝叶斯滤波

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

        如:       (1)

               如:                                                                  (2)

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

      从贝叶斯理论的观点来看,状态估计问题(目标跟踪、信号滤波)就是根据之前一系列的已有数据(后验知识)递推的计算出当前状态的可信度。这个可信度就是概率公式,它需要通过预测更新两个步奏来递推的计算。

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

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

     为了进行递推,不妨假设已知k-1时刻的概率密度函数

     预测:由上一时刻的概率密度得到,这个公式的含义是既然有了前面1:k-1时刻的测量数据,那就可以预测一下状态x(k)出现的概率。

     计算推导如下:

     

                         

                         

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

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

      第一:既然 ,x(k)都只由x(k-1)决定了,即,在这里弄一个是什么意思?

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

     第二:上面公式的最后一行是假设已知的,但是怎么得到呢?

     其实它由系统状态方程决定,它的概率分布形状和系统的过程噪声形状一模一样。如何理解呢?观察状态方程(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)) 这个常数就行了。

     更新:由得到后验概率。这个后验概率才是真正有用的东西,上一步还只是预测,这里又多了k时刻的测量,对上面的预测再进行修正,就是滤波了。这里的后验概率也将是代入到下次的预测,形成递推。

     推导:

     

                              

其中归一化常数:

      

等式第一行到第二行是因为测量方程知道, y(k)只与x(k)有关,也称之为似然函数,由量测方程决定。也和上面的推理一样,, x(k)部分是常数,也是只和量测噪声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.百度文库 《粒子滤波理论》

     

没有更多推荐了,返回首页