粒子滤波推导的博客的pdf版，详细推导了粒子滤波的由来，并且有例程代码。
这两个概率公式含义是不一样的,前一个是纯粹根据模型进行预测,x(k)实实在在的 由x(k-1)决定,后个是既然测到的数据和状态是有关系的,现在已经有了很多测量数据 y了,那么我可以根据已有的经验对你进行预测,只是猜测x(k),而不能决定X(k) 第二:上面公式的最后一行P(xk-11:k-1)是假设已知的,但是P( kak-1)怎么得 到呢? 其实P(xkxk-1)由系统状态方程法定,它的概率分布形状和系统的过程噪声k-1 形状一模一样。如何理解呢?观察状态方稈(1)式我们知道,x(k)= Constant(x(k-1)+ V(k-1)也就是x(k)由一个通过x(k-1)计算出的常数叠加一个噪声得到。看下面的图 如果没有噪声,X(k)完全由X(k-1)计算得到,也就没由概率分布这个概念了,由于出现了 噪声,所以x(k)不好确定,他的分布就如冋图中的阴影部分,实际上形状和噪声是一样 的,只是进行了一些平移。理解了这一点,对粒子滤波程序中,状态ⅹ(k)的采样的计算很 有帮助,要采样ⅹ(k),直接采样一个过程噪声,再叠加上f(x(k-1)这个常数就行了。 更新:由P(xk:k-1到后验概率p(rk1:k)。这个后验概率才是真正有用的东 西,上一步还只是预测,这里又多」k时刻的测量,对上面的预测再进行修正,就是滤波 了。这里的后验概率也将是代入到下次的预测,形成递推。 推 p(arly: k) 趴yk|k,y1:k-1pk|y1:k p(kivi: k-1) P(ykk)p(xk1: k-1) p\ 31: k-1 其中归一化常数 P(Uk:Iy1: k-1)=P(y: k)p(ak: 1y1 k-1)dak 等式第一行到第二行是因为测量方程知道,y(k)只与x()有关,P(kk也称之为似然函 数,由量测方程决定。也和上面的推一样,=h(xk)+1x(部分是常数, P(kxk)也是只和量测噪声n(k)的概率分布有关,注意这个也将为SR粒子滤波里权重的 采样提供编程依据 贝叶斯滤波到这里就告一段落了。但是,请注意上面的推导过程中需要用到积分, 这对于一般的非线性,非高斯系统,很难得到后验慨率的解析解。为了解决这个问题,就 得引进蒙特卡洛采样。关于它的具体推导请见下一篇博文。 (转载请注明作者和出处: 未经允许请勿用于 商业用途) 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粒子滤波:从推导到应用(二) 2014-11-1417:50670人阅读评论(1)收藏编辑时除 粒子滤波采样 、蒙特卡洛采样 假设我们能从一个目标概率分布p(x)中采样到一系列的样本(粒子)1,…N,(至于 怎么生成服从p(x)分布的样本,这个问题先放一放),那么就能利用这些样木去估计这个 分布的某些函数的期望值。譬如: E((a))=of(a)p(a)dx Var(f(x)=E(f(x)-E(f(x)2-J(f()-E(f(x)2p(a)dx 上面的式了其实都是计算期望的问题,只是被积分的函数不同。 蒙特卡洛采样的思想就是用平均值来代替积分,求期望: E(f()) f(x1)+…+f(m 这可以从大数定理的角度去理解它。我们用这种思想去指定不同的f(x)以便达到估计不同 东西的目的。比如:要估计一批同龄人的体重,不分男女,在大样木中男的有100个,女 的有20个,为了少做事,我们按比例抽取10个男的,2个女的,测算这12个人的体重 求平均就完事了。注意这里的按比例抽取,就可以看成从概率分布p(x)中进行抽样。 下面再看一个稍微学术一点的例子: 假设有一粒质地均匀的骰子。规定在一次游戏中,连续四次抛掷骰」,至少出现 次6个点朝上就算赢。现在来估计赢的概率。我们用k来表示在第n次游戏中,第k 次投掷的结果,k=1.4。对于分布均匀的骰子,每次投掷服从均匀分布即 k~(1,6) 这里的区间是取整数,1,2,34,56,代表6个面。由于每次投掷都是独立同分布的,所以 这里的目标分布p也是一个均匀分和x={1,、6y一次游戏就是x空间中的一个随机 为了估计取胜的概率,在第n次游戏中定义一个指示函数 <∑k=1{xk 其中,指示函数Ix}是指,若X的条件满足,则结果为1,不满足结果为0。回到这个问 题,这里函数f0的意义就是单次游戏中,若四次投掷中只要有一个6朝上,f0的结果就 会是1。由此,就可以估计在这样的游戏中取胜的期望,也就是取胜的概率: =E(f(x)≈∑=1(x) 当抽样次数N足够大的时候,上式就逼近真实取胜概率了,看上面这种估计概率的方法, 是通过蒙特卡洛方法的角度去求期望达到估计概率的目的。是不是就跟我们拋使币的例子 梓,抛的次数足够多就可以用来什计正面朝上或反面朝上的概率了。 当然可能有人会问,这样估计的误差有多大,对于这个问题,有兴趣的请去查看我 最下面列出的参考文献2。(哕嗦一句:管的太多太宽,很容易让我们忽略主要问题。博主就是在看 文献过程屮,这个是啥那个是啥,都厶査资料,到头来粒子滤波是干嘛完全不知道了,又重新看资料。个 人感觉有问题还是先放一放,主要思珞理顺了再关注细节。) 接卜来,回到我们的主线上,在滤波中蒙特卡洛又是怎么用的呢? 由上面我们知道,它可以用米估计概率,而在上一节中,贝叶斯后验概率的计算里 要用到积分,为了解决这个积分难的问题,可以用蒙特卡洛采样来代替计算后验概率。 假设可以从后验概率中采样到N个样本,那么后验概率的计算可表示为: (2) .n.1: k )≈p(xnl|yL:k) 其中,在这个蒙特卡洛方法中,我们定义八(x)=6(xn-x1),是秋拉克函数( dirac delta function),跟上面的指示函数意思差不多 看到这里,既然用藜特卡洛方法能够用来直接估计后验概率,现在估计出」后验概 率,那到底怎么用来做图像跟踪或者滤波呢?要做图像跟踪或者滤波,其实就是想知道当 前状态的期望值: Ef(xn)≈∫f(xn)p(xnly1:k)dxn ∑1Jf(x)5(xn-x)dxn ∑1f(x 乜就是用这些采样的粒子的状态值直接平均就得到了期望值,也就是滤波后的值,这里的 f(x)就是每个粒子的状态函数。这就是粒子滤波了,只要从后验概率中采样很多粒子,用 它们的状态求平均就得到了滤波结果。 思路看似简单,但是要命的是,后验概率不知道啊,怎么从后验概率分布中采样!所 以这样直接去应用是行不通的,这时候得引入重要性采样这个方法来解决这个问题 、重要性采样 无法从目标分布中采样,就从一个已知的可以采样的分布里去采样如q(xy),这样 上面的求期望问题就变成了: EIf(k-f(k) P(sky: k gangl: k q(xk y1: k).ck (y1 Ekp(ar) .Ch (y1: k)q(kg 9(2r y1: h ).Th 1-h ∫(x6 Wk(p Th|3/1:k)k 卩(3/1 (2)式 其中 P(g1: k)P(ak) P( ely k(k q(Ch 91:h 9(hl91:h 由于: )(g/1:h (1: k Tkp(k d ck 所以(2)式可以进一步写成: 1 0(y/1) f(CkWk(Tk q(Tk31: k)dIp If(ek Wk(kq(aklyi: k rk ∫p(v:kaxk)p(x)dxk If(an)Wk( r)q( y1: k)d Tk ∫W(xk)q(xk1:h)drh q(k1: R)JQR q(ckly: k)Lh(h (3)式 上面的期望计算都可以通过蒙特卡洛方法来解决它,也就是说,通过采样N个样本 {} glCky1:k ,用样本的平均来求它们的期望,所以上面的(3)式可以近似为 E[f(xk)≈ ∑(x)f(项) z=1 (4)式 其中 Wk(k) 这就是归一化以后的权重,而之前在(2)式中的那个权重是没有归一化的 注意上面的(4)式,它不再是(1)式中所有的粒子状态直接相加求平均了,而是 种加权和的形式。不同的粒子都有它们相应的权重,如果粒子权重大,说明信任该粒子比 较多。 到这里已经解决了不能从后验概率直接采样的问题,但是上面这种每个粒了的权重 都直接计算的方法,效率低,因为每增加一个采样,p(x(k)y(1:k)都得重新计算,并且还 不好计算这个式子。所以求权重时能否避开计算p(x(k)y(1:k)?而最佳的形式是能够以递 摧的方式去计算权重,这就是所谓的序贯重要性采样(SIS),粒子滤波的原型。 下面开始权重W递推形式的推导: 假设重要性概率密度函数k1k),这里x的下标是0:k,也就是说粒子滤波 是估计过去所有时刻的状态的后验。假设它可以分解为 (xak yik=g(xok-I vrk-I )(x k|:k-151 后验概率密度函数的递归形式可以表示为 P( Yup(xklxakYuplroklY) p(klY) pO:IrP(rxlxkprok-lY p(velI P(yk ck-1)p(o:k-1 Yk-1) 其中,为了衣示方便,将y(1:k)用Y(k)来表示,注意Y与y的区别。同时,上面这个 式子和上一节贝叶斯滤波中后验概率的推导是一样的,只是之前的x(k)变成了这里的 x(0:k),就是这个不同,导致贝叶斯估计里需要积分,而这里后验概率的分解形式却不用积 分 粒子权值的递归形式可以表示为 1"∞ 0:1t 0. A p(kIxk)p(rk lx-plrorlYx_) (1) q(xk I ) Y2)g(x011-) p( leo)p(xg xo) ()|;()w (5)式 注意,这种权重递摧形式的推导是在前面(2)式的形式下进行推导的,也就是没有归 ∑iaxk)f(x 化。而在进行状态估计的公式为=1 这个公式中的的权重是归一化以 后的,所以在实际应用中,递推计算出w(k)后,要进行归一化,才能够代入(4)式中去计算 期望。同时,上面(5)式中的分子是不是很熟悉,在上一节贝叶斯滤波中我们都已经做了介 绍,p(y×),p(x(k)×(k-1)的形状实际上和状态方程中噪声的桃率分布形状是一样的,只 是均值不同了。因此这个递推的(5)式和前面的非递推形式相比,公式里的概率都是已知 的,权重的计算可以说没有编程方面的难度了。权重也有了以后,只要进行稍微的总结就 可以得到 SIS Filter yy, Sequential Importance Sampling(SIS)Filter 在实际应用中我们可以假设重要性分布q0满足 q(ek o: k-1, 91: k)=q(ck Ck-1, 3k 这个假设说明重要性分布只和前时刻的状态X(k-1)以及测量y(k)有关了,那么(5)式就可 以转化为: () p(yk= )p(ap) g(x1x21,孙k) 在做了这么多假设和为了解决个个问题以后,终于有了个像样的粒了滤波算法了,他 就是序贯重要性采样滤波。 下面用伪代码的形式给出这个算法: pseudo code--- [{x2,12)21]=SYS({x,12)}x2F) For i=1: N (1)米样,x((()1- k|k-1 p(yk b ) p(= I tk∝tk-1 (2)根据 q(xkxk-1,yk)递推计算各个粒子的 权重 End for 粒子权值归一化。粒子有了,粒子的权重有了,就可以由(4)式,对每个粒子的 状态进行加权去估计目标的状态了。 -end 这个算法就是粒子滤波的前身了。只是在实际应用中,又发现了很多问题,如粒子权 重退化的问题,因此就有了重样( resample),就有了基本的粒子滤波算法。还有就是 亘要性概率密度q0的选择问题,等等。都留到下一章去解决。 在这一章中,我们是用的重要性采样这种方法去解决的后验概率无法采样的问题。实 际上,关于如何从后验概率采样,也就是如何生成特定概率密度的样本,有很多经典的方 法(如拒绝采样, 算法,