强跟踪UKF粒子滤波算法

杨丽华，葛 磊，李保林＋，黄海波

（1.湖北汽车工业学院 经济管理学院，湖北 十堰442002；2.中国航天科工集团第二研究院706所，北京100854；3.湖北汽车工业学院电气与信息工程学院，湖北 十堰442002）

0 引 言

就粒 子 滤 波 （particle filtering，PF）［1－7］本 身 而 言，其存在着缺点，主要为粒子退化问题，尽管通过重采样可以在一定程度上缓解粒子退化，但也由此产生了样本贫化。因此，众多学者通过研究选取重要性密度函数来缓解粒子退化问题，其中较为有效的是基于扩展卡尔曼滤波 （extended kalman filter，EKF）的 扩 展 粒 子 滤 波 （extended particle filter，EPF）和基于无迹卡尔曼滤波 （unscented Kalman filter，UKF）的无迹粒子滤波 （unscented particle filter，UPF）［8］。然而EPF和UPF虽然在一定程度上可以缓解粒子退化问题，但是由于EKF 和UKF 本身的局限，使两种算法对系统模型误差和状态突变的鲁棒性不强，当系统存在模型误差或状态突变时，滤波估计效果不佳，甚至发散。

为了提高PF对系统模型误差和状态突变的鲁棒性，提高滤波器对状态估计的跟踪能力，胡昌华等提出了强跟踪EPF （strong tracking EPF，STEPF）算法［9］，该算法将强跟踪EKF （strong tracking EKF，STEKF）与PF结合，利用STEKF对粒子进行估计，产生重要性密度函数，并更新粒子。由于STEKF对状态具有较强的跟踪能力，鲁棒性较强，故STEPF既避免了粒子退化和样本贫化，又具有高于STEKF的估计精度，同时对系统状态突变具有较强鲁棒性。然而STEKF是基于一阶线性化截断的方法，其估计精度不高，从而影响了STEPF 的估计精度，并且由于STEKF需要计算非线性函数的雅可比矩阵，计算繁琐，容易出错，难以模块化实现，从而影响了STEPF的数值稳定性，也增加了在实际应用中的实现难度。

本文基于STEPF引入STEKF产生重要性密度函数的思 想，提 出 了 将 强 跟 踪 UKF （strong tracking UKF，STUKF）与PF结合的强跟踪UPF （strong tracking UPF，STUPF）算法，该算法利用STUKF产生重要性密度函数，并更新粒子。由于在非线性高斯系统下，UKF估计精度可达非线性函数的三阶泰勒展开，远远高于EKF的一阶，所以STUPF的估计精度高于STEPF，同时STUKF 不必求取计算繁琐的雅可比矩阵，故STUPF具有较高的数值稳定性，也较容易实现。

1 STEPF算法

1.1 STEKF算法

考虑如下所示的非线性离散系统

其中：xk∈Rn、yk∈Rm分别为系统状态向量和量测向量；fk（·）：Rn→Rn、hk（·）：Rn→Rm分别为系统非线性函数和量测函数；wk、vk为互不相关的高斯白噪声，具有如下的统计特性

其中：Qk、Rk都为正定矩阵；δk，j为Kronecker－δ函数。

则STEKF算法的实现步骤如下

其中

STEKF对模型误差和状态突变具有良好的鲁棒性，能以较快的速度跟踪状态变化，具有较强的实用性。

1.2 STEPF算法

STEKF虽然具有良好的鲁棒性，但是由于STEKF 算法本身的局限，其估计精度不高，只能达到泰勒展开的一阶精度，在某些情况下难以满足对状态估计的高精度要求。而将STEKF 与PF 结合的STEPF，则可以在保证STEKF的鲁棒性的同时，提高滤波精度。STEPF 算法的实现步骤如下：

步骤2 已知k＋1时刻的量测值yk＋1，利用STEKF算法对每个粒子进行更新，得到粒子的重要性密度函数，并根据重要性密度函数抽样， 获得相应粒子｛xik＋1i＝1，2，…，N｝及其概率密度值

步骤3 更新权值并归一化

步骤4 对粒子样本｛xik＋1i＝1，2，…，N｝重采样得到新粒子样本｛xik＋1i＝1，2，…，N｝，并求取状态估计

相比于STEKF，STEPF 具有较高的估计精度，同时也克服了粒子退化和样本贫化的问题，并对系统模型误差和状态突变具有较强的鲁棒性，对状态具有较强的跟踪能力。但是由于STEKF的估计精度较低，而STEPF 的粒子更新过程是根据STEKF产生的重要性密度函数，所以也直接影响了STEPF的估计精度，同时由于STEKF需要计算雅可比矩阵，计算较为繁琐，且容易出错，这也影响了STEPF的数值稳定性。同时由于雅可比矩阵的计算难以模块化实现，导致STEPF也存在难以模块化的问题。

因此，寻找一种鲁棒性较强、估计精度更高、可以模块化实现的滤波算法，替代STEKF产生重要性密度函数，成为提高STEPF估计精度、实现方便的关键。而STUKF恰恰满足这些要求，将STUKF与PF结合的STUPF算法，可得到比STEPF估计精度更高，更容易实现的滤波算法。

2 STUPF算法

2.1 STUKF算法

基于非线性系统式 （1）和式 （2）的STUKF 算法可表示如下［10］：

步骤1 已知k 时刻的状态xk的统计特性（＾xk，Pk），根据Sigma采样策略，确定Sigma点ξi，k（i＝0，1，…，L）及其相应权值。

步骤2 计算ξi，k 经非线性函数（1）传播后的点γi，k＋1／k，从而得到一步状态预测及误差协方差阵P（l）k＋1／k

步骤5 求得λk＋1后，利用＾xk＋1／k和Pk＋1／k求 取Sigma点，再 通 过 非 线 性 测 量 函 数 （2）传 播 后 的 点ηi，k＋1／k，由可得引入 渐 消 因 子 后 的 自 协 方 差Pyk＋1和互协方差Pxk＋1yk＋1

步骤6 在获得新的量测后yk＋1，即可求得滤波增益Kk＋1、状态估计值和协方差Pk＋1

由于STUKF算法同样采用了U 变换，故当系统正常时，STUKF与UKF具有相同的估计精度，但是当系统具有模型误差或状态突变时，UKF的滤波增益不会自适应调整，导致状态估计不准确，严重时滤波会发散。而STUKF是基于正交性原理推导出来，通过以强迫输出残差序列正交的方法，自适应的调整滤波增益，所以STUKF 对系统模型误差和状态突变具有良好的鲁棒性，即使当滤波趋于稳定时，STUKF也会通过因状态异常而导致的量测异常，自适应地调整一步预测方差阵，从而达到调整滤波增益、跟踪真实状态的目的。

同时由于STUKF采用逼近精度更高的U 变换来近似状态的后验均值和协方差，所以与STEKF 相比，STUKF克服了STEKF通过一阶线性化截断导致精度偏低的缺点，又避免了计算雅可比矩阵，能提高滤波的数值稳定性，且有利于算法的模块化设计。

2.2 STUPF算法

STUPF算法的设计思想是，利用最新的观测数据，通过STUKF计算粒子重要性密度函数的均值和协方差，并根据重要性密度函数统计特性采样得到新的粒子及相应权值，再通过粒子权值的归一化和对粒子的重采样，再加权求和，完成对状态的估计。其具体实现步骤如下：

步骤1 初始化，设置粒子数目N ，并对N（＾x0，P0）抽样，得到初始粒子集｛i＝1，2，…，N｝，并设粒子的权值皆为1／N。

步骤2 在获得k＋1 时刻的量测值yk＋1后，利用STUKF对每个粒子进行状态估计，得到相应估计值和 协方差Pik＋1／k，并以此作为 粒 子 的 重 要 性 密 度 函 数进行抽样，从而得到新粒子及其概率密度值

步骤3 将粒子权值进行更新，并进行归一化处理

由于STUPF采用STUKF产生重要性密度函数，所以在一定程度上克服了粒子退化和样本贫化的问题，当系统正常时，由于UKF 与STUKF 具有相同的估计精度，故STUPF与UPF也具有相同的估计精度，所以STUPF算法兼具UPF的估计精度高和STUKF对状态突变的强跟踪能力，具有较强的适应性，即使当系统模型失配或发生状态突变时，也可以迅速跟踪状态真实值，完成对状态的估计。同时由于STUKF满足正交性原理，其估计结果是无偏的，而STUPF算法不影响其正交性，故STUPF可以保证估计值的无偏和次优。

与STEPF相比，由于STUKF 估计精度高于STEKF，所以由STUKF对粒子的更新更加集中于状态概率密度函数的高似然区域，故STUPF 的估计值更加精确。同时由于STUKF避免了求取雅可比矩阵，降低了计算复杂度，故增加了STUPF算法的可实现性和数值稳定性，且容易模块化实现。

3 数值仿真

本节分别采用两个非线性模型对STUPF进行仿真，其中例1是粒子滤波算法的标准检验模型，来验证STUPF算法对状态的估计能力，例2 是分时恒定值问题，来验证STUPF对状态突变的强跟踪能力。

例1：标准模型的状态方程和量测方程为

其中，wk和vk分别为均值为0，方差分别为Qk＝10，Rk＝1的高斯白噪声。设x0＝0.1，P0＝10，ρ＝0.95，取粒子数为100，进行100步的迭代估计，分别用UPF、STEPF 和STUPF算法对该模型进行滤波，所得的状态估计曲线和均方误差曲线分别如图1和图2所示。

图1 标准模型的状态估计

图2 标准模型的均方误差

从图1和图2 可以看到，UPF、STEPF 和STUPF 都能对该模型进行估计，其中UPF和STUPF估计效果较好，且精度相当，而STEPF 的均方误差曲线大于UPF 和STUPF，估计精度稍差。UPF 和STUPF 估计精度相当的原因是两种算法的重要性密度函数UKF 和STUKF，当系统没有模型误差或状态突变时，具有相同的估计精度。而STEPF估计精度稍差的原因是采用估计精度低于UKF 和STUKF的STEKF作为重要性密度函数。这也验证了当系统正常时，STUPF和UPF 估计精度相同，且高于STEPF的理论分析。

例2：分时恒定值的状态方程和量测方程为

设T ＝120，Q ＝1，P0＝5，ρ＝0.95，粒子滤波的样本个数为100，进行120 步的滤波计算。分别用UPF、STEPF和STUPF算法对该系统进行滤波，所得的滤波曲线如图3所示。

图3 分时恒定值的状态估计

从图3可以看到，UPF对状态突变完全失去了跟踪能力，其估计值难以跟踪状态真实值，产生较大误差，鲁棒性较差。而STEPF和STUPF对状态突变都具有较强的跟踪能力，都可以在状态突变后迅速跟踪到真实状态，并且估计精度都很高。但是，由于STEPF采用STEKF 作为重要性密度函数，而STEKF的估计精度不高，所以导致在对状态突变时刻进行估计时，STEPF的估计值与真实值的误差较大，行成一个尖刺，影响了状态估计的平滑性，而STUPF则由于估计精度高，避免了这一缺陷，从而验证了STUPF具有较高的估计精度和对状态突变较强的跟踪能力。

4 结束语

STEPF算法对系统模型误差和状态突变具有较强的鲁棒性，但是由于STEPF 以STEKF 作为重要性密度函数，导致其估计精度受到影响，且需要计算雅可比矩阵，增加了实现难度，影响了数值稳定性。本文提出的STUPF算法使用估计精度更高的STUKF 作为重要性密度函数，兼具了UPF估计精度高和STUKF 较强鲁棒性的优点，有效克服了STEPF 所存在的缺点。数值仿真结果表明，STUPF在具有较强的鲁棒性的同时，具有高于STEPF 的估计精度。

［1］YAO Anbang.A compact association of particle filtering and kernel based object tracking ［J］.Pattern Recognition，2012，45 （7）：2584－2597.

［2］YUAN Chen.Particle filtering for blind equalization with unknown noise variance ［J］.Advanced Materials Research，2012，462：10－16.

［3］Orguner U，Gustafsson F.Target tracking with particle filters under signal propagation delays ［J］.IEEE Transactions on Signal Processing，2011，59 （6）：2485－2495.

［4］Closas P，Femandez－Prades C.Bayesian nonlinear filters for direct position estimation ［C］／／IEEE Aerospace Coference，2010：1－12.

［5］ZHANG Miaohui，LIU Xiansheng.Target tracking algorithm based on MCMC unscented particle filter ［J］.Systems Engineering and Electronics，2009，31 （8）：1810－1813 （in Chinese）.［张苗辉，刘先省.基于MCMC 无味粒子滤波的目标跟踪 算 法 ［J］.系 统 工 程 与 电 子 技 术，2009，31 （8）：1810－1813.］

［6］SUN F，TANG LJ.Improved particle filter algorithm for INS／GPS integrated navigation system ［C］／／IEEE International Conference on Mechatronics and Automation， 2011：2392－2396.

［7］Hai H Hoang，Bing W Kwan.Suboptimal particle filtering for MIMO flat fading channel estimation ［J］.International Journal of Communication Systems，2013，26 （3）：356－368.

［8］Seok－Han Lee.Real－time camera tracking using aparticle filter combined with unscented kalman filters ［J］.Journal of Electronic Imaging，2014，23 （1）：1－18.

［9］HU Changhua，ZHANG Qi，QIAO Yukun.A strong tracking particle filter with application to fault prediction ［J］.Acta Automatica Sinica，2008，34 （12）：1522－1528 （in Chinese）.［胡昌华，张琪，乔玉坤.强跟踪粒子滤波算法及其在故障预报中的应用 ［J］.自动化学报，2008，34 （12）：1522－1528.］

［10］WANG Xiaoxu，ZHAO Lin，XIA Quanxi，et al.Strong tracking filter based on Unscented transformation ［J］.Control and Decision，2010，25 （7）：1063－1068 （in Chinese）.［王小旭，赵琳，夏全喜，等.基于Unscented变换的强跟踪滤波器 ［J］.控制与决策，2010，25 （7）：1063－1068.］