粒子滤波及matlab实现.docx

1、粒子滤波及matlab实现粒子滤波及 matlab 实现粒子滤波就是指： 通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函 数，用样本均值代替积分运算， 进而获得系统状态的最小方差估计的过程， 这些样本被形象 的称为“粒子” ，故而叫粒子滤波。粒子滤波通过非参数化的蒙特卡洛 (Monte Carlo) 模拟方 法来实现递推贝叶斯滤波， 适用于任何能用状态空间模型描述的非线性系统， 精度可以逼近 最优估计。 粒子滤波器具有简单、 易于实现等特点， 它为分析非线性动态系统提供了一种有 效的解决方法，从而引起目标跟踪、信号处理以及自动控制等领域的广泛关注。贝叶斯滤波动态系统的目标跟踪问题

2、可以通过下图所示的状态空间模型来描述。在目标跟踪问题中，动态系统的状态空间模型可描述为xk f(xk 1) uk 1 yk h(xk ) vk其中 f ( ), h( )分别为状态转移方程与观测方程， xk 为系统状态， yk 为观测值， uk为过程噪声， vk 为观测噪声。为了描述方便，用 Xk x0:k x0,x1, ,xk 与Yk y1:k y1, , yk分别表示 0到 k时刻所有的状态与观测值。在处理目标跟踪问题时， 通常假设目标的状态转移过程服从一阶马尔可夫模型，即当前时刻的状态 xk 只与上一时刻 的状态 xk-1有关。另外一个假设为观测值相互独立， 即观测值 yk只与 k时刻的

3、状态 xk有关。 贝叶斯滤波为非线性系统的状态估计问题提供了一种基于概率分布形式的解决方案。 贝 叶斯滤波将状态估计视为一个概率推理过程， 即将目标状态的估计问题转换为利用贝叶斯公 式求解后验概率密度 p(Xk |Yk )或滤波概率密度 p(xk |Yk ) ，进而获得目标状态的最优估计。 贝叶斯滤波包含预测和更新两个阶段， 预测过程利用系统模型预测状态的先验概率密度， 新过程则利用最新的测量值对先验概率密度进行修正，得到后验概率密度。假设已知 k 1时刻的概率密度函数为 p(xk 1|Yk 1 ) ，贝叶斯滤波的具体过程如下：(1)预测过程，由 p(xk 1|Yk 1) 得到 p(xk |Y

4、k 1)：p( xk, xk 1|Yk 1) p(xk | xk 1,Yk 1)p(xk 1|Yk 1)当给定 xk 1时，状态 xk 与 Yk 1相互独立，因此p(xk,xk 1|Yk 1) p(xk|xk 1)p(xk1|Yk 1)上式两端对 xk 1 积分，可得 Chapman-Komolgorov 方程p(xk|Yk 1) p(xk|xk 1)p(xk 1|Yk 1)dxk 1(2)更新过程，由 p(xk |Yk 1)得到 p(xk |Yk )：获取 k时刻的测量 yk 后，利用贝叶斯公式对先验概率密度进行更新，得到后验概率p(xk |Yk)p(yk |xk,Yk 1)p(xk |Yk

5、 1)p( yk |Yk 1)假设 yk 只由 xk 决定，即p(yk|xk,Yk 1) p(yk|xk)因此p(xk |Yk )p(yk |xk)p(xk |Yk 1)p(yk|Yk 1)其中， p(yk |Yk 1) 为归一化常数p(yk |Yk 1) p(yk |xk )p(xk |Yk 1)dxk贝叶斯滤波以递推的形式给出后验 (或滤波 ) 概率密度函数的最优解。 目标状态的最优估 计值可由后验 (或滤波)概率密度函数进行计算。 通常根据极大后验 (MAP) 准则或最小均方误 差(MMSE) 准则，将具有极大后验概率密度的状态或条件均值作为系统状态的估计值，即 x?kM A P =ar

6、g min p(xk |Yk) x?kM M S E =Ef(xk)|Yk f (xk)p(xk |Yk)dxk贝叶斯滤波需要进行积分运算， 除了一些特殊的系统模型 (如线性高斯系统， 有限状态 的离散系统)之外， 对于一般的非线性、 非高斯系统，贝叶斯滤波很难得到后验概率的封闭 解析式。 因此， 现有的非线性滤波器多采用近似的计算方法解决积分问题， 以此来获取估计的次优解。 在系统的非线性模型可由在当前状态展开的线性模型有限近似的前提下， 基于一 阶或二阶 Taylor 级数展开的扩展 Kalman滤波得到广泛应用。在一般情况下，逼近概率密度函 数比逼近非线性函数容易实现。据此， Julie

7、r与Uhlmann 提出一种 UnscentedKalman滤波器， 通过选定的 sigma点来精确估计随机变量经非线性变换后的均值和方差，从而更好的近似状 态的概率密度函数，其理论估计精度优于扩展 Kalman 滤波。获取次优解的另外一中方案便 是基于蒙特卡洛模拟的粒子滤波器。粒子滤波早在 20 世纪 50 年代， Hammersley 便采用基于序贯重要性采样 (Sequential importance sampling,SIS)的蒙特卡洛方法解决统计学问题 121。20 世纪 60年代后期， Handschin 与 Mayne 使用序贯蒙特卡洛方法解决自动控制领域的相关问题。 20 世

8、纪 70 年代， Handschin 、Akashi以及 Zaritskii 等学者的一系列研究工作使得序贯蒙特卡洛方法得到进一步发展。 限于当时的 计算能力以及算法本身存在的权值退化问题， 序贯重要性采样算法没有受到足够重视， 在随 后较长一段时间内进展较为缓慢。 直到 20世纪 80 年代末， 计算机处理能力的巨大进展使得 序贯蒙特卡洛方法重新受到关注。 Tanizaki 、Geweke 等采用基于重要性采样的蒙特卡洛方法 成功解决了一系列高维积分问题。 Smith 与 Gelfand 提出的采样 - 重采样思想为 Bayesian 推理 提供了一种易于实现的计算策略。随后， Smith

9、与 Gordon 等人合作，于 20 世纪 90 年代初 将重采样 (Resampling) 步骤引入到粒子滤波中，在一定程度上解决了序贯重要性采样的权值 退化问题，并由此产生了第一个可实现的 SIR(Sampling importance resampling) 粒子滤波算法 (Bootstrap 滤波 )，从而掀起粒子滤波的研究热潮。美国海军集成水下监控系统中的 Nodestar便是粒子滤波应用的一个实例。进入 21 世纪，粒子滤波器成为一个非常活跃的研究领域， Doucet 、 Liu 、Arulampalam 等对粒子滤波的研究作了精彩的总结， IEEE 出版的论文集“ Sequent

10、ial Monte Carlo Methods in Practice 对粒子滤波器”进行了详细介绍。贝叶斯重要性采样蒙特卡洛模拟是一种利用随机数求解物理和数学问题的计算方法， 又称为计算机随机模 拟方法。 该方法源于第一次世界大战期间美国研制原子弹的曼哈顿计划， 著名数学家冯诺伊 曼作为该计划的主持人之一， 用驰名世界的赌城， 摩纳哥的蒙特卡洛来命名这种方法。 蒙特 卡洛模拟方法利用所求状态空间中大量的样本点来近似逼近待估计变量的后验概率分布， 如 图 2.2 所示，从而将积分问题转换为有限样本点的求和问题。粒子滤波算法的核心思想便是 利用一系列随机样本的加权和表示后验概率密度， 通过求和来

11、近似积分操作。 假设可以从后 验概率密度 p(xk |Yk )中抽取 N 个独立同分布的随机样本 xk(i) ，i 1, ,N ，则有1 N (i)p(xk |Yk) N1 i 1 (xk xk(i) (0.1)这里 xk为连续变量， (x-xk )为单位冲激函数 (狄拉克函数 )，即 (x-xk) 0,x xk ，且 (x)dx 1。当 xk 为离散变量时，后验概率分布 P(xk |Yk ) 可近似逼近为1 N (i) P(xk |Yk) (xk xk(i)(0.2)Ni1其中， (xk xk(i) 1,xk xk(i); (xk xk(i) 0,xk xk(i)。图 2.1 经验概率分布函

12、数Fig. 2.1 Empirical probability distribution function设 xk(i)为从后验概率密度函数 p(xk | Yk )中获取的采样粒子，则任意函数 f (xk)的期望估计可以用求和方式逼近，即1 N (i)E f (xk )|Yk f (xk)p(xk | Yk )dxk 1 f (xk(i) ) (0.3)Ni1 蒙特卡洛方法一般可以归纳为以下三个步骤：(1)构造概率模型。对于本身具有随机性质的问题，主要工作是正确地描述和模拟这个概 率过程。对于确定性问题，比如计算定积分、求解线性方程组、偏微分方程等问题，采 用蒙特卡洛方法求解需要事先构造一个人

13、为的概率过程， 将它的某些参量视为问题的解。(2)从指定概率分布中采样。产生服从己知概率分布的随机变量是实现蒙特卡洛方法模拟 试验的关键步骤。(3)建立各种估计量的估计。一般说来，构造出概率模型并能从中抽样后，便可进行现模 拟试验。随后，就要确定一个随机变量，将其作为待求解问题的解进行估计。在实际计算中， 通常无法直接从后验概率分布中采样， 如何得到服从后验概率分布的随 机样本是蒙特卡洛方法中基本的问题之一。 重要性采样法引入一个已知的、 容易采样的重要 性概率密度函数 q(xk |Yk ) ，从中生成采样粒子，利用这些随机样本的加权和来逼近后验滤 波概率密度 p(xk |Yk ) ，如图 2

14、.3所示。令 xk(i) ,wk(i ),i 1, .N表示一支撑点集， 其中 xk(i)为 是 k 时刻第 i 个粒子的状态，其相应的权值为 w(ki) ，则后验滤波概率密度可以表示为Np(xk |Yk) wk(i) (xk xk(i) (0.4)i1其中，p(xk(i) |Yk) q(xk(i) |Yk)1NN1 i 1 f(xk(i)wk(i)(0.6)序贯重要性采样算法在基于重要性采样的蒙特卡洛模拟方法中， 估计后验滤波概率需要利用所有的观测数据， 每次新的观测数据来到都需要重新计算整个状态序列的重要性权值。 序贯重要性采样作为粒 子滤波的基础， 它将统计学中的序贯分析方法应用到的蒙特

15、卡洛方法中， 从而实现后验滤波 概率密度的递推估计。假设重要性概率密度函数 q( x0:k |y1:k )可以分解为q(x0:k |y1:k) q(x0:k 1|y1:k 1)q(xk|x0:k 1, y1:k ) (0.7)设系统状态是一个马尔可夫过程，且给定系统状态下各次观测独立，则有kp(x0:k) p(x0) p(xi |xi 1 ) (0.8)kp( y1:k | x1:k ) i 1p(yi | xi ) (0.9)粒子权值 wk(i) 的递归形式可以表示为通常，需要对粒子权值进行归一化处理，即序贯重要性采样算法从重要性概率密度函数中生成采样粒子， 并随着测量值的依次到来 递推求得

16、相应的权值， 最终以粒子加权和的形式来描述后验滤波概率密度， 进而得到状态估 计。序贯重要性采样算法的流程可以用如下伪代码描述：xk(i),wk(i)iN1 SIS(xk(i)1,wk(i)1iN1,Yk)For i=1:N(1)时间更新，根据重要性参考函数 q(xk(i) | x0(:ik) 1,Yk )生成采样粒子 xk(i)；(2)量测更新，根据最新观测值计算粒子权值 wk(i) ；End For粒子权值归一化，并计算目标状态。为了得到正确的状态估计， 通常希望粒子权值的方差尽可能趋近于零。 然而， 序贯蒙特卡洛模拟方法一般都存在权值退化问题。 在实际计算中， 经过数次迭代， 只有少数粒

17、子的权 值较大， 其余粒子的权值可忽略不计。 粒子权值的方差随着时间增大， 状态空间中的有效粒 子数较少。 随着无效采样粒子数目的增加， 使得大量的计算浪费在对估计后验滤波概率分布 几乎不起作用的粒子更新上，使得估计性能下降。通常采用有效粒子数 Neff 来衡量粒子权值的退化程度，即Neff N /(1 var(w*k( i) ) (0.13)w*k(i) q(xp(i)x|kx(|i)y,1:ky) ) (0.14) q(xk |xk 1,y1:k)有效粒子数越小，表明权值退化越严重。在实际计算中，有效粒子数 Neff 可以近似为N?eff N 1 (0.15)(wk(i)2i1在进行序贯重

18、要性采样时，若 N?eff 小于事先设定的某一阈值，则应当采取一些措施加 以控制。 克服序贯重要性采样算法权值退化现象最直接的方法是增加粒子数， 而这会造成计 算量的相应增加，影响计算的实时性。因此，一般采用以下两种途径： (1)选择合适的重要性概率密度函数； (2)在序贯重要性采样之后，采用重采样方法。重要密度函数的选择重要性概率密度函数的选择对粒子滤波的性能有很大影响， 在设计与实现粒子滤波器的 过程中十分重要。在工程应用中，通常选取状态变量的转移概率密度函数 p(xk | xk 1)作为 重要性概率密度函数。此时，粒子的权值为wk(i) wk(i )1 p( yk |xk(i)(0.16

19、) 转移概率的形式简单且易于实现， 在观测精度不高的场合， 将其作为重要性概率密度函 数可以取得较好的滤波效果。 然而， 采用转移概率密度函数作为重要性概率密度函数没有考 虑最新观测数据所提供的信息， 从中抽取的样本与真实后验分布产生的样本存在一定的偏差， 特别是当观测模型具有较高的精度或预测先验与似然函数之间重叠部分较少时， 这种偏差尤 为明显。选择重要性概率密度函数的一个标准是使得粒子权值 wk(i)iN1的方差最小。 Doucet等给出的最优重要性概率密度函数为q(xk(i)| xk(i)1,yk) p(xk(i)|xk(i)1,yk)p(yk |xk(i),xk(i)1)p(xk(i)

20、|xk(i)1)p(yk | xk(i)1)p( yk | xk(i) p(x(ki) |xk(i)1)(0.17) (i ) (0.17) p(yk | xk(i )1)此时，粒子的权值为wk(i) wk(i )1 p( yk | x(ki)1) (0.18)以 p(xk(i) | x(ki)1, yk)作为重要性概率密度函数需要对其直接采样。此外，只有在 xk为有限离散状态或 p( xk( i ) | xk(i)1, yk )为高斯函数时， p(yk | xk(i)1)才存在解析解。在实际情况中， 构造最优重要性概率密度函数的困难程度与直接从后验概率分布中抽取样本的困难程度等 同。从最优重

21、要性概率密度函数的表达形式来看， 产生下一个预测粒子依赖于已有的粒子和 最新的观测数据， 这对于设计重要性概率密度函数具有重要的指导作用， 即应该有效利用最 新的观测信息， 在易于采样实现的基础上， 将更多的粒子移动到似然函数值较高的区域， 如 图 2.4 所示。图 2.3 移动粒子至高似然区域Fig. 2.3 Move the samples in the prior to regions of high likelihood辅助粒子滤波算法利用 k 时刻的信息，将 k 1时刻最有前途 (预测似然度大 )的粒子扩展 到k 时刻137 ，从而生成采样粒子。与 SIR滤波器相比，当粒子的似然函数

22、位于先验分布的 尾部或似然函数形状比较狭窄时， 辅助粒子滤波能够得到更精确的估计结果。 辅助粒子滤波 引入辅助变量 m 来表示 k 1时刻的粒子列表，应用贝叶斯定理，联合概率密度函数 p( xk , m | y1:k )可以描述为p(xk,m|y1:k) p(yk | xk ) p(xk , m | y1:k 1)p( yk | xk ) p(xt | m, y1:k 1)p(m| y1:k 1)p(yk |xkm)p(xk | xkm 1)wkm1 (0.19)生成 xk(i),m(i)iN1的重要性概率密度函数 q( xk ,m | x0:k 1,y1:k)为q(xk,m|x0:k 1,y

23、1:k) p(yk| km)p(xk | xkm 1) wkm1 (0.20)其中 km为由xk(i)1iN1预测出的与 xk相关的特征， 可以是采样值 km p(xk | xkm 1 )或预 测均值 km Exk | xkm 1 。定义 q(xk | m, y1:k) p(xk | xkm 1) ，由于q(xk ,m| y1:k ) q(xk |m, y1:k)q(m| y1:k ) (0.21)则有q(m | y1:k ) p(yk | km)wkm1(0.22) 此时，粒子权值 wk(i) 为w(i) wm(i) p( yk | xk(i ) ) p(xik |xkm ) p(yk |x

24、k(i) (0.23) wk wk m m(i ) (0.23)q( xk ,m | x0m:k 1,yk) p(yk | km )采用局部线性化的方法来逼近 p(xk | xk 1, yk )是另一种提高粒子采样效率的有效方法。扩展 Kalman 粒子滤波与 Uncented粒子滤波算法在滤波的每一步迭代过程中， 首先利用最新观 测值，采用 UKF或者 EKF对各个粒子进行更新， 得到随机变量经非线性变换后的均值和方差， 并将它作为重要性概率密度函数 138 。另外，利用似然函数的梯度信息，采用牛顿迭代 139 或均值漂移 140 等方法移动粒子至高似然区域，也是一种可行的方案，如图 2.5

25、所示。以上这些方法的共同特点是将最新的观测数据融入到系统状态的转移过程中， 引导粒子到高似然区 域，由此产生的预测粒子可较好地服从状态的后验概率分布， 从而有效地减少描述后验概率 密度函数所需的粒子数。 xk(i )1, N 1 xk(i),N 1 x(ki),w(ki) xk(i),wk(i) x(ki) ,N 1图 2.4 结合均值漂移的粒子滤波算法Fig. 2.4 Particle filter combined with mean shift重采样方法针对序贯重要性采样算法存在的权值退化现象， Gordon 等提出了一种名为 Bootstrap 的 粒子滤波算法。 该算法在每步迭代过程

26、中， 根据粒子权值对离散粒子进行重采样， 在一定程 度上克服了这个问题。 重采样方法舍弃权值较小的粒子， 代之以权值较大的粒子。 重采样过 程在满足 p(xk(i ) xk(i) wk(i)条件下，将粒子集合 xk(i ) , wk(i ) 1N更新为 x(ki ) ,1/ N1N 。重采样 策略包括固定时间间隔重采样与根据粒子权值进行的动态重采样。 动态重采样通常根据当前 的有效粒子数或最大与最小权值比来判断是否需要进行重采样。 常用的重采样方法包括多项式 (Multinomial resampling) 重采样、残差重采样 (Residual resampling) 、分层重采样 (Str

27、atified resampling)与系统重采样 (Systematic resampling) 等。残余重采样法具有效率高、实现方便的 特点。设 N i Nwk(i) ，其中 为取整操作。残余重采样采用新的权值Nw*k( i ) Nk1 Nwk(i) Ni 选择余下的 Nk N Ni 个粒子，如图 2.6所示。残余重采样i1的主要过程为(1) 计算剩余粒子的权值累计量 j, j 1, ,Nk。(2) 生成 Nk 在个 0，1区间均匀分布的随机数 l lN?k1 ；(3) 对于每个 i ，寻找归一化权值累计量大于或等于 i 的最小标号 m ，即m1 l m。当 i落在区间 m1, m时， x

28、km被复制一次。这样，每个粒子 xk(i )经重采样后的个数为步骤 (3)中被选择的若干粒子数目与 Ni 之和。图 2.5 残差重采样Fig. 2.5 Residual Resampling重采样并没有从根本上解决权值退化问题。重采样后的粒子之间不再是统计独立关系， 给估计结果带来额外的方差。重采样破坏了序贯重要性采样算法的并行性，不利于 VLSI 硬 件实现。 另外， 频繁的重采样会降低对测量数据中野值的鲁棒性。 由于重采样后的粒子集中 包含了多个重复的粒子， 重采样过程可能导致粒子多样性的丧失， 此类问题在噪声较小的环 境下更加严重。 因此， 一个好的重采样算法应该在增加粒子多样性和减少权

29、值较小的粒子数 目之间进行有效折衷。图 2.7 为粒子滤波算法的示意图，该图描述了粒子滤波算法包含的时间更新、观测更新 和重采样三个步骤。 k 1时刻的先验概率由 N 个权值为 1/ N 的粒子 xk(i )1近似表示。 在时间 更新过程中，通过系统状态转移方程预测每个粒子在 k 时刻的状态 xk(i) 。经过观测值后，更 新粒子权值 wk(i) 。重采样过程舍弃权值较小的粒子， 代之以权值较大的粒子， 粒子的权值被 重新设置为 1/ N 。图 2.6SIR 算法示意图Fig. 2.6SIR algorithm标准的粒子滤波算法流程为：(1) 粒子集初始化， k 0 ：对于 i 1,2, ,N

30、 ，由先验 p(x0) 生成采样粒子 x0(i)iN1(2) 对于 k 1,2, ，循环执行以下步骤：重要性采样：对于 i 1,2, ,N ，从重要性概率密度中生成采样粒子 xk(i)iN1，计 算粒子权值 wk(i) ，并进行归一化；重采样：对粒子集 x(ki),wk(i) 进行重采样，重采样后的粒子集为 x(ki ) ,1/ N；N输出：计算 k 时刻的状态估计值： x?k xk(i )w(ki) 。i1粒子滤波中的权值退化问题是不可避免的。 虽然重采样方法可以在一定程度上缓解权值 退化现象，但重采样方法也会带来一些其它的问题。重采样需要综合所有的粒子才能实现， 限制了粒子滤波的并行计算。

31、 另外， 根据重采样的原则， 粒子权值较大的粒子必然会更多的 被选中复制，经过若干步迭代后，必然导致相同的粒子越来越多， 粒子将缺乏多样性， 可能 出现粒子退化现象， 从而使状态估计产生较大偏差。 针对粒子退化问题， 一个有效的解决方 法是增加马尔可夫蒙特卡洛 (Markov chain monte carlo,MCMC) 移动步骤 141 。马尔可夫链蒙特 卡洛方法 (如 Gibbs 采样、Metropolis-Hastings 采样等 )利用不可约马尔可夫过程可逆平稳分布 的性质， 将马尔可夫过程的平稳分布视为目标分布， 通过构造马尔可夫链产生来自目标分布 的粒子。 粒子退化问题是由于重采样使得粒子过分集中在某些状态上而导致的， 对重采样后 的粒子进行马尔可夫跳转可以提高粒子群的多样性， 同时保证跳转后的粒子同样能够准确的 描述既定的后验分布。 设粒子分布服从后验概率 p( x0:k | y1:k ) ，实施核为 k (x0:k |x0:k) 的马尔 科夫链变换之后，若满足k(x0:k |x0:k)p(x0:k |y1:k)dx0:k p(x0:k | y1:k) (0.24)便可以得到一组满足既定后验概率分布的粒子集合， 且这组新的粒子可能移动到状态空 间中更为有利的位置。qq(xx(k*ki)p