修正的强跟踪有限差分滤波算法

巫春玲，巨永锋，段晨东，刘盼芝

（长安大学 陕西 西安 710064）

对于再入阶段的弹道目标跟踪问题，由于目标运动模型内在的非线性，需要使用非线性的滤波技术来估计目标的状态。文献[1-3]等使用扩展卡尔曼滤波（EKF）[3-4]，无味卡尔曼滤波（UKF）[5]，粒子滤波（PF）[3]等算法来跟踪弹道目标。 EKF算法虽然简单，但其需要计算Jacobia矩阵和/或Hessians矩阵；UKF效果较好，但对高维情况，也较复杂；PF算法精度很高，但计算量相当大；由于再入弹道目标速度很快，留待跟踪的时间很短，所以应该采用计算量小又精度高的算法。本文提出一种强跟踪有限差分近似[6-7]的滤波算法来跟踪再入阶段的弹道目标。强跟踪滤波[8]具有较强的鲁棒性，极强的跟踪能力，适中的计算复杂性。在有限差分扩展卡尔曼滤波算法中，引入强跟踪的次优渐消因子，从而得到本文的强跟踪有限差分扩展卡尔曼滤波算法（STFDEKF），仿真实验表明新算法是比较有效的针对再入弹道目标跟踪的滤波算法。

1 弹道目标动力学模型和雷达量测模型

1.1 目标运动模型

所用模型为一再入弹道目标的跟踪问题，参见文献[3]。设k是一个非负整数，T是连续两个雷达量测的时间间隔。为简单起见，假设地球是平的，通过以下离散时间非线性动态系统状态等式定义目标运动

式中：xk为k时刻的目标状态向量；ψk为状态转移函数；β为目标弹道系数；wk为过程噪声。

假定过程噪声序列是零均值高斯白噪声的，协方差矩阵给定如下：

其中，q是过程噪声强度参数。过程噪声说明了模型中没有考虑到的所有力以及模型与现实的偏离的影响。

式（1）中，非线性函数 ψk（xk，β）给定如下：

其中，g=9.8 m/s2是引力加速度。矩阵Φ和G分别为：

f（xk，β）代表气动阻力的影响，其表达式为：

其中，ρ（y）是空气密度函数，满足：ρ（y）=1.21907·e－y/9146.64

1.2 雷达量测模型

雷达观测量有两个，斜距r和俯仰角ε；斜距和俯仰角的量测误差标准离差分别为 σr（伪距）和 σε（俯仰角）；雷达坐标始终为xR=0，yR=0；将雷达量测转换到笛卡尔坐标系下为横坐标d=r cosε和纵坐标h=r sinε，因此量测的线性等式为

其中，

另外，dk，hk分别是雷达量测转换到笛卡尔坐标系下的横坐标和纵坐标；Hk是观测矩阵；vk是笛卡尔坐标系中的量测噪声，与过程噪声和初始状态独立，为零均值的高斯白噪声，其协方差矩阵为Rk。

2 修正的强跟踪滤波器

在原强跟踪滤波器（STF）[8]基础上，做出以下修正：

第一，增加状态噪声协方差的权重

在P（k+1|k）的递推计算过程中，增加协方差 Q（k）的权重。这样的话，P（k+1|k）可如下定义：

第二，为了使状态的估计更光滑，采用平方根函数的特性，那么渐消因子的计算被修正为

其中，

其中，V0（k+1）是残差方差矩阵，如下计算：

其中，r（k+1）为残差，r（1）是初始残差，tr[·]表示对矩阵求秩运算。ρ为遗忘因子，其范围是0＜ρ≤1，引入遗忘因子的目的是为了进一步对过去的老数据渐消从而突出最新残差向量的影响，进一步提高强跟踪滤波器的快速跟踪能力，通常选择ρ=0.95。β是弱化因子，用来改进状态估计的光滑性。这个值通常通过计算机仿真，凭经验决定。

第三，保证估计误差协方差矩阵的对称性和半正定性，改用以下的数值上更加鲁棒的等式来递推：

通过以上修正，强跟踪滤波器的数值特性及滤波精度在一定程度上得以改进。

3 强跟踪有限差分扩展卡尔曼滤波器

强跟踪滤波具有较强的鲁棒性，极强的跟踪能力，适中的计算复杂性。在有限差分扩展卡尔曼滤波算法中，引入以上修正的强跟踪次优渐消因子，从而得到本文的强跟踪有限差分扩展卡尔曼滤波算法（STFDEKF）。具体如下：

考虑如下的非线性系统状态方程和量测方程：

其中，xk和yk分别表示k时刻的状态向量和观测向量；uk为控制输入向量；wk和vk分别表示满足某种分布的状态噪声和观测噪声；并且通常假定噪声序列{wk}和{vk}是相互独立的。状态噪声和观测噪声假定是不相关的白噪声过程，均值和方差分别为：

则在FDEKF算法基础上，在预测误差协方差矩阵中引入上述修正的强跟踪算法，得到STFDEKF算法如下：

1）引入以下Cholesky分解：

2）得到如下4个矩阵

3）计算状态一步预测和预测误差协方差矩阵

λ（k+1）为前述被修正了的强跟踪的渐消因子。

4）量测预测，增益，状态估计及估计误差协方差矩阵

接下来，通过仿真实验对提出的算法进行性能分析。

4 实验及结果分析

4.1 实验场景

这一部分，通过Monte Carlo仿真比较了3种算法的性能。仿真场景设为：扫描周期T=2 s，过程噪声强度为q=1，雷达量测的伪距误差标准离差为σr=200 m，俯仰角误差标准离差为 σε=0.05 rad，目标弹道系数设为已知，β=40 000 kgm-1s-2。初始状态x0为高斯随机向量，其均值和方差已知，均值为m0=[232000 2290cos（190°） 88000 2290sin（190°）]，其中，距离的单位为m，速度的单位为m/s；方差为∑=diag（[1000220210002202]），仿真目标跟踪步长为 120步，运行 500次 Monte Carlo仿真，并假定目标检测概率为1，虚警概率为0。

4.2 实验结果

比较的标准为各个算法的位置均方根误差，速度均方根误差，滤波可靠性，以及各算法的执行时间等。

用平均标准化估计误差平方 （Average Normalized Estimation Error Square，ANEES）比较 3种算法的滤波可靠性。ANEES的定义如下[3]：

式中：n——状态维数；M——Monte-Carlo仿真次数；xi——第i次Monte Carlo运行时的真实状态；x^i——第i次Monte Carlo运行时的状态估计；Pi——第i次Monte Carlo运行时的状态估计误差协方差矩阵。

ANEES曲线越接近于1，表示滤波可靠性越高。

下面，将STFDEKF算法分别与FDEKF、EKF和UKF算法相比较，比较的准则是均方根误差、滤波可靠性及计算复杂性。然后在图1至图4中给出各算法的位置和速度均方根误差及滤波可靠性比较，最后在表1中给出各算法的计算复杂性比较。

图1 位置均方根误差Fig.1 Positon RMSE

在图1至图4中，图1是对图的尾部的放大，以便可以清楚地看到各个算法的滤波误差曲线。通过观察，可以看出，FDEKF算法无论是在位置RMSE还是速度RMSE上，均很明显的比EKF的小，其跟踪精度明显高于EKF的精度。另外，其滤波可靠性也明显提高。而STFDEKF的估计精度与UKF很接近，而且其可靠性明显高于FDEKF和EKF，与UKF很接近。

图2 位置均方根误差（对图尾部的放大）Fig.2 Positon RMSE（enlarge Fig.1）

图3 速度均方根误差Fig.3 Velocity RMSE

图4 ANEES比较Fig.4 ANEEScomparision

在计算复杂性上，各种算法的500次Monte Carlo仿真的计算时间如表 1所示。从表 1中可见，FDEKF和EKF的计算量相当，STFDEKF算法计算量适中，而UKF耗时最多。

5 结束语

在扩展卡尔曼滤波器基础上，结合有限差分及强跟踪滤波的优点，提出一种改进的强跟踪有限差分扩展卡尔曼滤波算法用于再入阶段的弹道目标跟踪。算法采用有限差分运算进行非线性函数的近似，避免了非线性函数的求导运算。使得算法实现简单。另外，算法中引入了强跟踪的次优渐消因子，可以实时调整状态预测误差协方差矩阵。算法改进了跟踪精度，扩大了应用范围，增强了滤波收敛性。仿真实验就估计精度，滤波可靠性及计算复杂性等方面对新算法与FDEKF、EKF和UKF算法进行了比较，结果表明，STFDEKF算法计算量适中，而且精度较高，对于再入阶段的弹道目标跟踪是个很好的选择。

表1 500次Monte Carlo仿真的计算时间比较Tab.1 Computation Time Comparison of 500 Monte Carlo simulation

[1]Bar－Shalom Y，Li X R，Kirubarajan T.Estimation with Applications to Tracking and Navigation:Theory， Algorithms，and Software[M].Wiley， New York，2001.

[2]Li X R，Jilkov V P.A Survey of Maneuvering Target Tracking.Part II:Ballistic Target Models[C]//In Proc.2001 SPIE Conf.on Signal and Data Processing of Small Targets，San Diego， CA， USA， July－Aug.2001，4473:559－581.

[3]Farina， D.Benvenuti， B.Ristic.Tracking a ballistic target:comparison of several nonlinear filters[J].IEEE Trans.Aerospace andElectronic Systems，2002， AES－38:854－867.

[4]Costa P.Adaptive model architecture and extended Kalman－Bucy filters[J].IEEE Trans.Aerospace and Electronic Systems，1994 AES－30:525－533.

[5]Julier S J，Uhlmann J.Unscented Filtering and Nonlinear Estimation.Proceedings of the IEEE，2004，92（3）:401－422.

[6]Schei T S.A Finite－Difference Method for Linearization in Nonlinear Estimation Algorithms[J].Automatica，1997，33（11）:2053－2058.

[7]R.van der Merwe，E.A.Wan.Efficient derivative －free Kalman filters for online learning[C]//in Proc.Eur.Symp.on Artificial Neural Networks （ESANN）， Bruges， Belgium ～Apr.2001.

[8]Zhou D H，Frank P M.Strong tracking Kalman filtering of nonlinear time－varying stochastic systems with coloured noise:Application to parameter estimation and empirical robustness analysis[J].Int.J.Control，1996，65（5）:295－307.