基于SSUKF的粒子滤波算法研究

杨 萌，郝燕玲，高 伟

(哈尔滨工程大学 自动化学院，黑龙江 哈尔滨 150001)

粒子滤波是当前得到广泛应用的一种非线性滤波，其主要问题是粒子退化、样本贫化以及计算量大，前两者会对滤波精度产生影响，后者则会影响算法的快速性.粒子滤波的原理决定了滤波精度与采样点数量关系密切，即要达到较为理想的滤波精度，就需要大量的采样点，算法的计算量也随之增大.此外，各种改进步骤以及对算法的优化也会增加整个算法的复杂性，进而影响滤波的实时性.由此可以得到粒子滤波的上述3个问题并不是孤立的，而是彼此关联的.也就是说，如果想抑制粒子退化和样本贫化，取得较好的滤波精度，就需要增加采样点数量或增大算法的复杂性，导致滤波的实时性降低，反之亦然.因此一种理想的粒子滤波算法应该在滤波精度和实时性上都具有较好的表现，或者针对所应用的具体场合在这两方面达到某种程度的平衡.

1 无迹粒子滤波(UPF)

1.1 粒子滤波

粒子滤波以蒙特卡罗方法和递推贝叶斯估计为基础，用带有权值的随机采样点来表示需要的后验概率密度［4］，根据测量调整粒子的权重和位置，获得状态变量的最优估计值.当采样点的数量足够多时，可以认为采样值的统计特性近似于后验概率密度的统计特性.

具体计算步骤如下:

1)初始化粒子，设置其初始均值和方差.

2)产生粒子:xik～π(xk|xik-1，yk)，选取重要性密度为先验密度.

3)重要性权值递推:

然后对权值进行归一化处理.

4)再采样:定义 Neff来衡量有效粒子数量，给定某一门限值 Nthr，当 Neff＜Nthr时，执行再采样.产生新的粒子集，重新设定粒子的权值为:wik=1/N.

5)输出一组粒子{(xik，wik)，i=1，…，N}，得到当前时间步的后验均值估计或加权均值估计.

1.2 UPF 算法

UPF算法即在PF的基础上利用UKF获得重要性概率密度.每一次采样后的粒子都由UKF算法进行更新，所得的均值和方差用于下次采样新的粒子［5］.在重要性采样环节引入最新的观测量，可以在一定程度上抑制粒子退化，但UKF也增加了算法的计算量.UKF采用UT变换，非线性函数无需线性化，将被估计状态的后验概率密度和方差精确到三阶，使分布朝高似然度区域移动，获得的重要性分布函数更接近状态的后验概率密度，因此滤波精度相对PF有较大提高.

2 超球面分布采样UKF(SSUKF)

UKF算法的计算效率很大程度上取决于UT变换中采样点的个数.对于n维随机向量，标准的UT变换采用2n+1个 sigma点对称采样［1］，计算量随向量维数的增大而增长.超球面分布采样变换是通过n+1个分布在以随机状态的均值为原点的超球面上的等权值sigma点来近似状态的概率分布，由n+1个超球面分布采样点和状态的均值点构成了n+2个Unscented变换采样点.

均值为0，均方差阵为单位阵的n维随机变量y的超球面分布采样点算法如下［5］:

1)选择权值 w0，满足:0≤w0≤1.

2)确定权值wi:

3)初始化向量序列:

4)扩展向量序列(j=2，…，n):

式中:eji表示j维随机变量的第i个采样点;0j表示j维零向量.

通过上述算法得到y的n+2个采样点eni，i=0，1，…，n+1.对于均值为 x^，均方差为 Pxx的 n 维随机变量x的超球面分布采样点可以由下式得到:

SSUKF用n+2个超球面分布的采样点代替了2n+1个对称分布的采样点，对于多维系统而言，减少了采样点个数，减少了计算量.

3 粒子群优化算法

PSO初始化为一群随机粒子，然后计算适配值对每个粒子进行评价，最后通过迭代找到最优解［6］.在每次迭代的过程中，粒子通过跟踪2个极值来更新自己的速度和位置.第i个粒子的位置表示为 Xi=(xi1，xi2，…，xin)，速度 Vi=(vi1，vi2，…，vin).个体极值 pbestPi=(pi1，pi2，…，pin);全局极值gbestGi=(gi1，gi2，…，gin).在找到上面 2 个最优解之后，每个粒子根据下面两式来更新其速度和位置:

式中:r1和r2是介于(0，1)区间的随机数;c1和c2称为学习因子，本文仿真中取c1=c2=2;w称为惯性因数，w较大则算法具有较强的全局搜索能力，w较小则算法倾向于局部搜索，本文仿真中取［0.9 0.4］随时间步线性递减.粒子适应度函数定义如下:

式中:Rk是观测噪声方差，yNew是最新的量测值，ypred是预测量测值.如果粒子集都分布在真实状态附近，那么粒子群中每个粒子的适应度都很高.反之，如果粒子群中每个粒子的个体最优值以及粒子群的全局最优值都很低，则说明粒子没有分布在真实状态附近.此时粒子集根据最优值并利用迭代来更新每个粒子的速度与位置，使得粒子不断地向真实状态靠近.

4 基于SSUKF的粒子滤波算法

基于 SSUKF的粒子滤波算法(SSUPF)即在UPF的UT变换环节采用超球面分布的SSUT变换，进而获得重要性概率密度.在理论上，SSUKF的滤波性能与采用对称分布采样的标准UKF相当［7］.因此，SSUPF算法既具有UPF充分考虑当前时刻观测值的特点，又可以有效提高计算效率.

PSO-SSUPF算法设计如下:

1)初始时刻，由初始分布p(x0)中得到一组初始粒子，并设置其初始的均值和方差.设置粒子初速度，设置惯性因数w，学习因子c1和c2，求解初始时刻的全局最优解G(0).

3)产生粒子:引入比例参数 c(0＜c＜1)，从SSUKF结果N(，Pik)中产生c×N个粒子，从先验概率分布中产生(1-c)×N个粒子.

4)权值更新，之后对其进行归一化处理.

5)再采样，并重新设定粒子权值.在本文仿真中不设定有效粒子阈值，直接进行再采样.

7)求解当前时刻的全局最优解G(t).

5 仿真及结果分析

5.1 单变量非静态增长模型(UNGM)

利用UNGM模型对PSO-SSUPF算法的性能和其他粒子滤波的性能进行比较，其状态方程和观测方程如下:

其中观测噪声vt～N(0，1)，系统噪声为如下的高斯和分布:

为验证PSO-SSUPF滤波方法使用较少的粒子，就可以获得相对较高的滤波精度，仿真分别使用50、100个粒子，建议分布比例参数c取为0.6.通过50次独立实验对目标的状态进行估计.

仿真数据在表1中显示，实验表明，当粒子数为50时，使用PSO优化的SSUPF性能有了明显的改善，误差明显减小，这是因为加入PSO后，粒子的多样性得到增强，精度随之提高.当粒子数为100时，本文算法的性能也明显优于其他粒子滤波算法.

表1 不同粒子滤波的RMSE比较Table 1 RMSE of different particle filters

5.2 强非线性非高斯模型

UKF算法中，sigma点数量的多少决定计算量的大小［7］.无论UT变换采用对称分布，还是超球面分布，计算量都会随系统状态维数的加大而增长，即目标状态维数越大，sigma点的数量越多，系统的计算量也就越大，本文提出的SSUPF在计算效率上的优势也就越明显.

综合上述条件，利用下面的模型对算法的性能进行验证［8］，该模型是比较各种粒子滤波的算法性能的标准验证程序之一［9］，其状态方程和观测方程如下:

其中过程噪声vt服从Gamma(3，2)分布，量测噪声nk服从高斯分布N(0，10-5).系统状态估计为，一次独立实验的均方误差为:Var=

在实验中，分别采用标准PF、EKPF以及UPF与本文提出的PSO-SSUPF算法进行比较，仿真的粒子数N分别为100和200个，量测时间为T=60，时间间隔为1 s，目标初始状态设为进行x0=1，50次独立的实验，仿真在Windows XP平台通过Matlab编程实现.

图1给出了在不同c的取值下各运行10次，PSO-SSUPF算法与其他粒子滤波算法均方根误差的对比曲线.可以得到，当 c取0时，算法即相当于PSOPF;当c取0～0.2之间时，本文算法的误差大于UPF和SSUPF，运算时间较少;当 c取0.3时，算法均方根误差与UPF非常接近，运算时间为UPF的29%;当c取0.4时，算法误差比UPF低17%，运算时间是 UPF的33%;当 c取0.5时，算法误差比UPF低23%，运算时间是UPF的38%;当c取0.5以上时，均方根误差变化不明显，运算时间逐渐增大，直到比SSUPF略大.总体上，随着c值的增加，本文算法在运行时间上也随之增加，均方根误差则呈现递减的趋势.在实际应用中，通过调整c的取值以平衡精度和运行效率，c值的合理取值范围应该在0.4与0.7之间，这样才能取得比较理想的优化效果，下面的仿真将c取为0.6.

图2给出了粒子数为200条件下标准PF、EKPF与本文算法在一次独立实验中的状态估计结果.图3为3种算法的估计误差.表2和表3分别给出了经过50次运行4种粒子滤波算法的均方根误差(RMSE)、有效粒子数以及平均运算时间.

图2 不同粒子滤波的估计(N=200)Fig.2 Estimation performance of different particle filters(N=200)

图3 不同粒子滤波的估计误差(N=200)Fig.3 Estimation error of different particle filters(N=200)

在理论上，标准UPF算法与SSUPF算法的复杂性相同，SSUT的优势是对于高维系统sigma点的数量是标准UT的一半略多.本文算法在使用混合建议分布基础上，加入了PSO，PSO对于粒子只是增加了2次加法运算，因此对于算法复杂性的影响很小.通过性能指标的比较可以得到，在4种滤波算法中，EKPF、UPF和PSO-SSUPF对于非线性问题的适应性要明显好于标准PF，本文算法的精度与UKF相近，较为明显的好于EKPF.但由于算法的改进和优化，增大了计算量，使上述3种算法的运算时间相对传统PF算法明显增大.本文算法的运算效率好于UPF，平均运算时间大幅少于UPF.在有效粒子数方面，UPF和本文算法的有效样本多于EKPF和标准PF，尤其是本文算法具有较高的粒子利用率，这表明PSO在抑制粒子退化改善粒子多样性上作用较为明显.此外，通过对表2和表3数据的纵向比较可以得到，各种滤波算法的精度会随着粒子数的增大而提高，同时运算时间也会随之增加.这说明在对于快速性要求不高的场合，使用较大数量的粒子，可以得到较高的滤波精度.

表2 不同粒子滤波的性能比较(N=100)Table 2 Performance comparison of different particle filters(N=100)

表3 不同粒子滤波的性能比较(N=200)Table 3 Performance comparison of different particle filters(N=200)

6 结束语

本文从兼顾算法计算效率和精度的角度出发，对标准UPF算法加以改进，采用SSUKF设计重要性概率密度函数和先验分布相结合的混合建议分布，并引入PSO方法，提出了一种基于SSUKF的粒子滤波算法，即PSO-SSUPF.通过仿真对比本文算法与标准PF、EKPF、UPF的性能，仿真结果以及统计数据表明:1)这种算法保持了与标准UPF算法相近的滤波精度，优于标准PF和EKPF，运算时间明显少于UPF，而与EKPF相当.2)由UT变换和SSUT变换的原理可以得到，系统维数越大，SSUKF在计算效率上的优势就将越发明显.

本文只是运用简单的非线性模型对PSO-SSUPF算法的理论性能进行了验证，而这种算法在解决复杂非线性问题时的性能，以及如何与再采样环节相结合发挥出理想的滤波效果还有很大的研究空间.

［1］JULIER S J，UHLMANN J K，DURRANT-WHYTTE H F.A new method for the nonlinear transformation of means and covariance in filter and estimators［J］.IEEE Transactions on Automatic Control，2000，45(3):477-482.

［2］JULIER S J.The spherical simplex unscented transformation［C］//Proceedings of the American Control Conference.Denver，USA，2003.

［3］宫轶松，归庆明，孙付平，等.智能优化方法与粒子滤波技术融合的分析与展望［J］.海洋测绘，2009，29(2):74-77.GONG Yisong，GUI Qingming，SUN Fuping，et al.Survey and prospect of the fusion of intelligent compupational approaches and particle filtering technique［J］.Hydrographic Surveying and Charting，2009，29(2):74-77.

［4］HAUG A J.A tutorial on Bayesian estimation and tracking techniques applicable to nonlinear and non-Gaussian processes［R］.［s.l.］MITRE Technical Report，2005.

［5］丁杨斌，申功勋.Unscented粒子滤波在静基座捷联惯导系统大方位失准角初始对准中的应用研究［J］.航空学报，2007，28(2):397-401.DING Yangbin，SHEN Gongxun.Study on unscented particle filter applied in initial alignment of large azimuth misalignment on static base of SINS［J］.Acta Aeronautica et Astronautica Sinica，2007，28(2):397-401.

［6］TOMOAKI K，KEITA N.Real time object tracking on video image sequence using particle swarm optimization［J］.Control，Aotomation and Systems，2007，10:1773-1778.

［7］潘泉，杨峰，叶亮，等.一类非线性滤波器——UKF综述［J］.控制与决策，2005，20(5):481-489.PAN Quan，YANG Feng，YE Liang，et al.Survey of a kind of nonlinear filters—UKF［J］.Control and Decision，2005，20(5):481-489.

［8］KOTECHA J H，DJURIC P M.Gaussian partical filter［J］.IEEE Transactions on Signal Processing，2003，51(2):2592-2601.

［9］方正，佟国峰，徐心和.粒子群优化粒子滤波方法［J］.控制与决策，2007，22(3):273-277.FANG Zheng，TONG Guofeng，XU Xinhe.Particle swarm optimized particle filter［J］.Control and Decision，2007，22(3):273-277.