基于概率核函数的改进粒子滤波算法研究

张恒浩 唐 超 王小锭 魏 明 张 霞 吴胜宝

中国运载火箭技术研究院研发中心，北京100076

针对粒子滤波算法在组合导航系统应用中出现的粒子衰减退化问题［1-2］，引入基于概率核函数的改进粒子滤波算法。这种改进算法在设计过程中引入基于概率统计的核函数，通过改进粒子滤波算法中的重要性函数来获得全面的系统信息;然后设计出核函数和对应的交互核函数，并将其应用在粒子滤波算法的重采样计算中;最后利用取得的粒子集合和对应的权值生成新的粒子集。这样改进的粒子滤波算法在阻止粒子集合退化现象发生的同时还可以保持粒子的多样性，从而可以达到提高系统导航精度的目的。仿真结果表明，引入改进粒子滤波算法的组合导航系统可以有效降低系统的误差，提高系统导航精度。

1 重要性采样函数设计

可以看出，改进粒子滤波算法在计算重要性采样时考虑了已知量测值条件下状态估计概率和前一状态值条件下的状态估计概率。通过这2种状态估计概率值的比例，最大程度上反映了重要性采样函数表示的系统信息，避免了可能因为传感器精度的限制而出现的采样集合丢失。这样的设计可以有效地提高系统粒子滤波的性能。

2 基于随机概率统计的核函数设计

核函数算法主要是用来提供一种处理非线性问题的方法。粒子滤波算法中引入核函数的主要目的是希望通过核函数算法来解决粒子滤波算法在计算过程中发生的粒子退化现象。核函数的设计要与粒子滤波算法中得到的粒子集合的特征相对应。另外在设计核函数的过程中还要考虑粒子滤波算法的计算量和计算复杂度，避免在计算过程中出现获得非线性积分数据的困难［8］。

因此，针对应用在组合导航系统中的粒子滤波算法改进这一特定问题，选择什么样的核函数是至关重要的。而且即使选择了某一核函数，相应参数选取的不同也会影响改进的粒子滤波算法的计算精度。所以改进粒子滤波算法中引入的核函数算法必须考虑重采样粒子的概率特征，通过概率特征来设计核函数的概率密度函数和分布函数，然后再通过重采样计算得到新的粒子集。下面给出基于概率统计理论的核函数推导。

假设X作为一个随机变量，则将函数式(5)所表示的函数称为随机变量X的概率分布函数［9］。

根据上述概率密度函数的定义，核函数设计的核心是要从粒子滤波得到的重要性采样样本数据中建立一个概率密度函数［11］，然后将这一概率密度函数引入重采样计算中。

为了设计核函数，必须考虑粒子滤波中重采样得到的粒子集合的特征，然后才能根据特征设计有针对性的核函数。下面分析原来典型的粒子滤波重采样算法得到的粒子集合的性质。

重采样粒子具有随机性，所以它们彼此之间具有独立性，相互之间不影响对方的采样数值。粒子重采样还是基于系统状态方程的估计进行采样的，这些粒子同系统状态方程的分布有相同的分布，所以具有同分布性。同时满足同分布性和随机性的粒子集合可以近似看做是随机抽样样本［12］。经过上面描述可知，粒子滤波过程中的重采样得到的新的粒子集合满足上述条件，在引入核函数改进粒子滤波算法的过程中可以将重采样过程得到的粒子集合看做是随机采样数据集合。在设计核函数的时候利用随机抽样集合的特征来建立核函数方程。

假设粒子滤波计算过程中一共有n个重采样粒子，可以看做是n个样本估计。设对应的第i个粒子对应得到的数据为k(xi，x)。由样本估计原理计算得到粒子集合整体的概率密度估计表达式为:

定义由重采样计算得到的第i个粒子对应的函数表达式k(xi，x)为交互核函数，对于在粒子滤波算法中的核函数则定义其表达式为K(xi，x)。

因为交互核函数k(xi，x)实际上是重采样算法得到的粒子中第i个粒子的概率密度函数值，所以对于任意的x，交互核函数k(xi，x)必须满足条件:

核函数K(xi，x)表示的含义是在粒子滤波重采样中得到的第i个粒子的分布函数值。核函数和交互核函数之间的关系如下:

为了在计算中方便求解，一般在设计交互核函数时要尽可能地将已知量xi的函数表达式和未知量x的函数表达式分开。这样在计算过程中可以更方便地求出核函数，这种设计方法可以使粒子滤波算法减小计算量，有利于满足系统的实时性要求。

根据这一思路，设计的交互核函数一般将其改进为:

式(12)中，因为设计的交互核函数k(xi，x)已将关于xi的函数表达式和x的函数表达式分开，在求解核函数K(xi，x)时只对x做积分运算。所以在计算过程中可以将函数ψi(xi)看做常数。设改进后的交互核函数ψi(xi)φi(x)对x求积分得到的核函数为:

设计出核函数K(xi，x)和交互核函数k(xi，x)后，将它们引入粒子滤波算法中的重采样计算中，利用取得的粒子集合和对应的权值生成新的粒子集，在阻止粒子集合退化现象发生的同时保持粒子的多样性，这样可以提高粒子滤波算法的计算精度。

3 引入核函数的改进粒子滤波算法设计

因为传统的粒子滤波算法所依据的信息主要是离散的样本粒子及其在后验概率分布中占有比重权值的粒子。在传统的粒子滤波算法中引入重采样方法的主要目的就是利用粒子权值来区别对待所获得的采样粒子。它通过保留或者复制粒子权值较大的粒子、放弃删除粒子权值较小的粒子来解决粒子退化问题。由于大量的粒子在迭代过程中逐渐由相同的大权值粒子复制而来，这样的结果必然导致粒子样本的多样性丧失，样本粒子逐渐枯竭。

改进粒子滤波算法根据前面推导得到的样本粒子集合以及对应的权值，通过设计的核函数来模拟拟合离散分布，估计出后验概率密度的连续模型分布函数，再根据这个模型对粒子滤波算法进行重采样，这样可以得到更具多样性的粒子，从而保证了粒子集合的多样性，并能有效地抑制粒子退化现象。

因此，推导改进的核函数粒子滤波算法的计算步骤为:

步骤5:状态输出估计:当系统的改进粒子滤波在步骤4中进行重采样计算后，则状态输出估计表达式为:

步骤6:设k=k+1，并返回步骤2继续计算。

这6个步骤给出了引入核函数的粒子滤波改进算法的计算推导过程。在这一改进算法中，首先，对重要性函数设计所作的改进会使算法能通过采样粒子更全面地反映系统状态，从而提高粒子滤波器的工作性能;其次，在重采样过程中引入基于概率统计的核函数算法，计算生成新的粒子集合。核函数的引入可以使粒子滤波算法在计算过程中保持粒子集合的多样性，不因粒子滤波算法的迭代而使粒子性质变得单一;最后由于核函数计算过程中引入了随机抽样算法的平均计算思路，这样如果计算过程中出现粒子失真现象时可通过加权平均的思想将其变得稳定。所以在计算粒子滤波过程中，核函数解算可使粒子滤波中得到的重采样粒子集合变得平滑。

4 仿真验证

为验证改进粒子滤波算法的性能，在组合导航系统中应用改进的粒子滤波核函数算法和传统的粒子滤波算法进行组合导航系统的滤波设计，通过两者结果的比较来验证改进的粒子滤波算法的有效性和可行性。

组合导航系统由惯性导航系统和GPS卫星定位系统组成，组合导航系统的状态方程和观测方程表达式如式(24)和(25)所示:

组合导航系统工作时的初始参数如下:

初始状态的速度误差和位置误差均为0，计算过程中的迭代时间为300s;

粒子算法中计算的粒子数目设为40，重采样的粒子限制值Nth设为20。

在计算过程中通过对组合导航系统内部的状态误差进行估计，并得到计算结果。通过比对2种算法的计算结果来判断改进算法的优劣。

组合导航系统在应用改进的粒子滤波核函数算法时需要设计核函数，根据推导核函数时给出的核函数相关信息，选择核函数K(xi，x)的表达式为:

为了简化计算，在核函数式(27)中设参数λ的值为1。

这样将设置好参数的核函数和交互核函数引入改进粒子滤波算法的重采样算法中，分别将改进的粒子滤波核函数算法和原来的经典粒子滤波算法应用到组合导航系统中。这2种算法都是通过粒子解算估计出组合导航状态X的估计值，再通过滤波算法得到下一状态的推测值。对比2种算法得到的误差结果，从而判断改进的粒子滤波算法是否能起到改善组合导航系统性能的要求。

图1～5为2种算法的结果比较。图中的实线表示引进核函数的改进粒子滤波算法得到的结果，虚线表示传统的粒子滤波算法得到的结果。

从仿真结果可以看出，当粒子集合退化严重，粒子集合向单一性发展的时候，传统的粒子滤波算法会产生较大的导航误差，并且误差有发散趋势。引入核函数的改进粒子滤波算法在工作中可以降低由粒子集合退化产生的组合导航系统误差，并且使系统误差的变化趋于平稳。组合导航系统在工作时误差的变化率变得平稳，这样有利于系统在工作中达到稳定状态。

图1 正北方向位置误差比较

图2 正东方向位置误差比较

图3 高度位置误差比较

图4 北向速度误差比较

图5 东向速度误差比较

5 结论

针对传统粒子滤波算法中出现的粒子衰减退化问题，引入了基于概率核函数设计的改进粒子滤波算法。并将这一算法应用在组合导航系统中。该算法在粒子滤波算法出现粒子衰减退化现象时能够有效抑制粒子衰减并保持粒子的多样性。仿真结果表明，这种基于概率核函数设计的改进粒子滤波算法可以很好地解决粒子滤波算法在计算过程中出现的粒子衰减退化问题，提高组合导航系统的计算精度。

［1］ Julier S，Unlmann J.Unscented Filtering and Nonlinear Estimation［J］.Proceedings of IEEE，2004，192(3):401-422.

［2］ Rudolph V M，Arnaud D.The Unscented Particle Filter［R］.CU ED/F-Infeng/Tr380.Cambridge University Engineering Department，2001.

［3］ Erik B，Peter J A，Nils C，John M S.Monte Caulo filter for Non-linear State Estimation［J］.Automatica，2001，37(2):177-183.

［4］ 庄泽森，张建秋，尹建君.Rao—Blackwellizes粒子概率假设密度滤波算法［J］.航空学报，2009，30(4):698-705.(ZHUANG Zesen， ZHANG Jianqiu， YIN Jianjun.Rao-Blackwellized Particle Probability Hypothesis Density Filter［J］.Acta Aeronautica ET Astronautica Sinica，2009，30(4):698-705.)

［5］ 张磊.一种基于高斯混合模型粒子滤波的故障预测算法［J］.航空学报，2009，30(2):319-324.(ZHANG Lei.A Fault Prognostic Based on Gaussian Mixture Model Particle Filter.Acta Aeronautica ET Astronautica Sinica，2009，30(2):319-324.

［6］ Petovello M G，Lachapelle G.Comparison of Vectorbased Software Receiver Implementations with Application to Ultra-tight GPS/INS Integration［C］.ION GNSS 2006.Fort Worth TX，2006:1-10.

［7］ Rankin J.An Errormodel for Sensor Simulation GPS and Differential GPS［C］.IEEE Position Location and Navigation Symposium，1994:260-266.

［8］ Sumit Roy，Ronald A.Iltis Decentralized Linear Estimation in Correlated Measurement Noise［J］.IEEE Transaction on Aerospace and Electronic Systems，1999，27(6):939-941.

［9］ 葛哲学，杨拥民.非高斯噪声下基于Unscented粒子滤波器的非线性系统故障诊断方法［J］.兵工学报，2007，28(3):332-335.(GE Zhemin，YANG Yongmin.Unscented Particle Filter-based Fault Diagnosis of Nonlinear System with Non-Gaussian Noises［J］.Actaarmamentarll，2007，28(3):332-335.)

［10］ 于飞，唐小勇，潘洪悦.改进粒子群算法在三维水下导航规划中的应用［J］.北京理工大学学报，2010，30(9):1059-1064.(YU Fei，TANG Xiaoyong，PAN Hongyue.The Application of an Improved PSO to the Submersible Path-Planning［J］.Transactions of Beijing Institute of Technology，2010，30(9):1059-1064.)

［11］ Thierry Chateau.M2SIR:A Multi Modal Sequential Importance Resampling Algorithm for Particle Filters［C］.IEEE ICIP2009:4073-4076.

［12］ Wataru Sawabe.Application of Particle Filter to Autonomous Navigation System for Outdoor Environment［C］.SICE Annual Conference 2008:93-96.