粒子滤波及组合模式 张勇刚

1、粒子滤波及组合模式内容概要 1 组合导航中的状态估计问题 (为什么要用粒子滤波方法?) 2 粒子滤波方法 (什么是粒子滤波方法？) 3 基于粒子滤波的组合模式 (怎么用粒子滤波方法？)1. 组合导航中的状态估计问题 线性，高斯噪声 非线性，高斯噪声 非线性，非高斯噪声 卡尔曼滤波(Kalman Filter，KF)(最优线性滤波) 扩展卡尔曼滤波(Extended Kalman Filter,EKF)；无迹卡尔曼滤波(Uncented Kalman Filter,UKF) 最优贝叶斯滤波(Bayes Filter) 粒子滤波(Particle Filter) (基于蒙特卡洛仿真的递归贝叶斯滤波

2、)系统模型系统模型滤波方法滤波方法组合导航中的状态估计问题 组合导航实质为通过信息融合方式，从不同导航信息源通过滤波方法对于载体状态进行最优估计，以提高导航精度和可靠性。2. 粒子滤波方法 一、预备知识 二、最优贝叶斯滤波 三、蒙特卡洛仿真 四、粒子滤波一、预备知识 状态方程: p(xk|xk-1) (状态转移概率密度) 观测方程: p(zk|xk) (观测值似然概率密度) 状态估计：在已知状态方程(状态转移概率密度) 、观测方程(观测值似然概率密度)及观测向量基础上，推算状态向量后验概率密度，进而对状态向量进行估计。2kz1kzkz)|(kkpxz1. 1. 状态估计与概率分布模型状态估计与

3、概率分布模型2. 2. 几种概率密度意义几种概率密度意义 p(x xk|x xk-1)状态转移概率密度状态模型：已知船舶前一位置为x xk-1，则船舶当前位置为x xk的概率密度。 p(z zk|x xk)观测值似然概率密度观测模型：已知船舶当前位置为x xk，其观测值z zk的似然概率密度。 p(x xk|z z1:k) 后验概率密度已知对于船舶位置的观测值序列z z1:k，则船舶当前真实位置x xk的概率密度。以对船舶位置的估计为例： 3. 3. 概率论预备知识概率论预备知识dBBApAp),()()|()|(),|(),|(CBpCApCABpCBAp贝叶斯理论联合概率密度联合概率密度积

4、分)|(),|()|(),|()|,(CApCABpCBpCBApCBAp 4. 4. 系统模型假设系统模型假设kjjjkppp110: 0)|()()(xxxxkjjjkkpp1: 0: 1)|()|(xzxz假设1: 状态量为一阶随机马尔科夫过程，状态向量仅与前一状态向量相关，即假设2：观测量仅与当前状态量相关，即A1A2二、最优贝叶斯滤波 目标为基于所能获得的观测量z z1:k迭代估计未知状态量 x xk的后验概率密度 p(x xk|z z1:k)。 最优贝叶斯滤波方法可分为四步：步骤1.状态向量概率密度预测步骤2.观测向量概率密度预测步骤3.状态向量后验概率密度计算 步骤4.状态向量估

5、计 步骤步骤1 1：假定在k-1时刻已经获得 ，那么状态向量预测概率密度函数可推导如下)|(1: 11kkpzx11: 11111: 111: 1111: 111: 1 )|()|( )|(),|( )|,()|(kkkkkkkkkkkkkkkkkdppdppdppxzxxxxzxzxxxzxxzx A1 步骤步骤2 2：观测向量预测概率密度推导kkkkkkkkkkkkkkkkkdppdppdppxzxxzxzxzxzxzxzzz )|()|( )|(),|( )|,()|(1: 11: 11: 11: 11: 1 A2 步骤步骤3 3：状态向量后验概率密度推导)|()|()|()|()|()

6、,|()|()|,(),|()|(1: 11: 11: 11: 11: 11: 11: 11: 1: 1kkkkkkkkkkkkkkkkkkkkkkkppppppppppzzzxxzzzzxzxzzzzzxzzxzx A2步骤步骤4 4：状态向量估计kkkkkkkdpExzxxzxx)|(|: 1: 1最优贝叶斯滤波方法实际应用中存在的问题最优贝叶斯滤波方法实际应用中存在的问题1.对于任意概率密度积分往往不存在解析解。2.即使对于存在解析解的情况，高维积分也导致其计算非常困难。p(x0)PredictionUpdatePredictionPredictionUpdateUpdatep(x0|z

7、0)p(x1|z1)p(x1|z0)p(x2|z1)p(xk|zk-1)p(xk|zk)p(xk+1|zk)z0z1zkp(z1|z0)p(z2|z1)p(zk|zk-1)三、蒙特卡洛仿真 非线性、非高斯系统状态的后验概率密度p(xk|z1:k)的估计不存在解析解，但运用蒙特卡洛仿真，后验概率密度可以被N个带有权值的抽样点所近似。粒子滤波方法就是一种基于蒙特卡洛方法的递归状态贝叶斯估计方法。 对于状态序列 其联合后验概率密度可由一系列的从后验概率密度分布产生获得样本抽样 表示，该样本抽样被称为粒子),(0: 0kkxxxNikkkdNpik1: 0: 1: 0)(1)|(: 0 xzxxik

8、: 0 x如下图所示，某高斯分布可由计算机仿真所获得随机数的柱状图近似，即某点的概率密度可由落在其某一领域内点的个数来近似。 则对于序列 的任意函数 的概率分布可由相应粒子函数值的柱状图近似，其期望值则可由粒子函数值的平均获得。即对于期望值k : 0 xkkkkkdpggE: 0: 1: 0: 0: 0 )|()()(xzxxxNiikkgNgE1: 0: 0)(1)(xx)(: 0 kg x 可被计算如下 当粒子数量达到一定程度时，可以非常接近的近似真实概率分布，因此对于已知概率密度的变量，当其任意函数的概率密度分布数学分析无法进行时，蒙特卡洛仿真可以获得其估计，从而避免解析求解。 取 p(

9、x)=Gamma(4,1)，生成随机粒子，取较小的领域，画出其柱状图，可以看出，柱状图非常接近真实概率分布。0246810121416182000.050.4020406080100120140160180200024681012200 采样点0246810121416182000.050.40.450246810121416182000.05500 采样点1000 采样点051015202500.00.25051015202500.050.10.

10、150.20.25200000 采样点5000 采样点四、粒子滤波方法 实际应用中无法从未知的后验概率密度获得粒子。因此往往从一个已知的、简单的概率密度获得采样粒子。该概率密度函数称为重要性概率密度函数(Importance Density Function Distribution)，采样过程称为重要性采样(Importance Sampling)，采样而产生的样本称为采样粒子(Sampling Particle)，粒子滤波迭代过程中的采样称为序贯重要性函数采样。1. 1. 序贯重要性函数采样序贯重要性函数采样设此重要性函数用 表示，则有)|(: 1: 0kkqzxkkkkkkkkkkkkk

11、kkkkkkkkkkkkkdqpwgdqqpppgdqqpggE: 0: 1: 0: 1: 0: 0: 0: 1: 0: 1: 0: 1: 0: 0: 1: 0: 0: 1: 0: 1: 0: 1: 0: 0: 0 )|()()()( )|()|()()()|()( )|()|()|()()(xzxzxxxzxzxzxxzxxzxzxzxxx 后验概后验概率采样率采样kkkkkdpggE: 0: 1: 0: 0: 0 )|()()(xzxxxNiikkgNgE1: 0: 0)(1)(xx重要性函重要性函数采样数采样kkkkkkkkdqpwggE: 0: 1: 0: 1: 0: 0: 0 )|(

12、)()()()(xzxzxxxNikikkikkpwggE1: 1: 0: 0: 0)()()()(zxxx)(: 0ikkxw则 计算状态函数期望时，问题变成了对于权值 和概率密度 的求取。则)(: 1kp zkkkkkkkkkkkkkkkkkdqwdxqqppdpp: 0: 1: 0: 0: 0: 1: 0: 1: 0: 0: 0: 1: 0: 0: 1: 1 )|()( )|()|()()|( ),()(xzxxxzzxxxzxxzz因为因此kkkkkkkkkkkkkkkkkkkdqwdqwgdqpwggE: 0: 1: 0: 0: 0: 1: 0: 0: 0: 0: 1: 0: 1:

13、0: 0: 0 )|()( )|()()( )|()()()()(xzxxxzxxxxzxzxxx NiikkikNiikkNiikkikkwgwNwgNgE1: 0: 01: 01: 0: 0: 0)()()(1)()(1)(xxxxxx其中 为归一化粒子权值，且粒子由重要性函数产生。因此Niikkikkikkwww1: 0: 0: 0)()()(xxx问题转化为：如何迭代获取粒子权值？2. 2. 粒子权值迭代粒子权值迭代)|()|()|()( )|()|()|()|()()|()|()|()()|()|()|()()|()(: 1, 111: 01: 1, 1: 11: 011: 01:

14、01: 1: 1: 011: 01: 01: 1: 1: 0: 0: 0: 1: 0kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkqppwqqppppqppppqppwzxxxxxzxzxxzxxxxzxxzzxxxxxzxzzxxxzxA1,A2 )|(),|(1: 11: 0kkkkkpqxxzxx如果我们选取状态转移概率密度函数为重要性函数，即则粒子权值迭代如下)|(1kkkkpwwxz 此时后验概率密度可被近似如下Niikkkikwp1: 1)|(xzx状态估计值如下Niikikkw1xx粒子权值退化现象(Particle Weight Degenerati

15、on)x后验概率密度在序贯重要性采样方法中，存在着粒子权值退化现象。即经过若干次迭代后，除一个粒子外，其余的粒子只有微小的权值，可忽略不计。退化现象意味着大量的计算工作都被用来更新那些对求解后验概率密度几乎不起任何作用的粒子上，导致系统估计方差变大，性能下降。粒子退化定量描述 对粒子滤波算法粒子退化的一个量度为有效样本容有效样本容量量，定义如下NiikeffwN12)(1其中 ，而很小的 就意味着严重退化。NNeffeffN粒子重采样法(Particle Resampling)Nxik1,1Nxik1,1重采样算法是降低粒子权值退化现象的另一种方法，其思想是通过对粒子和相应权值表示的概率密度函数重新采样，增加权值较大的粒子数ikikwx ,Nxik1,1ikikwx11,Nxik1,1Nxik1,序贯重要性采样粒子滤波(Sequential Importance Sampling,SIS)Initialize particlesOutputOutput estimates12M. . .ParticlegenerationNew observationExitNormalize