基于似然分布的样本数自适应UPF算法

高 怡，高 雅，高社生

（1. 西安石油大学 电子工程学院，西安 710065；2. 西安工业大学 电子信息工程学院，西安 710021；3. 西北工业大学 自动化学院，西安 710072）

基于似然分布的样本数自适应UPF算法

高 怡1，高 雅2，高社生3

（1. 西安石油大学 电子工程学院，西安 710065；2. 西安工业大学 电子信息工程学院，西安 710021；3. 西北工业大学 自动化学院，西安 710072）

针对粒子滤波算法的实时性较差，计算量随着粒子数的增加成级数增加，提出一种基于似然分布的样本数自适应UPF算法。该算法以UPF为基础，吸收了似然分布自适应和样本数自适应的优点，在每一步状态方差估计中规定样本数的下限，同时考虑状态方差过大和过小的情况，在重采样阶段嵌入似然采样，根据反映量测噪声实时统计性能的精度因子α自适应地调整似然分布状态，使之尾部更为平坦，增加先验和似然的重叠区，减少粒子退化。利用 UT变换获得各个粒子的重要性密度函数，并将最新的量测信息引入到重要性密度函数设计以及重采样过程中，从而达到提高算法估计性能的目的。将提出的算法应用到SINS/SAR组合导航系统中进行仿真验证，结果表明，与PF和UPF算法相比，提出的基于似然分布的粒子数自适应UPF算法能有效改善滤波性能，提高解算精度。

Unscented粒子滤波；样本数；自适应滤波；似然分布；组合导航

粒子滤波(Particle Filters, PF)算法是一种基于贝叶斯采样估计的顺序重要采样滤波算法，通过寻找一组在状态空间中传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差估计的过程。它对非线性系统和非高斯估计问题提供了统一的解决框架，尤其对强非线性系统的滤波问题有独特的优势，摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约条件[1]。但粒子滤波算法在滤波过程中使用的粒子数目较多，导致计算量大，实时性较差，无法避免地出现粒子退化现象。而解决退化问题的常用方法是选取重要性密度函数和重采样，R. Van der Merwe等学者提出了一种Unscented粒子滤波(Unscented Particle Filter, UPF)算法。该算法利用UT变换得到粒子状态均值和方差，以近似正态分布N(,p)作为采样的重要性密度函数，从重要性函数中抽取随机粒子，由UKF的Sigma点构成粒子集，使得这些采样粒子包含了新的量测信息，从而提高了滤波性能。UPF算法克服了标准粒子滤波不考虑最新量测信息的缺点[1-2]。但UPF算法依然存在粒子退化问题，当粒子数分布不合理也会减少粒子的多样性。

粒子数量是决定粒子滤波效率和精度的关键因素之一，同时也与计算复杂度和收敛速度密不可分。因此，在保证一定滤波精度的条件下，降低滤波过程使用的粒子数，就能有效降低滤波的时间复杂度，但是无限制地减少粒子数有可能造成粒子集表征后验概率密度真实分布的能力退化，从而使得滤波精确程度大幅降低。对标准的粒子滤波算法而言，其样本数是固定不变的，不变的样本容量直接影响计算复杂度，增加了算法计算负担，使得算法的实时性和精度变差。为了减少计算量，使得粒子滤波能用于实时数据处理，2002年 D Fox提出了粒子个数可变的自适应粒子滤波，即基于Kullback-Leibler（KL）信息数或KL距离（KLD）采样的自适应粒子滤波器[3]。该方法通过粒子数的自适应变化来保证后验密度的真值与估计值之间的误差限，这种误差限用 KLD表示。在概率密度集中在状态空间的小范围(状态分布不确定性较小)时采用少量粒子数目，反之则采用较多粒子。但是，考虑到实际系统应用的硬件条件，该方法的计算负荷过高。Torma等学者提出了基于似然分布自适应调整的粒子滤波算法[4]，该算法选取先验密度作为重要性密度函数，所需的粒子数应能保证非归一化似然值和超过某一预定的门限，实现简单。但是，该方法没有考虑最新量测信息的影响[5]，权值方差对确定粒子数影响很大，而且还会增强粒子间的相关性，增加了高速并行实现的难度。

针对标准粒子滤波器在较高观测精度的场合下可能导致滤波发散，文献[6]提出了一种基于自调整粒子滤波的组合导航方法，根据观测噪声的统计大小，自适应调整似然分布的形状，使之与先验分布重叠的区域更大，有效提高滤波稳定性。但是，该方法人为地增大了似然分布的影响，虽然增强了粒子滤波的鲁棒性，但在一定程度上影响了估计精度[7]。文献[8]提出了一种用于状态估计的自适应粒子滤波算法，该算法通过UKF构造粒子群，而粒子中的每个Sigma点用自适应强跟踪滤波来更新，可以在线调节因子。但是，随着系统维数的增大，计算量将迅速增加，特别是在系统非线性、非高斯较强时，滤波性能急剧下降，甚至发散。

本文在上述学者研究成果的基础上，提出一种基于似然分布的样本数自适应 UPF(Unscented Particle Filter)算法。该方法以UPF算法为基础，吸收了似然分布自适应和样本数自适应的优点，对粒子数进行自适应调整，利用UT变换获得各粒子的重要性密度函数，将最新量测信息引入到粒子的重要性密度函数设计以及重采样中，观测噪声实时统计性能，通过改进策略自适应调整似然分布的分布状态，使之尾部更为平坦，增加先验和似然的重叠区，有效提高滤波稳定性，从而达到提高算法估计性能的目的。

1 基于似然分布的样本数自适应UPF算法

考虑非线性系统模型如下[7]：

1.1 样本数可自适应调整的粒子滤波

样本数可自适应调整的粒子滤波算法描述如下[7]：

① 参数初始化。

1.2 基于似然分布的样本数自适应UPF算法

基于似然分布的样本数自适应 UPF算法的基本思想是，首先在每一步状态方差估计中规定样本数的下限，同时考虑了状态方差过大和过小的情况，利用UT变换获得各个粒子的重要性密度函数，并将最新的量测信息引入到粒子的重要性密度函数设计以及重采样中，然后在重采样阶段嵌入似然采样，根据反映量测噪声信息实时统计性能的精度因子的大小，自适应地调整似然分布状态，使之尾部更为平坦，增加先验和似然的重叠区，从而提高滤波精度。

基于似然分布的样本数自适应UPF算法步骤如下：

① 初始化

b) 时间更新

在获得新的量测yk后，进行滤波量测更新，得到

2）计算权值

式中，β为自适应系数，由量测噪声的统计特性决定。

d）归一化权值

这一步充分利用了新的量测值来改进粒子采样的精度。

3）均值与方差估计

4）重采样

5）预测样本数

结合当前k的量测值来估计k+1时刻状态误差的方差σ2(k+1)，粒子数的下界表达式如下：

根据式(23)计算样本数量Nk+1，即给定置信区间L和参数α， k时刻的方差，即可得到粒子数的下限。

6）令k=k+1，返回步骤1），计算下一时刻的状态估值，直到仿真时间T结束。

2 仿真实验与分析

2.1 SINS/SAR组合导航系统

将提出的基于似然分布的样本数自适应 UPF (LDSNAUPF)算法应用到 SINS/SAR组合导航系统中，并与 Unscented粒子滤波(UPF)算法进行仿真比较。实验数据来源于某型号运载器在西安郊区的一段飞行试验。系统数学模型见文献[9]。设运载器的初始位置为初始位置为北纬34.24°，东经108.99°，初始高度为800 m，初始速度为90 m/s，航向正北，经过水平、爬升、左右转弯、俯冲等动作，终点位置为北纬37.34°，东经110.19°，高度为1256 m，飞行时间为1800 s。设陀螺常值漂移为0.1 (°)/h，测量白噪声为0.05 (°)/h，随机游走为加速度计零偏为1× 10-4g，随机游走为SAR图像匹配计算时间取为1 s，SAR的水平定位精度为15 m，气压高度表测量误差为10 m。设SINS初始水平对准误差为0.01°，方位对准误差为0.03°，初始速度误差(东向、北向、天向)分别为0.5 m/s、0.3 m/s和0.5 m/s，初始位置误差(经度、纬度、高度)分别为6 m，4 m和5 m，初始姿态误差(航向角、俯仰角、横滚角)分别为0.1°、0.1°和0.2°。选取粒子数为100，进行50次Monte-Carlo仿真。

仿真时间为1800 s。采用标准UP、UPF和提出的LDSNAUPF算法计算得到的定位误差如图1～3所示。

由图1～3可以看到，采用提出的LDSNAUPF算法计算得到的东向、北向和天向位置误差相对最小，滤波后东向位置误差控制在[-5 m, +7 m]，北向位置误差控制在[-6 m, +6 m]，天向位置误差控制在[-5 m, +6 m]，明显优于PF和UPF。

图1 PF定位误差Fig.1 Positioning error of PF

图2 UPF定位误差Fig.2 Positioning error of UPF

图3 提出的LDSNAUPF定位误差Fig.3 Positioning error of LDSNAUPF

2.2 单变量非静态增长模型

采用单变量非静态增长模型[10]，状态模型和观测模型如下：

式中，wk与vk均服从标准正态分布，过程噪声方差为Q=10，量测噪声方差R=1，初始状态取x0=0.1，P0=3。

图4为一次仿真实验中采用三种滤波算法对系统状态估计对比曲线，横坐标表示运行时间，纵坐标表示状态。图5给出了三种滤波算法的RMSE曲线比较。LDSNAUPF算法与PF和UPF相比，均方误差的均值和方差都有所减小，这是因为引入了能反映量测噪声信息统计性能的精度因子α，通过改变α的大小来自适应地调整似然分布状态，从而改变粒子对应权值的分布，增加有用粒子的权值，从而提高算法估计性能。

图4 采用三种滤波算法对系统状态估计对比Fig.4 System state estimations by three filters

图5 三种滤波算法的RMSE曲线Fig.5 Comparison on RMSEs by three filtering algorithms

3 结 论

为了提高组合导航系统的滤波精度，针对粒子滤波算法的实时性较差，提出了一种新的基于似然分布的粒子数自适应UPF算法。该算法在每一步状态方差估计中规定样本数的下限，在重采样阶段嵌入似然采样，根据反映量测噪声实时统计性能的精度因子α的大小，自适应调整似然分布状态，增加先验和似然的重叠区，减少了粒子退化。利用UT变换获得各个粒子的重要性密度函数，并将最新量测信息引入到粒子的重要性密度函数设计以及重采样中，从而达到提高算法估计性能的目的。

将该算法应用到SINS/SAR组合导航系统中进行仿真验证，实验结果表明，提出的算法与PF和UPF算法相比，能有效改善滤波性能，提高滤波精度及稳定性，适用于非线性非高斯系统。该方法虽然增大了似然分布的范围，增强了滤波的鲁棒性，但是在一定程度上增加了计算复杂度，下一步需要解决的是如何降低计算量的问题。

（References）：

[1] 赵琳, 王小旭, 丁继成, 等. 组合导航系统非线性滤波算法综述[J]. 中国惯性技术学报, 2009, 17(1): 46-52. Zhao Lin, Wang Xiao-xu, Ding Ji-cheng, et al. Overview of nonlinear filter methods applied in integrated navigation system[J]. Journal of Chinese Inertial Technology, 2009, 17(1): 46-52.

[2] 高怡, 高社生, 冯志华. 一种新的非线性模型预测UPF算法[J]. 西北工业大学学报, 2012, 30(5): 734-738. Gao Yi, Gao She-sheng, Feng Zhi-hua. Exploring an effective nonlinear model predictive unscented particle filtering (NMPUPF) algorithm[J]. Journal of Northwestern Polytechnical University, 2012, 30(5): 734-738.

[3] Fox D. KLD-sampling: Adaptive particle filters[J]. International Journal of Robotics Research, 2003, 22(12): 985-1003.

[4] Torma P, Szepesvari C. On using likelihood-adjusted proposals in particle filtering: local importance sampling [C]//Proceedings of the 4th International Symposium on Image and Signal Processing and Analysis, 2005: 58-63.

[5] 赵岩, 高社生, 杨一. 自适应SDV-UPF算法及其在紧组合中的应用[J]. 中国惯性技术学报, 2014, 22(1): 83-88. Zhao Yan, Gao She-sheng, Yang Yi. Adaptive SVD-UPF algorithm and application to tightly-coupled integrated navigation[J]. Journal of Chinese Inertial Technology, 2014, 22(1): 83-88.

[6] 崔平远, 郑黎方, 裴福俊. 基于自调整粒子滤波的组合导航方法研究[J]. 计算机工程, 2008, 34(14): 185-187. Cui Ping-yuan, Zheng Li-fang, Pei Fu-jun. Research on integrated navigation system based on self adjust particle filter[J]. Computer Engineering, 2008, 34(14): 185-187.

[7] Doucet A, Johansen A M. A tutorial on particle filtering and smoothing: fifteen years later[M]//Handbook of Nonlinear Filtering. Cambridge University Press, 2009.

[8] 邓小龙, 谢剑英, 郭为忠. 用于状态估计的自适应粒子滤波器研究[J]. 华南理工大学学报, 2006, 34(1): 57-62. Deng Xiao-long, Xie Jian-ying, Guo Wei-zhong. Adaptive particle filtration for state estimation[J]. Journal of South China University of Technology, 2006, 34(1): 57-62.

[9] Zhong Y, Gao S, Li W. A quaternion-based method for SINS/SAR integrated navigation system[J]. IEEE Transactions on Aerospace and Electronic Systems, 2012, 48(1): 514-524.

[10] Gordon N J, Salmond D J, Smith A F M. Novel approach to nonlinear/non-Gaussian Bayesian state estimation[C]// IEEE Proceedings-F, 1993, 140(2): 107-113.

Adaptive sample-size unscented particle filter based on likelihood distribution

GAO Yi1, GAO Ya2, GAO She-sheng2
(1. School of Electronic Engineering, Xian ShiYou University, Xi’an 710065, China; 2. School of Electronic Information Engineering, Xian Technological University, Xi’an 710021, China; 3. Department of Automatics Control, Northwestern Polytechnical University, Xi’an 710072, China)

Aiming at the poor real-time performance of particle filtering and the computation amount’s exponentially increasing with particle numbers, this paper presents an adaptive sample size UPF (unscented particle filtering) algorithm, which takes the advantages of the adaptivities of likelihood distribution and sample number. In the state variance estimation, the lower limit of sample is set at each step, and the cases when state variance is too large or too small are taken into account. In the resampling phase, the likelihood samples are embedded, and the likelihood distribution state is adaptively adjusted based on the precision factor α which can reflect the real-time statistical performance of observational noises to increase the overlapping area of the prior and the likelihood and reduce particle degeneration. In addition, the method uses the unscented transformation to obtain the importance density function of each particle, and introduces the latest observational information to the importance density function and resample, thus effectively improves the estimation performance. By applying the proposed algorithm to the SINS/SAR integrated navigation system, the simulation results and their analysis demonstrate that, compared with the PF and UPF algorithms, the proposed algorithm can effectively improve filter performance and calculation precision.

unscented particle filter; sample number; adaptive filter; likelihood distribution; integrated navigation

TP301.6

：A

2015-06-15；

：2015-09-23

陕西省科技攻关项目（2013k09-18）；西安石油大学青年科技创新基金项目

高怡（1978—），女，博士，讲师，从事控制理论与控制工程。E-mail：gaoyi_nwpu@163.com

1005-6734(2015)05-0648-05

10.13695/j.cnki.12-1222/o3.2015.05.016