1、2 粒子滤波理论粒子滤波通过非参数化的蒙特卡洛(Monte Carlo)模拟方法来实现递推贝叶斯滤波，适用于任何能用状态空间模型描述的非线性系统，精度可以逼近最优估计。粒子滤波器具有简单、易于实现等特点，它为分析非线性动态系统提供了一种有效的解决方法，从而引起目标跟踪、信号处理以及自动控制等领域的广泛关注。本章首先概述用于求解目标状态后验概率的贝叶斯滤波理论，随后介绍具有普遍适用性的粒子滤波器，最后针对当前粒子滤波器存在的粒子多样性丧失问题，提出了一种量子进化粒子滤波算法。2.1 贝叶斯滤波动态系统的目标跟踪问题可以通过图2.1所示的状态空间模型来描述。本节在贝叶斯滤波框架下讨论目标跟踪问题。
2、图 2.1 状态空间模型Fig. 2.1 State space model在目标跟踪问题中，动态系统的状态空间模型可描述为* MERGEF1()kkxfuyhvORMAT (2.1)其中 分别为状态转移方程与观测方程， 为系统状态， 为观测值， 为过程(),fh kxkyku噪声， 为观测噪声。为了描述方便，用 与kv 0:1,Xx分别表示 到 时刻所有的状态与观测值。在处理目标跟踪问题时，1:,kYyy 0k通常假设目标的状态转移过程服从一阶马尔可夫模型，即当前时刻的状态 只与上一时刻kx的状态 有关。另外一个假设为观测值相互独立，即观测值 只与 时刻的状态 有关。-1kx kyk贝叶斯滤
3、波为非线性系统的状态估计问题提供了一种基于概率分布形式的解决方案。贝叶斯滤波将状态估计视为一个概率推理过程，即将目标状态的估计问题转换为利用贝叶斯公式求解后验概率密度 或滤波概率密度 ，进而获得目标状态的最优(|)kpXY(|)kpxY估计。贝叶斯滤波包含预测和更新两个阶段，预测过程利用系统模型预测状态的先验概率密度，更新过程则利用最新的测量值对先验概率密度进行修正，得到后验概率密度。假设已知 时刻的概率密度函数为 ，贝叶斯滤波的具体过程如下：1k1(|)kx(1) 预测过程，由 得到 ：1(|)kpxY|pY* MERGEFORMAT (2.2)1 1(,|)|,(|)k kkpxx 当给定
4、 时，状态 与 相互独立，因此x1* MERGEFORMAT (2.3)1 1(,|)(|)(|)kkkkxYpxY 上式两端对 积分，可得 Chapman-Komolgorov 方程（？111(|)(|)(|)dkkkkpxpxYx？） * MERGEFORMAT (2.4)(2) 更新过程，由 得到 ：1(|)kY)|(kY获取 时刻的测量 后，利用贝叶斯公式对先验概率密度进行更新，得到后验概率ky* MERGEFORMAT 11(|,)(|)(|)|kkkkpxYpxY(2.5)假设 只由 决定，即kykx* MERGEFORMAT 1(|,)(|)kkpyYpyx(2.6)因此* 1(
5、|)(|)(|)kkkpyxYxYMERGEFORMAT (2.7)其中， 为归一化常数1(|)kpyY* 1(|)kpyY1(|)(|)dkkkpyxYxMERGEFORMAT (2.8)贝叶斯滤波以递推的形式给出后验(或滤波)概率密度函数的最优解。目标状态的最优估计值可由后验(或滤波)概率密度函数进行计算。通常根据极大后验(MAP)准则或最小均方误差(MMSE)准则，将具有极大后验概率密度的状态或条件均值作为系统状态的估计值，即* =argmin(|)kMAPkkxpYMERGEFORMAT (2.9) * MERGEFORMAT (2.10) =E()|()|dMSkkkkxfYfxpY
6、贝叶斯滤波需要进行积分运算，除了一些特殊的系统模型（如线性高斯系统，有限状态的离散系统）之外，对于一般的非线性、非高斯系统，贝叶斯滤波很难得到后验概率的封闭解析式。因此，现有的非线性滤波器多采用近似的计算方法解决积分问题，以此来获取估计的次优解。在系统的非线性模型可由在当前状态展开的线性模型有限近似的前提下，基于一阶或二阶 Taylor 级数展开的扩展 Kalman 滤波得到广泛应用 119。在一般情况下，逼近概率密度函数比逼近非线性函数容易实现。据此，Julier 与 Uhlmann 提出一种 Unscented Kalman 滤波器，通过选定的 sigma 点来精确估计随机变量经非线性变换
7、后的均值和方差，从而更好的近似状态的概率密度函数，其理论估计精度优于扩展 Kalman 滤波 120。获取次优解的另外一中方案便是基于蒙特卡洛模拟的粒子滤波器。2.2 粒子滤波早在 20 世纪 50 年代，Hammersley 便采用基于序贯重要性采样(Sequential importance sampling,SIS)的蒙特卡洛方法解决统计学问题 121。20 世纪 60 年代后期，Handschin 与Mayne 使用序贯蒙特卡洛方法解决自动控制领域的相关问题 122。20 世纪 70 年代，Handschin、Akashi 以及 Zaritskii 等学者的一系列研究工作使得序贯蒙特卡
8、洛方法得到进一步发展 123 124, 125 126。限于当时的计算能力以及算法本身存在的权值退化问题，序贯重要性采样算法没有受到足够重视，在随后较长一段时间内进展较为缓慢。直到 20 世纪 80 年代末，计算机处理能力的巨大进展使得序贯蒙特卡洛方法重新受到关注。Tanizaki、Geweke 等采用基于重要性采样的蒙特卡洛方法成功解决了一系列高维积分问题127-130。Smith 与 Gelfand 提出的采样-重采样思想为 Bayesian 推理提供了一种易于实现的计算策略 131。随后，Smith 与 Gordon 等人合作，于 20 世纪 90 年代初将重采样(Resampling)
9、步骤引入到粒子滤波中，在一定程度上解决了序贯重要性采样的权值退化问题，并由此产生了第一个可实现的 SIR(Sampling importance resampling)粒子滤波算法(Bootstrap 滤波) 132，从而掀起粒子滤波的研究热潮。美国海军集成水下监控系统中的 Nodestar 便是粒子滤波应用的一个实例。进入 21 世纪，粒子滤波器成为一个非常活跃的研究领域，Doucet、Liu、Arulampalam 等对粒子滤波的研究作了精彩的总结 133-135，IEEE 出版的论文集“Sequential Monte Carlo Methods in Practice”对粒子滤波器进行
10、了详细介绍 136。2.2.1 贝叶斯重要性采样蒙特卡洛模拟是一种利用随机数求解物理和数学问题的计算方法，又称为计算机随机模拟方法。该方法源于第一次世界大战期间美国研制原子弹的曼哈顿计划，著名数学家冯诺伊曼作为该计划的主持人之一，用驰名世界的赌城，摩纳哥的蒙特卡洛来命名这种方:法。蒙特卡洛模拟方法利用所求状态空间中大量的样本点来近似逼近待估计变量的后验概率分布，如图 2.2 所示，从而将积分问题转换为有限样本点的求和问题。粒子滤波算法的核心思想便是利用一系列随机样本的加权和表示后验概率密度，通过求和来近似积分操作。假设可以从后验概率密度 中抽取 个独立同分布的随机样本 ，(|)kpxYN()i
11、kx，则有 1,iN* MERGEFORMAT (2.11)()1(|)NikkipxYx这里 为连续变量， 为单位冲激函数(狄拉克函数) ，即 ，k-k (-)0,kkxx且 。当 为离散变量时，后验概率分布 可近似逼近为()d1xx(|)kPY* MER()1(|)NikkiPxYxGEFORMAT (2.12)其中， 。()()1,;iikkxx()()0,i ikkxxP图 2.2 经验概率分布函数Fig. 2.2 Empirical probability distribution function设 为从后验概率密度函数 中获取的采样粒子，则任意函数 的期望()ikx)|(kYxp
12、 ()kfx估计可以用求和方式逼近，即* MERGEFORMAT (2.13)()k 1E()|()|dNikkkifYfxf蒙特卡洛方法一般可以归纳为以下三个步骤：(1)构造概率模型。对于本身具有随机性质的问题，主要工作是正确地描述和模拟这个概率过程。对于确定性问题，比如计算定积分、求解线性方程组、偏微分方程等问题，采用蒙特卡洛方法求解需要事先构造一个人为的概率过程，将它的某些参量视为问题的解。(2)从指定概率分布中采样。产生服从己知概率分布的随机变量是实现蒙特卡洛方法模拟试验的关键步骤。(3)建立各种估计量的估计。一般说来，构造出概率模型并能从中抽样后，便可进行现模拟试验。随后，就要确定一
13、个随机变量，将其作为待求解问题的解进行估计。在实际计算中，通常无法直接从后验概率分布中采样，如何得到服从后验概率分布的随机样本是蒙特卡洛方法中基本的问题之一。重要性采样法引入一个已知的、容易采样的重要性概率密度函数 ，从中生成采样粒子，利用这些随机样本的加权和来逼近后(|)kqxY验滤波概率密度 ，如图2.3所示。令 表示一支撑点集，其中|p(),1,.iikxwN为是 时刻第 个粒子的状态，其相应的权值为 ，则后验滤波概率密度可以表示为()ikxi ()i* MERGEFORMA()()1(|)NiikkkipxYxT (2.14)其中，* MERGEFORMAT (2.15)()()|ii
14、kkpxYwq图 2.3 重要性采样Fig. 2.3 Importance sampling当采样粒子的数目很大时，式(2.14)便可近似逼近真实的后验概率密度函数。任意函数的期望估计为()kfx* MERGEFORMAT ()() ()1 1|E()|=iNNi iikkk ki ipxYfYf fxwq2.16)2.2.2 序贯重要性采样算法在基于重要性采样的蒙特卡洛模拟方法中，估计后验滤波概率需要利用所有的观测数据，每次新的观测数据来到都需要重新计算整个状态序列的重要性权值。序贯重要性采样作为粒子滤波的基础，它将统计学中的序贯分析方法应用到的蒙特卡洛方法中，从而实现后验滤波概率密度的递推
15、估计。假设重要性概率密度函数 可以分解为0:1:(|)kqxy* MERGEFORMAT (2.17)0:1:0:1:0:1:(|)(|)(|,)kkkkqxyqxyqxy设系统状态是一个马尔可夫过程，且给定系统状态下各次观测独立，则有* MERGEFORMAT (0:011()(|)kkiipxpx2.18)* MERGEFORMAT (2.11:1(|)(|)kkiiyy9)后验概率密度函数的递归形式可以表示为0:10:10:(|,)(|)(|)|kkkkpyxYpxY* MERGEFOR0:10:10:1:1(|,)(|,)(|)|()kkkkkkkkyxYpypxYMAT (2.20)
16、粒子权值 的递归形式可以表示为()ikw()()0:|iikkpxYwq()()()10:10:|,iiiikkkyxpYq* MERGEFORMAT (2.21)()()() 110:|,iiiikkkpyxwqY通常，需要对粒子权值进行归一化处理，即* MERGEFO()()1iikkNiiwRMAT (2.22)序贯重要性采样算法从重要性概率密度函数中生成采样粒子，并随着测量值的依次到来递推求得相应的权值，最终以粒子加权和的形式来描述后验滤波概率密度，进而得到状态估计。序贯重要性采样算法的流程可以用如下伪代码描述： () ()111,)iiNiiNkkkxwSIxwYFor i=1:N(
17、1)时间更新，根据重要性参考函数 生成采样粒子 ；()0:1|,)iikkqxY()ikx(2)量测更新，根据最新观测值计算粒子权值 ；()iwEnd For粒子权值归一化，并计算目标状态。为了得到正确的状态估计，通常希望粒子权值的方差尽可能趋近于零。然而，序贯蒙特卡洛模拟方法一般都存在权值退化问题。在实际计算中，经过数次迭代，只有少数粒子的权值较大，其余粒子的权值可忽略不计。粒子权值的方差随着时间增大，状态空间中的有效粒子数较少。随着无效采样粒子数目的增加，使得大量的计算浪费在对估计后验滤波概率分布几乎不起作用的粒子更新上，使得估计性能下降。通常采用有效粒子数 来衡efN量粒子权值的退化程度
18、，即* MERG*()/(1varief kNwEFORMAT (2.23)* MERGEFORMAT (2.24)()*()1:|,iikkkpxywq有效粒子数越小，表明权值退化越严重。在实际计算中，有效粒子数 可以近似为efN* MERGEFORMAT (2.25)()21efNikiw在进行序贯重要性采样时，若 小于事先设定的某一阈值，则应当采取一些措施加ef以控制。克服序贯重要性采样算法权值退化现象最直接的方法是增加粒子数，而这会造成计算量的相应增加，影响计算的实时性。因此，一般采用以下两种途径：(1)选择合适的重要性概率密度函数；(2)在序贯重要性采样之后，采用重采样方法。2.2.
19、3 重要密度函数的选择重要性概率密度函数的选择对粒子滤波的性能有很大影响，在设计与实现粒子滤波器的过程中十分重要。在工程应用中，通常选取状态变量的转移概率密度函数 作1(|)kpx为重要性概率密度函数。此时，粒子的权值为* MERGEFORMAT (2.26)()()()1|iiikkwpyx转移概率的形式简单且易于实现，在观测精度不高的场合，将其作为重要性概率密度函数可以取得较好的滤波效果。然而，采用转移概率密度函数作为重要性概率密度函数没有考虑最新观测数据所提供的信息，从中抽取的样本与真实后验分布产生的样本存在一定的偏差，特别是当观测模型具有较高的精度或预测先验与似然函数之间重叠部分较少时
20、，这种偏差尤为明显。选择重要性概率密度函数的一个标准是使得粒子权值 的方差最小。Doucet 等()1iNkw给出的最优重要性概率密度函数为 () ()11()()1|,|,|iiiikkkkiikkqxypxypx* MERGEF()()1|iiikkpyxORMAT (2.27)此时，粒子的权值为* MERGEFORM()()()11|iiikkwpyxAT (2.28)以 作为重要性概率密度函数需要对其直接采样。此外，只有在 为有限离()1|,)iikkpxy kx散状态或 为高斯函数时， 才存在解析解。在实际情况中，构造()|ii ()1|ikpyx最优重要性概率密度函数的困难程度与直
21、接从后验概率分布中抽取样本的困难程度等同。从最优重要性概率密度函数的表达形式来看，产生下一个预测粒子依赖于已有的粒子和最新的观测数据，这对于设计重要性概率密度函数具有重要的指导作用，即应该有效利用最新的观测信息，在易于采样实现的基础上，将更多的粒子移动到似然函数值较高的区域，如图 2.4 所示。图 2.4 移动粒子至高似然区域Fig. 2.4 Move the samples in the prior to regions of high likelihood辅助粒子滤波算法利用 时刻的信息，将 时刻最有前途(预测似然度大) 的粒子扩k1k展到 时刻 137，从而生成采样粒子。与 SIR 滤波
22、器相比，当粒子的似然函数位于先验分布k的尾部或似然函数形状比较狭窄时，辅助粒子滤波能够得到更精确的估计结果。辅助粒子滤波引入辅助变量 来表示 时刻的粒子列表，应用贝叶斯定理，联合概率密度函数m1可以描述为1:(,|)kkpxy1: 1:1:(,|)(|)(,|)(|)kkkkkt kpxypxmyp* MERGEF1(|)(|)mkkkwORMAT (2.29)生成 的重要性概率密度函数 为()1,iiNkxm 0:1:(,|,)kkqxy* MERGEFORMAT (2.30)0:1: 1(,|,)(|)|)mmkkkkkqypw 其中 为由 预测出的与 相关的特征，可以是采样值 或预测mk
23、()1iNkxkx 1(|)mmkkpx:均值 。|mE定义 ，由于1: 1(|,)(|)mkkkqxypx* MERGEFORMAT (2.31)1: 1:1:,|,(|kkkkkyq则有* MERGEFORMAT (2.32)1: 1(|)(|)mkkkqypw此时，粒子权值 为()ikw* MERGEFORMAT (2.()() ()()() 0:1|,ii iiimimkkkk mpyxpyxq 33)采用局部线性化的方法来逼近 是另一种提高粒子采样效率的有效方法。1(|,)kkpxy扩展Kalman粒子滤波与Uncented粒子滤波算法在滤波的每一步迭代过程中，首先利用最新观测值，采
24、用UKF或者EKF 对各个粒子进行更新，得到随机变量经非线性变换后的均值和方差，并将它作为重要性概率密度函数 138。另外，利用似然函数的梯度信息，采用牛顿迭代 139或均值漂移 140等方法移动粒子至高似然区域，也是一种可行的方案，如图2.5所示。以上这些方法的共同特点是将最新的观测数据融入到系统状态的转移过程中，引导粒子到高似然区域，由此产生的预测粒子可较好地服从状态的后验概率分布，从而有效地减少描述后验概率密度函数所需的粒子数。 ()1,ikxN(),ik(),iikxw(),ik()1ixN图 2.5 结合均值漂移的粒子滤波算法Fig. 2.5 Particle filter comb
25、ined with mean shift2.2.4 重采样方法针对序贯重要性采样算法存在的权值退化现象，Gordon 等提出了一种名为 Bootstrap的粒子滤波算法。该算法在每步迭代过程中，根据粒子权值对离散粒子进行重采样，在一定程度上克服了这个问题。重采样方法舍弃权值较小的粒子，代之以权值较大的粒子。重采样过程在满足 条件下，将粒子集合 更新为 。()()()iiikkpxw()1,iiNkxw()1,/iNkx重采样策略包括固定时间间隔重采样与根据粒子权值进行的动态重采样。动态重采样通常根据当前的有效粒子数或最大与最小权值比来判断是否需要进行重采样。常用的重采样方法包括多项式(Multinomial resampling)重采样、残差重采样 (Residual resampling)、分层重采样(Stratified resampling)与系统重采样 (Systematic resampling)等。残余重采样法具有效率高、实现方便的特点。设 ，其中 为取整操作。残余重采样采用新的权值()iikNw:选择余下的 个粒子，如图 2.6 所示。残余重采样*()1()iiikkw 1Niki的主要过程为
