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

粒子滤波及组合模式内容概要•1组合导航中的状态估计问题(为什么要用粒子滤波方法?)•2粒子滤波方法(什么是粒子滤波方法？)•3基于粒子滤波的组合模式(怎么用粒子滤波方法？)1.组合导航中的状态估计问题•线性，高斯噪声•非线性，高斯噪声•非线性，非高斯噪声•卡尔曼滤波(KalmanFilter，KF)(最优线性滤波)•扩展卡尔曼滤波(ExtendedKalmanFilter,EKF)；无迹卡尔曼滤波(UncentedKalmanFilter,UKF)•最优贝叶斯滤波(BayesFilter)粒子滤波(ParticleFilter)(基于蒙特卡洛仿真的递归贝叶斯滤波)系统模型滤波方法组合导航中的状态估计问题组合导航实质为通过信息融合方式，从不同导航信息源通过滤波方法对于载体状态进行最优估计，以提高导航精度和可靠性。2.粒子滤波方法•一、预备知识•二、最优贝叶斯滤波•三、蒙特卡洛仿真•四、粒子滤波一、预备知识状态方程:p(xk|xk-1)(状态转移概率密度)观测方程:p(zk|xk)(观测值似然概率密度)状态估计：在已知状态方程(状态转移概率密度)、观测方程(观测值似然概率密度)及观测向量基础上，推算状态向量后验概率密度，进而对状态向量进行估计。2kz1kzkz)|(kkpxz1.状态估计与概率分布模型2.几种概率密度意义•p(xk|xk-1)状态转移概率密度–状态模型：已知船舶前一位置为xk-1，则船舶当前位置为xk的概率密度。•p(zk|xk)观测值似然概率密度–观测模型：已知船舶当前位置为xk，其观测值zk的似然概率密度。•p(xk|z1:k)后验概率密度–已知对于船舶位置的观测值序列z1:k，则船舶当前真实位置xk的概率密度。以对船舶位置的估计为例：3.概率论预备知识dBBApAp),()()|()|(),|(),|(CBpCApCABpCBAp•贝叶斯理论•联合概率密度•联合概率密度积分)|(),|()|(),|()|,(CApCABpCBpCBApCBAp②①③4.系统模型假设kjjjkppp110:0)|()()(xxxxkjjjkkpp1:0:1)|()|(xzxz假设1:状态量为一阶随机马尔科夫过程，状态向量仅与前一状态向量相关，即假设2：观测量仅与当前状态量相关，即A1A2二、最优贝叶斯滤波•目标为基于所能获得的观测量z1:k迭代估计未知状态量xk的后验概率密度p(xk|z1:k)。•最优贝叶斯滤波方法可分为四步：步骤1.状态向量概率密度预测步骤2.观测向量概率密度预测步骤3.状态向量后验概率密度计算步骤4.状态向量估计步骤1：假定在k-1时刻已经获得，那么状态向量预测概率密度函数可推导如下)|(1:11kkpzx11:11111:111:1111:111:1)|()|()|(),|()|,()|(kkkkkkkkkkkkkkkkkdppdppdppxzxxxxzxzxxxzxxzx③A1①步骤2：观测向量预测概率密度推导kkkkkkkkkkkkkkkkkdppdppdppxzxxzxzxzxzxzxzzz)|()|()|(),|()|,()|(1:11:11:11:11:1③A2①步骤3：状态向量后验概率密度推导)|()|()|()|()|(),|()|()|,(),|()|(1:11:11:11:11:11:11:11:1:1kkkkkkkkkkkkkkkkkkkkkkkppppppppppzzzxxzzzzxzxzzzzzxzzxzx②①A2步骤4：状态向量估计kkkkkkkdpExzxxzxx)|(}|{ˆ:1:1最优贝叶斯滤波方法实际应用中存在的问题1.对于任意概率密度积分往往不存在解析解。2.即使对于存在解析解的情况，高维积分也导致其计算非常困难。p(x0)PredictionUpdatePredictionPredictionUpdateUpdate…p(x0|z0)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:0kkxxxNikkkdNpik1:0:1:0)(1)|(:0xzxxik:0x如下图所示，某高斯分布可由计算机仿真所获得随机数的柱状图近似，即某点的概率密度可由落在其某一领域内点的个数来近似。则对于序列的任意函数的概率分布可由相应粒子函数值的柱状图近似，其期望值则可由粒子函数值的平均获得。即对于期望值k:0xkkkkkdpggE:0:1:0:0:0)|()())((xzxxxNiikkgNgE1:0:0)(1))((xx)(:0kgx可被计算如下当粒子数量达到一定程度时，可以非常接近的近似真实概率分布，因此对于已知概率密度的变量，当其任意函数的概率密度分布数学分析无法进行时，蒙特卡洛仿真可以获得其估计，从而避免解析求解。•取p(x)=Gamma(4,1)，生成随机粒子，取较小的领域，画出其柱状图，可以看出，柱状图非常接近真实概率分布。0246810121416182000.050.10.150.20.250.30.350.4020406080100120140160180200024681012200采样点0246810121416182000.050.10.150.20.250.30.350.40.450246810121416182000.050.10.150.20.250.30.35500采样点1000采样点051015202500.050.10.150.20.25051015202500.050.10.150.20.25200000采样点5000采样点四、粒子滤波方法实际应用中无法从未知的后验概率密度获得粒子。因此往往从一个已知的、简单的概率密度获得采样粒子。该概率密度函数称为重要性概率密度函数(ImportanceDensityFunctionDistribution)，采样过程称为重要性采样(ImportanceSampling)，采样而产生的样本称为采样粒子(SamplingParticle)，粒子滤波迭代过程中的采样称为序贯重要性函数采样。1.序贯重要性函数采样设此重要性函数用表示，则有)|(:1:0kkqzxkkkkkkkkkkkkkkkkkkkkkkkkkkdqpwgdqqpppgdqqpggE: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)|()())((xzxxxNiikkgNgE1:0:0)(1))((xx重要性函数采样kkkkkkkkdqpwggE:0:1:0:1:0:0:0)|()()()())((xzxzxxxNikikkikkpwggE1:1:0:0:0)()()())((zxxx)(~:0ikkxw则计算状态函数期望时，问题变成了对于权值和概率密度的求取。则)(:1kpzkkkkkkkkkkkkkkkkkdqwdxqqppdpp: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:0:0:0)|()()|())()(()|()()()())((xzxxxzxxxxzxzxxx③①NiikkikNiikkNiikkikkwgwNwgNgE1:0:01:01:0:0:0)(~)()(1)()(1))((xxxxxx其中为归一化粒子权值，且粒子由重要性函数产生。因此Niikkikkikk:0:0)()()(~xxx问题转化为：如何迭代获取粒子权值？2.粒子权值迭代)|()|()|()()|()|()|()|()()|()|()|()()|()|()|()()|()(:1,111:01:1,1:11:011:01:01:1:1:011:01:01:1:1:0:0:0:1:0kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkqppwqqppppqppppqppwzxxxxxzxzxxzxxxxzxxzzxxxxxzxzzxxxzxA1,A2①)|(),|(1:11:0kkkkkpqxxzxx如果我们选取状态转移概率密度函数为重要性函数，即则粒子权值迭代如下)|(1kkkkpwwxz此时后验概率密度可被近似如下Niikkkikwp1:1)|(xzx状态估计值如下Niikikkw1xx粒子权值退化现象(ParticleWeightDegeneration)x后验概率密度在序贯重要性采样方法中，存在着粒子权值退化现象。即经过若干次迭代后，除一个粒子外，其余的粒子只有微小的权值，可忽略不计。退化现象意味着大量的计算工作都被用来更新那些对求解后验概率密度几乎不起任何作用的粒子上，导致系统估计方差变大，性能下降。粒子退化定量描述•对粒子滤波算法粒子退化的一个量度为有效样本容量，定义如下NiikeffwN12)(1其中，而很小的就意味着严重退化。NNeffeffN粒子重采样法(ParticleResampling)Nxik1,~1Nxik1,~1重采样算法是降低粒子权值退化现象的另一种方法，其思想是通过对粒子和相应权值表示的概率密度函数重新采样，增加权值较大的粒子数ikikwx,~Nxik1,~1ikikwx11,~Nxik1,1Nxik1,~序贯重要性采样粒子滤波(SequentialImportanceSampling,SIS)InitializeparticlesOutputOutputestimates12M...ParticlegenerationNewobservationExitNormalizeweights12M...WeightcomputationResamplingMoreobservations?yesnoDegeraration？基于优选重要性密度函数的粒子滤波方法•基本思想：使重要性密度函数尽可能接近最优概率密度函数，从而解决粒子退化问题。•可以证明：粒子滤波器最优概率密度函数为状态向量的后验概率密度函数。•解决方法：取后验概率密度函数的高斯近似作为重要性密度函数1.EKF粒子滤波器2.UKF粒子滤波器3.高斯混合粒子滤波3基于粒子滤波的组合模式