用于弹道目标跟踪的有限差分扩展卡尔曼滤波算法

第 42 卷 　第 2 期 2008 年 2 月 　　 　 西 　安 　交 　通 　大 　学 　学 　报 J OU RNAL O F XI′AN J IAO TON G UN IV ERSIT Y Vol. 42 　№2 Feb. 2008 用于弹道目标跟踪的有限差分扩展卡尔曼滤波算法 巫春玲 , 韩崇昭 (西安交通大学电子与信息工程学院 , 710049 , 西安) 摘要 : 针对目前常用的滤波算法不能同时做到精确和高效跟踪目标的缺点 ,提出一种有限差分扩 展卡尔曼滤波 ( FDE KF)算法用于再入阶段的弹道目标跟踪. 该算法应用有限差分运算得到滤波的 验前、验后误差协方差矩阵 ,避免了非线性函数求导运算 ,以及 J acobian 阵和 Hessian 阵的计算 ,降 低了计算难度 ,扩大了应用范围 ,增强了滤波过程的收敛性. Monte Carlo 数值仿真 表 关于同志近三年现实表现材料材料类招标技术评分表图表与交易pdf视力表打印pdf用图表说话 pdf 明 ,FDE KF 算法与扩展卡尔曼滤波 ( E KF) 算法和无味卡尔曼滤波 ( U KF) 算法相比较 ,在跟踪精度上比 E KF 算法提高了约 20 % ,与 U KF 算法相当 ,在计算复杂度上比 E KF 算法稍有增加 ,但比 U KF 算法低 约 39 %. 这说明 FD E KF 算法在计算量增加不多的情况下 ,滤波精度有显著提高. 关键词 : 弹道目标跟踪 ;扩展卡尔曼滤波 ;无味卡尔曼滤波 ;有限差分 中图分类号 : TP391 　文献标志码 : A 　文章编号 : 02532987X(2008) 0220143204 Finite2Difference Extended Kalman Filtering Algorithm for Ball istic Target Tracking WU Chunling , HAN Chongzhao (School of Elect ronics and Information Engineering , Xi′an Jiaotong University , Xi′an 710049 , China) Abstract : In order to overcome t he disadvantage of t he common used filtering algorit hms t hat can not achieve t he t racking accuracy and effectiveness at t he same time , a finite2difference extending Kalman filter algorit hm was proposed for ballistic target t racking problem in t he re2ent ry p hase. This algorit hm uses finite differences to approximate t he priori error covariance mat rix and t he posterior error covariance mat rix , and avoids evaluations of derivatives , t he J acobian and Hessian mat rices , which enlarge t he application areas and imp rove the filtering convergence. The Monte Carlo simulations show t hat , compared wit h t he extended Kalman filter ( E KF) and t he unsented Kalman filter (U KF) , t he t racking accuracy of t he new algorit hm is close to t hat of U KF , but 20 % higher t han t hat of E KF ; t he comp utational complexity of t he new algorit hm is close to t hat of E KF , but 39 % lower than t hat of U KF. All these result s show that t he filtering accuracy of t he p roposed algorit hm is improved evidently with a lit tle increasing in comp utational cost . Keywords : ballistic target t racking ; extended Kalman filter ; unscented Kalman filter ; finite2 difference 　　由于理论和实际应用的原因 ,再入阶段的弹道 目标跟踪问题[1 ] 已引起研究者们的广泛关注. 由于 弹道目标运动方程是非线性的 ,因此需要建立一种 随机的非线性滤波算法. 目前 ,最常用的非线性滤波 算法是扩展卡尔曼滤波 ( E KF)算法[122 ] 、无味卡尔曼 滤波 ( U KF) 算法[324 ] 以及各种粒子滤波 ( PF) 算 法[526 ]等. E KF 算法简单 ,易于实现 ,但当滤波问题 高度非线性或局部线性化假设不成立时 ,E KF 算法 收稿日期 : 2007207206. 　作者简介 : 巫春玲 (1978 - ) ,女 ,博士生 ;韩崇昭 (联系人) ,男 ,教授 ,博士生导师. 　基金项目 : 国家重点基础研究发展计划资助项目 (2001309405) ;国家自然科学基金资助项目 (60574033) . 会引进很大的估计误差 ,导致滤波发散. U KF 算法 采用了确定性抽样的方法 ,相比 E KF 算法来说 ,在 计算量增加不多的情况下 ,滤波精度有很大提高 ,但 当系统维数增加时 ,计算量也会大大增加. PF 算法 在理论上有很高的估计精度 ,但由于运算过程复杂、 耗时多而无法走向实用. 本文提出有限差分扩展卡 尔曼滤波 ( FD E KF) 算法 ,利用有限差分方法[ 728 ] 计 算扩展卡尔曼滤波算法中非线性函数的偏导数运 算 ,避免了 J acobian 阵和 Hessian 阵的计算 ,算法实 现简单 ,计算精度高. 计算机仿真表明 ,本文算法对 再入阶段的弹道目标跟踪有效. 1 　弹道目标跟踪问题 111 　目标运动模型 弹道目标运动的离散时间非线性状态方程为 Xk+1 = Ψ( Xk ) + Wk (1) 式中 : k 是非负整数 ; Xk = [ x k , Ûx k , y k , Ûy k ]T 是状态向 量 ,其中 x k 和 y k 是笛卡儿坐标系 ( x , y) 中目标的位 置 , Ûx k 和 Ûy k 是目标速度 ; { Wk } , k ≥0 是过程噪声序 列 ,与量测噪声和初始状态独立 ,假定过程噪声序列 是 0 均值的高斯白噪声 ,其协方差矩阵为 Q = q E 0 0 E ; E = T3 3 T2 2 T2 2 T (2) 式中 : T 是连续两个雷达量测的时间间隔 ; q 是过程 噪声强度参数. 过程噪声解释了所有模型中没有考 虑到的力的作用 ,以及模型与实际偏离等造成的影 响. 式 (1) 中的函数Ψ( Xk ) 由于空气阻力和地球引 力的作用 ,所以是非线性的 ,表达式为 Ψ( Xk ) = MXk + Gf ( Xk ) + G 0 - g (3) 其中 : g 为重力加速度 ,为已知常数 ;矩阵 M 和 G 分 别为 M = 1 T 0 0 0 1 0 0 0 0 1 T 0 0 0 1 ; 　 G = T2 2 0 T 0 0 T 2 2 0 T (4) f ( Xk ) 为一个非线性函数 ,代表气动阻力的影响 f ( Xk ) = - 015 gβρ( y k ) ( Ûx2k + Ûy2k ) 1/ 2 Ûx kÛy k (5) 式中 :ρ( y k ) 是空气密度 ,随着目标高度的下降按指 数衰减[3 ] . 112 　雷达量测模型 雷达观测量有 2 个 ,斜距 r 和俯仰角ε;斜距和 俯仰角的量测误差标准离差分别为σr 和σε;雷达坐 标始终为 xR = 0 , yR = 0 ;将雷达量测转换到笛卡尔 坐标系下为横坐标 d = rcosε和纵坐标 h = rsinε,因 此量测的线性等式为 Zk = HXk + V k (6) 其中 Zk = [ dk h k ]T ; H = 1 0 0 0 0 0 1 0 式中 : dk 、hk 分别是雷达量测转换到笛卡尔坐标系 下的横坐标和纵坐标 ; H 是观测矩阵 ;V k 是笛卡尔 坐标系中的量测噪声 ,与过程噪声和初始状态独立 , 为 0 均值的高斯白噪声 ,其协方差矩阵[9 ]为 R = σ2d σdh σdh σ2h 它的各个元素分别为 σ2d =σ2r cos2 (ε) + r2σ2ε sin2 (ε) σ2h =σ2r sin2 (ε) + r2σ2ε cos2 (ε) σdh = (σ2r - r2σ2ε) sin (ε) cos (ε) (7) 式中 :σ2d 、σ2h 、σdh分别是笛卡尔坐标系中的量测误差 方差和它们的互协方差. 对于所有的实际应用的目 的 ,这是一个很好的近似 ,大大简化了跟踪算法 ,否 则必须考虑量测等式的非线性. 2 　跟踪滤波算法 211 　扩展卡尔曼滤波算法 对于本文的再入阶段的弹道目标运动模型 ,给 定直到 k 时刻的所有量测对目标的状态估计为 X^ k/ k ,相应的估计误差协方差阵为 Pk/ k ,则扩展卡尔 曼滤波的单步实现流程如下 : (1) 滤波初始化 , k = 0 时刻 ,目标的状态估计为 X^0/ 0 = X^0 ,对应的误差协方差矩阵为 P0/ 0 = P0 ; (2) k + 1 时刻的状态预测和相应的误差协方差 矩阵为 X^k+1/ k = MX^ k/ k + Gf k ( X^k/ k ) + G 0 - g Pk+1/ k = ( M + GF k ) Pk/ k ( M + GF k ) T + Q (8) 式中 : Fk 是非线性函数 f k ( Xk ) 的 J acobian 矩阵 ,取 前一时刻的状态估计 X^k/ k所得到的值 ,即 Fk = [ ¨ f Tk ( Xk) ]T Xk = X^k/ k 441 西　安　交　通　大　学　学　报　　　　　　　　　　　　　　　　　　第 42 卷　 　　(3) 量测预测为 Z^k+1/ k = HX^ k+1/ k (9) 　　(4) 卡尔曼增益为 Kk+1 = Pk+1/ k H T ( HP k+1/ k H T + Rk ) - 1 (10) 　　(5) 在给定直到 k + 1 时刻的所有量测时 ,对应 k + 1 时刻的状态估计及其估计误差协方差阵为 X^k+1/ k+1 = X^k+1/ k + Kk+1 ( Zk+1 - Z^k+1/ k ) Pk+1/ k+1 = ( I - Kk+1 H) Pk+1/ k (11) 212 　Unscented 卡尔曼滤波算法 U nsecnted 卡尔曼滤波 ( U KF) 算法是基于无味 变换 ( U T) 方法[42 5 ] 发展起来的. U KF 使用真实的 非线性状态模型或量测等式来近似状态向量的概率 密度函数. 这个密度仍旧是高斯的 ,是通过一套确 定性抽样的粒子点 ( Sigma 点) 给定的. Sigma 点完 全捕获了高斯密度的均值和协方差 ,当这些 Sigma 点通过非线性系统传播时 ,捕获到后验的均值和协 方差 ,对任何非线性 ,此准确度可达到二阶. U KF 的单步实现流程如下 : 　　(1) 滤波初始化方法同 E KF ; (2) 计算 2 nX + 1 个 Sigma 点χi 和相应的标量 权重 W i ,其中 nX 为状态维数 W 0 = α ( nX +α) (12) W i = 1[2 ( nX + 1) ] 　 i = 1 , ⋯,2 nX (13) χk = [ X^k/ k , X^k/ k + ( ( ( nX +τ) Pk/ k ) 1/ 2 ) i , X^k/ k - ( ( ( nX +τ) Pk/ k ) 1/ 2 ) i ] (14) 式中 ( ( ( nX +τ) Pk/ k ) 1/ 2 ) i 是平方根矩阵 ( ( nX + τ) Pk/ k ) 1/ 2的第 i 行或列 ,α是一个标量参数 ; (3) 使用非线性状态等式 (1) 传播这些 sigma 点 χk+1/ k = Ψ(χk ) (15) 　　(4) 通过对 sigma 点χk + 1/ k和权重 W i 的加权求 和 ,得到预测状态的均值和协方差阵 X^k+1/ k = ∑ 2 nX i = 0 W iχk+1/ k ( i) Pk+1/ k = Q + ∑ 2 nX i = 0 W i (χk+1/ k ( i) - X^ k+1/ k ) · (χk+1/ k ( i) - X^k+1/ k ) T (16) 　　(5) 预测量测 sigma 点为 ξk+1/ k = Hχk+1/ k (17) 　　(6) 预测量测及其协方差阵为 Z^k+1/ k = ∑ 2 nX i = 0 W iξk+1/ k ( i) PZZ = R + ∑ 2 nX i = 0 W i (ξk+1/ k ( i) - Z^k+1/ k ) · (ξk+1/ k ( i) - Z^k+1/ k ) T PX Z = ∑ 2 nX i = 0 W i (χk+1/ k ( i) - X^k+1/ k ) · (ξk+1/ k ( i) - Z^k+1/ k ) T (18) 式中 : PZZ和 P X Z分别是预测量测的误差协方差矩阵 和量测与状态变量的互协方差矩阵. (7) U KF 增益矩阵及 k + 1 时刻的估计值及其 估计误差协方差阵为 Kk+1 = PX Z P - 1ZZ X^k+1/ k+1 = X^k+1/ k + Kk+1 ( Zk+1 - Z^k+1/ k) Pk+1/ k+1 = Pk+1/ k - Kk+1 P- 1ZZ KTk+1 (19) 213 　有限差分扩展卡尔曼滤波算法 有限差分的思想最早由 Schei[7 ]提出 ,该算法的 理论基础是 ,采用多项式近似技术和一阶中心差分 法计算非线性函数的偏导数 , 它具有二阶非线性近 似的能力. 设非线性函数 y = f ( x) ,则其在 x = x 处 的二阶有限中心差分展开为 f ( x) ≈ f ( Šx) + f′DD ( Šx) ( x - Šx) + f″DD ( Šx)2 ! ( x - Šx) 2 (20) f′DD ( Šx) = f ( Šx + h) - f ( Šx - h)2 h f″DD ( Šx) = f ( Šx + h) + f ( Šx - h) - 2 f ( Šx)h2 (21) 式中 : h 是区间长度参数 ; f ( x) 为 f ( x) 在 x = x 处的 取值 ; f′DD ( x) 为展开式一阶项的导数 ; f″DD ( x)2 ! 为二 阶项的系数 ;下标 DD 代表用差分的方法求解得到 的系数. 式 (20) 用 Taylor 级数展开后为 f ( x) = f ( Šx) + f′( Šx) ( x - Šx) + f″( Šx)2 ! ( x - Šx) 2 + f (3) ( Šx) 3 ! h 2 + f (5) ( Šx) 5 ! h 4 + ⋯ ( x - Šx) + f (4) ( Šx) 4 ! h 2 + f (6) ( Šx) 6 ! h 4 + ⋯ ( x - Šx) 2 (22) 　　比较式 (21) 、式 (22) 可以看出 ,式 (22) 右边的前 3 项与二阶 Taylor 展开式相同 , 后 2 项对应高阶 项 ,其精度由 h 控制. 显然 ,用中心差分代替一阶、 二阶导数得到展开式的精度高于一般二阶 Taylor 级数的精度 ,而且适用于不同非线性函数. 有限差分 扩展卡尔曼滤波的单步实现流程如下. (1) 滤波初始化方法同 E KF ,引进如下 4 个矩 阵的 Cholesky 分解 541　第 2 期　　　　　　　　　　　　　　巫春玲 ,等 :用于弹道目标跟踪的有限差分扩展卡尔曼滤波算法 Pk/ k = S^ X S^ X T ; 　Pk+1/ k = …S X …S X T Q = SW S TW ; 　R = SV S TV 　　(2) 状态一步预测 X^k+1/ k = Ψ( X^k/ k ) = MX^ k/ k + Gf k ( X^k/ k ) + G 0 - g (23) 　　(3) 预测误差协方差矩阵 首先运用一阶中心差分法近似计算预测误差协 方差矩阵的 Cholesky 分解为…S X ( i , j) = Ψi ( X^k/ k + hS^ X , j ) - Ψi ( X^k/ k - hS^ X , j )2 h j = 1 ,2 , ⋯, nX Pk+1/ k = …S X …S TX + Q (24) 式中 : h 是步长的调节系数 ,近似设定 h = 3 12 (对高 斯分布 ,它是最优的) . 　　(4) 量测预测为 Z^k+1/ k = HX^ k+1/ k (25) 　　(5) 滤波增益为 K = Pk+1/ k H T ( HP k+1/ k H T + Rk ) - 1 (26) 　　(6) 滤波估计和估计误差协方差阵 X^k+1/ k+1 = X^k+1/ k + K( Zk+1 - Z^k+1/ k ) Pk+1/ k+1 = ( I - KH) Pk+1/ k (27) 　　(7) 估计误差的 Choleskey 分解为 S^ X ( k + 1/ k + 1) = { chol ( Pk+1/ k+1 ) } T (28) 3 　仿真实验 设定扫描周期 T = 2 s ,雷达量测的伪距误差标 准离差σr = 100 m , 俯仰角误差标准离差σε = 0105 rad ,目标弹道系数β= 40 000 kg/ ( m ·s2 ) . 初始状 态 X0 是高斯随机向量 , 其均值 m0 = [232 000 , 2 290cos (190°) , 88 000 ,2 290sin (190°) ] ,位置的单 位为 m ,速度单位为 m/ s ;方差 CX0 = diag ( [ 1 0002 　202 　1 0002 　202 ]) ,仿真目标跟踪步长为 120 , 运行 100 次蒙特卡罗仿真. 假定目标检测概率为 1 , 雷达虚警概率为 0 . 将本文的 FDE KF 算法分别与 E KF 和 U KF 算 法相比较 ,它们的位置和速度的均方根误差 (分别表 示为 EP 和 EV )如图 1、图 2 所示. 位置与速度的均 方根误差的平均值 (分别表示为 ŠEP 和 ŠEV ) 及计算 时间的平均值 (表示为 €t) 如表 1 所示. 由图 1 和图 2 可以看出 , FD E KF 算法的位置均方根误差和速度 均方根误差均与 U KF 算法相当 ,比 E KF 算法的 小 ,表明 FDE KF 算法的跟踪精度接近于 U KF 算 法 ,比 E KF 算法的跟踪精度高. 从表 1 也可以看 出 ,在位置和速度的均方根误差上 , FD E KF 算法比 E KF 算法约有 25 %的减小 , 而 100 次蒙特卡罗仿 真的平均计算时间两者则比较接近. 表 1 　3 种算法的平均跟踪误差及 平均计算时间比较 滤波算法 ŠEP / m ŠEV / m ·s - 1 €t/ s EKF 511331 7 51194 7 01030 7 U KF 401443 1 31863 0 01070 2 FDEKF 411167 5 31989 9 01038 7 图 1 　3 种算法的位置均方根误差比较 图 2 　3 种算法的速度均方根误差比较 4 　结束语 研究了二维再入阶段的弹道目标跟踪问题 ,比 较了 E KF、U KF 和 FDE KF 3 种次优非线性滤波算 法. 针对本文的非线性估计问题 ,从跟踪准确性、计 算复杂度等方面比较了它们的性能. 仿真结果表 明 : FD E KF 算法在跟踪准确性上接近于 U KF 算 法 ,比 E KF 算法约提高约 25 % ;在计算复杂性上 FDE KF 算法比 E KF 算法稍大 ,几乎接近 ,比 U KF 约低 39 % ,表明 FDE KF 算法是研究再入阶段弹道 目标状态估计的有效算法. (下转第 242 页) 641 西　安　交　通　大　学　学　报　　　　　　　　　　　　　　　　　　第 42 卷　 318 阶. 根据分数阶稳定性理论 , 设计 领导形象设计圆作业设计ao工艺污水处理厂设计附属工程施工组织设计清扫机器人结构设计 了一种改进的 状态观测器 ,采用解析的方法求得广义同步的响应 系统 ,从理论上证明了该方法的可行性. 利用该方法 实现了超混沌 Chen 系统的广义同步 ,数值仿真结 果证实了该方法的有效性. 另外 ,该同步方法无需计 算系统同步时的条件 L yap unov 指数 ,简单可靠 ,易 于工程应用. 参考文献 : [1 ] 　HARTL Y T T , LORENZO C F , QAMMER H K. Chaos in a f ractional order Chua’s system [J ] . IEEE Trans CAS : I ,1995 ,42 (8) :4852490. [ 2 ] 　L U J unguo , CH EN Guanrong. A note on the f raction2 al2order Chen system [J ] . Chaos , Solitons and Frac2 tals , 2006 ,27 (3) : 6852688. [3 ] 　L I Chunguang , CH EN Guanrong. Chaos and hyper2 chaos in the f ractional2order RÊssler equations [ J ] . Physica : A , 2004 , 341 (1) :55261. [4 ] 　PECORA L M , CARROLL T L . Synchronization in chaotic systems [J ] . Phys Rev Lett , 1990 , 64 ( 8) : 8212824. [5 ] 　L I Chunguang , L IAO Xiaofeng , YU J uebang. Syn2 chronization of f ractional order chaotic system [ J ] . Phys Rev : E , 2003 , 68 (6) :123. [6 ] 　L U J unguo. Synchronization of a class of f ractional2 order chaotic system via a scalar t ransmitted signal [J ] . Chaos , Solitons and Fractals , 2006 , 27 (2) : 5192 525. [7 ] 　逯俊杰 ,刘崇新 ,张作鹏 ,等. 基于状态观测器的分数 阶统一混沌系统的同步控制 [J ] . 西安交通大学学 报 ,2007 ,41 (4) :4972500. 　　 L U J unjie , L IU Chongxin , ZHAN G Zuopeng , et al. State2observer based synchronization control between f ractional2order unified chaotic systems [J ] . Journal of Xi′an Jiaotong University , 2007 , 41 (4) :4972500. [8 ] 　RUL KOV N F , SUSHCHIK M M , TSIMRIN G L S , et al. Generalized synchronization of chaos in direc2 tionally coupled chaotic system [ J ] . Phys Rev : E , 1995 , 51 (2) :9802995. [9 ] 　CHAREL A , SUN H H , TSAO Y Y , et al. Fractal system as represented by singularity function [ J ] . IEEE Trans Auto Contr , 1992 ,37 (9) :146521470. [10 ] DIET H EL M K , FORD N J , FREED A D. A predic2 tor2corrector approach for the numerical solution of f ractional differential equations [ J ] . Nonlinear Dyn , 2002 , 29 (1) :3222. [11 ] L I Yuxia , TAN G W K S , CH EN Guanrong. Genera2 ting hyperchaos via state feedback control [J ] . Int J Bifurc Chaos ,2005 , 15 (10) : 336723375. [ 12 ] MA TIGNON D. Stability result s of f ractional differen2 tial equations with applications to control processing [ M ]. Lille , France : IMACS , IEEE2SMC , 1996 :9632 968. (编辑 　杜秀杰) (上接第 146 页) 参考文献 ; [1 ] 　L I Xiaorong , J IL KOV V P. A survey of maneuvering target t racking : Part II : ballistic target models [ C ] ∥ Proc 2001 SPIE Conf on Signal and Data Processing of Small Target s. San Diego , USA : The International So2 ciety for Optical Engineering , 2001 : 5592581. [2 ] 　COSTA P. Adaptive model architecture and extended Kalman2Bucy filters [J ] . IEEE Trans Aerospace and Elect ronic Systems , 1994 , 30 (2) :5252533. [3 ] 　FARINA A , BENV ENU TI D , RISTIC B. Tracking a ballistic target : comparison of several nonlinear filters [J ] . IEEE Trans Aerospace and Elect ronic Systems , 2002 , 38 (3) : 8542867. [ 4 ] 　J UL IER S J , U HL MANN J , DU RRAN T2W H YTE H F. A new method for nonlinear t ransformation of means and covariances in filters and estimatiors [J ] . IEEE Trans Automatic Control , 2000 , 45 ( 3) : 4772 482.[5 ] 　J UL IER S , U HL MANN J . Unscented filtering andnonlinear estimation [ J ] . Proceedings of the IEEE ,2004 ,92 (3) : 4012422.[ 6 ] 　BRUNO M G S , PAVLOV A. A density2assisted par2ticle filter algorithm for target t racking with unknownballistic coefficient [ C ] ∥Proc ICASSP205. Piscat2away ,USA : IEEE , 2005 : 528.[7 ] 　SCH EI T S. A finite2difference method for lineariza2tion in nonlinear estimation algorithm [J ] . Automati2ca ,1997 ,33 (11) : 205322058.[8 ] 　ITO K , XION G K. Gaussian filters for nonlinear filte2ring problems [J ] . Automatica , 2000 ,45 (5) :9102927.[9 ] 　FARINA A , STUDER F A. Radar data processing[ M ] ∥Int roduction and Tracking : Ⅰ. New York : Re2searches Studies Press , 1985. (编辑 　刘杨) 242 西　安　交　通　大　学　学　报　　　　　　　　　　　　　　　　　　第 42 卷