文章编号:1671 - 4598(2019)10 - 0254 - 05
Computer Measurement & Control设计与应用DOI：10. 16526/j. cnki. 11-4762/tp. 2019. 10.052 中图分类号:TN911 文献标识码：A基于粗大误差检测和补偿的改进型EKF
动态目标跟踪算法馳 迪，衲正任，创祛建，宋志亮(温州大学电气数字化设计技术国家地方联合工程实验室，浙江温州325035)摘要：卡尔曼滤波作为当前动态目标跟踪中的常用滤波算法，研究其动态跟踪的准确性对于军事制导，交通导航等领域具有
重大意义；针对动态系统目标跟踪观测过程中存在的坏值、静差和漂移3种粗大误差，基于传统扩展卡尔曼滤波(Extended Kal-
man Filter, EKF)算法框架，引入了一种粗大误差检测和补偿方法，实现了对动态系统观测值中粗大误差的准确辨识和优化补
偿，使得扩展卡尔曼滤波能够结合粗大误差检测和补偿方法有效地排除观测值中的粗大误差，滤波后的状态估计值更加准确地逼
近真实值；经过仿真实验和对比，提出的改进型EKF算法能有效地排除粗大误差观测值对状态预测过程的影响，并且实现了对 动态系统目标的准确跟踪，这大大提高了动态目标跟踪的精确度。关键词：动态目标跟踪；粗大误差；扩展卡尔曼滤波Gross Error Detection and Compensation Based Modified EKF Algorithm for
Dynamic Target TrackingZhang Di, Zhang Zhengjiang, Hu Guiting, Zhu Zhiliang(National—Local Joint Engineering Laboratory for Digitalize Electrical Design Technology, Wenzhou University,
Wenzhou 325035, China)Abstract: Kalman filtering is a commonly used filtering algorithm in current dynamic target tracking. It is of great significance to
study its dynamic tracking accuracy for military guidance, traffic navigation and other fields. Three kinds of gross errors including outli
ers, biases and drifts, are considered in the observations of dynamic target tracking system. Based on the traditional Extended Kalman Filter (EKF) framework, a methodology of gross error detection and compensation realizes the accurate identification and compensation of the gross errors in the observations. So that the modified EKF can effectively eliminate the gross errors in the observations by combining the system equations and the results of state estimation is closer to the true values. After simulations and comparisons, the modified EKF
algorithm achieves accurate tracking of dynamic targets by effectively eliminating the influence under the condition of different types of gross errors in the observations. It can greatly improve the accuracy of dynamic target tracking.Keywords: dynamic target tracking； gross error； extended Kalman filter0引言卡尔曼滤波理论是目前最重要的最优估计理论之
对非线性系统提出了扩展卡尔曼滤波算法(Extended Kal
man Filter, EKF)MO扩展卡尔曼滤波的思想是基于非线性
一，因其目标跟踪算法实用性强，滤波效率高，状态估计 结果较为优化，在寻的制导、路径识别、视频监控等领域
方程的线性化。扩展卡尔曼滤波状态估计的精确度与系统 方程非线性程度密切相关。系统的非线性程度越小，滤波 结果越精确。得到广泛应用。卡尔曼滤波算法在线性系统中为最优估计 算法，但工业中实际的系统往往为非线性系统，因此，针目前，国内外学者基于扩展卡尔曼滤波解决了很多工
业中的工程问题。赵佳等为解决无人飞行器姿态传感器低 成本高精度的要求，引入扩展卡尔曼滤波算法，有效地消
收稿日期：2019-03-27；修回日期：2019 -04 -28。除了无人机飞行器运动过程对姿态解算精度的影响陈 清华等针对锂电池剩余电量(SOC)无法精确估算的问题，
基金项目：国家自然科学基金项目(61703309)；浙江省自然科 学基金项目(LY18F030014 \"浙江省科技计划项目
(LGG18F040002)。作者简介：张迪(1998 男，浙江湖州人，主要从事状态估计
提岀了一种基于改进Thevenin模型的EKF算法询，实现了
锂电池SOC的精确估算。上述扩展卡尔曼滤波算法是基于动态目标跟踪过程中 观测值只含随机误差的情况，但是实际观测值中难免会出 现各种粗大误差(gross errors)™,比如坏值(outlier),静
方向的研究。张正江(1982 -),男，江西乐平人，副教授，硕士研究生导师，主 要从事状态估计、系统建模与控制方向的研究。
第10期张迪.等：基于粗大误差检测和补偿的改进型EKF动态目标跟踪算法• 255 •差(bias)和漂移(drift)等何。受到粗大误差的影响，动 态目标跟踪的滤波状态容易岀现异常，这直接影响状态估 计的准确性。本文提出一种基于粗大误差观测值检测和补
曼滤波递推方程：衣(人 + 1«) = _/■(*«\"))
p« + l M)= FG + llQPd\")FT(A + 1M)+Q(A + 1)
(11)(12)偿的改进型EKF动态目标跟踪算法，通过对粗大误差观测 值的精确检测和优化分类补偿，弥补了传统的EKF算法对
KQ + 1) = P(k + l\\k')HT(k+l\\k'>于实际应用中的不足。实验仿真结果表明，改进型EKF算 [H(i 4- 1)P(A + 11 «HT(i +1) +R(* + 1)P (13)Xd + 1 M + l) = x« + l M)+K(A + 1)[Z(A + 1) —/i(X(R + l \"))]
(14)法能够实现粗大误差观测值下的动态系统目标跟踪，且具 有较好的准确性。1扩展卡尔曼滤波算法非线性离散系统的动态方程如下：XQ+1) = K>,XO)] + GQ)W(&)
PQ + l) = [I_K4 + 1)HQ + 1)：]PQ + 1 |A) (15)
式中，P(& + 1 «)表示根据上一个k时刻的协方差值来估计
(1)人+ 1时刻的协方差值。PQ + 1)表示人+ 1时刻更新的协方
ZO) =(2)差值。式(1)为系统状态方程，式(2)为观测方程。X。) 是\"维系统状态向量;Z«)是m维观测向量和
2观测值的粗大误差2.1粗大误差的类型是X(A)和ZQ)对应的非线性系统函数；G4)
是系统噪声驱动矩阵；WU)则是零均值系统白噪声矩阵;
V(k)是零均值观测白噪声，且｛W(Q｝和｛▼(&)｝互不相
观测数据中通常出现的粗大误差大致有3种，分别是 坏值(outlier),静差(bias)和漂移(drift),如图1所示。关。将式(1)中XU + 1)做一阶泰勒展开，得：XQ + 1) ~
° ?攵、[XW — &(厂]+d A\\k)G[衣(A),RW(&) = F(& + 1|A)X(A)+G(A)+Wd)+9(A)
FG + 1 |Q = q 签、|
O AkR) I(3)(4)X(*)
卩(&) = /•[衣W]— 3 %、I
d A IQ | x⑴= Xd)(5)X(k)表示状态估计值，XCk')-X(.k')表示k时刻的估
计值与状态值的差值.FCk + l\\k)是状态转移矩阵，是根据
k时刻的最优状态值来估计A + 1时刻的状态值。(c)drift将式(2)中的Zd)做一阶泰勒展开，得：d A^R)图1存在粗大误差情况下的观测值坏值：也称异常值或离群值，如图1 (a)所示，坏值
H-V(^)
(6)V(k) = H(.k)X(k)
是观测数据中明显偏离其他观测数据的异常观测值，通常
H(k)=yw =
(7)Z ⑻情况下为个别观测数据。假设第»个观测变量在念时刻出
现了坏值.那么总时存在坏值的观测值可以被记为式(16) 的形式。Z(6)=,X(虽)]+
+ V(^i)
O A (16)其中：outlier”*表示在\"个观测变量在总时刻出现的坏 值与实际值的偏差量大小。(10)静差：属于稳态误差的一种。如图1 (b)所示，由于 测控设备本身的问题，导致观测值与真实值之间存在误差 式(9)和式(10)是将非线性方程线性化之后的一阶 线性方程，与传统的卡尔曼滤波方程相比，状态转移矩阵 F(k + l\\k')和观测矩阵HQ)都是根据和h[_k, 近似为常数的现象.偏差量可正可负，可能发生在所有观 测数据中，也可能发生在部分观测数据中。假设在\"个观测 X(A)]的Jacobian矩阵代替。根据传统线性卡尔曼滤波的 变量在©至Q时刻存在静差，则存在静差的观测值可以被 递推方程，对系统进行滤波处理.得到非线性系统近似的 最优估计值。与卡尔曼滤波的基本方程小类比，可以得到扩展卡尔记为式(17)所示的形式。Z(人)=,X(A)] + + V(jfe) (17)其中：k = ki + 1.. . . ,kj ’bias”.、—,表示在 n 个观测变 量在k.至匕时刻存在静差的观测值与实际值的偏差量大小。• 256 •计算机测量与控制第27卷漂移：如图1 (c)所示，与前两种粗大误差不同，观 测数据的漂移表现为观测值与真实值之间存在偏差量为非 A (Residua^.,, — Residual„.uy 线性函数的异常情况。假设第”个观测变量在化至匕时刻 存在漂移，则存在漂移的观测值可以被记为式(18)所示 壬土1-------:~：------------- (22)I — 1Residual„,u =寺 工 Residual… '* = T-/+1(23)(24)的形式。Z(A) =+ +VO) (18)bias = Residual „,u 其中M = k.,k. + \\,... ,kt,drifts 是复杂变化的函 由于对应于非线性系统的随机误差服从正态分布，因 此当存在静差的观测值与原正态分布进行比较时就是存在 数.通常情况下为非线性的形式。2.2粗大误差的检测一个平移量为尿2$的正态分布。氏心为静差偏移量，是一个 常数，可正可负。式(25)和式(26)分别表示正常观测 对于动态目标识别中传感器得到观测数据中存在的粗 大误差值，需要对其进行准确检测，从而进行相应的补偿。 2. 2. 1 坏值(outlier)的检测时和存在静差时的残差情况。Residual(k)〜”(0,孑) (25)(26)对于坏值的检测：坏值是一组观测数据中存在的孤立 Residual d 〜NS，/),” = bias 异常值，因此需要对每个时刻每个观测数据进行检测，从 而判断坏值存在与否。坏值的检测通常可采用依拉达准则假设动态目标 式中，“为一段时间内残差的平均值。在静差检测移动窗口内，根据求得的标准差和残差, 可以对观测值中是否存在静差进行判断。如果移动窗口移 传感器观测值数据与扩展卡尔曼滤波方程中预估的观测数 据偏差量的绝对值为Residual”* ,即残差，可表示为式(19)。Residualn,k = | Z(&)—\\ (19)动3个时间点，3次移动窗口中残差的平均值可以近似表示 为一个常数量，即残差的平均值P = bias ,这里卩=bias表 示一个近似的常数.并且移动窗口内的标准差满足依拉达 法则，即Residual > 3。。移动窗口内标准差的浮动范围小 其中：Z(Q为传感器的观测数据.E 为扩展卡尔曼 滤波中根据系统理想模型得到的观测预估值。由式(10) 于一定的阈值，即I。|V Threshold ,则可以标记为 bias _ Flag = 1。Threshold为标准差o的阈值范围。 可得观测预估值为：Z(^) = H(k)X(.k) + y(k) Threshold的大小是根据F分布表中所取，因为观测值的噪 (20)声为白噪声，则残差满足F分布皿】。如果\"不为一个常数, 根据贝塞尔法求得标准差6I 为 Residual”./而是一个变化的函数形式，并且标准差I。|> Threshold , 说明这种粗大误差不为静差，可能为漂移。2.2.3漂移(drift)的检测(21)T-1 与坏值和静差不同，漂移表现为连续多个观测值与真 丁为设定的检测区间长度，当Residual服从正态分布 实值的偏差量很大且难以用具体的函数来表示。时，一组观测数据中,Residual < 3。的占比为99.73%,这 漂移的检测是基于静差检测基础之上的，当移动检测 窗口的观测值中已经检测岀Residual > 3o且残差的平均值 说明观测数据中残差Residual < 3。的情况是占大多数的。若Residual > 3。，此时出现该观测数据的可能性为 0.07%,可信度低，为高度异常的坏值，可以确定该观测 “不为一个常数时，如果满足\"IN Threshold ,即标准差 大于一定的阈值则检测到该时间的观测值为漂移，标 记为 drift _ Flag = 1 ,否则，标记为 drift _ Flag = 0 , 值为坏值，将坏值标记为outlier _ Flag = 1 ,非坏值标记 为outlier _ Flag = 0 o当每次检测出一个坏值时，就对坏 值进行一次标记和补偿。需要注意的是.依拉达准则只适用于观测次数较多的 对于漂移.可以采用局部线性拟合的方式，在局部构建一 个线性方程近似漂移量，如(27)所示：drift = p\\k + p2 (27)情况(观测次数” >10)血。2. 2.2 静差(bias)的检测其中:drift表示A时刻的观测漂移偏差量，P,表示局部 线性区线性方程的斜率大小，介表示局部线性区线性方程的 截距大小。2.3粗大误差的补偿静差与坏值不同，静差的存在使得连续多个观测值与 真实值之间的差值近似为常数，且差值可正可负。静差可 能发生在所有的观测值中，也可能发生在部分观测值中。 因此，准确检测静差时不能只对怡时刻第”个观测值进行检 粗大误差值已经可以通过上述讨论的方法进行检测， 下一步就需要对已经检测到的不同类型的粗大误差进行分 测，而是对k时刻前的第丁 一/时刻观测值到第丁时刻观测 值进行检测.其中• 2为设定的检测区间长度，可长可短。 类补偿。2. 3.1坏值(outlier)的补偿先设定个静差检测移动窗口⑷，长度为那么对于该静差 检测移动窗口，则标准差和残差的平均值分别为：根据依拉达法则.可以检测出此时的观测值是否存在 坏值，并将其标记为outlier _ Flag = 1 <,第10期张 迪，等：基于粗大误差检测和补偿的改进型EKF动态目标跟踪算法• 257 •对于标记为outlier _ Flag = 1的观测值，可以通过式(21)中的残差Residual”*来对此时的观测值进行补偿，即 阵如下所示：L 此时的观测值减去残差，使得最后观测值得以修正。若用 Z*d)表示补偿后的观测值，则可得到式(28)：Z„, (.k~) = Z(k) —Residualn,t 2.3.2静差(bias)的补偿【3 13 03 j2*(e_Ad/ + 1—1) 13(33)F= 03 +(1-严)人e-^I3— ^tl3(28)_03 通过式(26)可以检测到存在静差的观测值，并将其 标记为bias _ Flag = 1。对于移动窗口内标记为bias _ Flag =1的观测值，对其进行补偿。静差与坏值不同，静差为连 G = (34)_ 03 _续多个观测值存在的粗大误差，如果直接采用此时的残差 在此三维系统中，导弹对对动态目标采用纯方位观测， 观测为俯仰角和水平方向偏向角，实际观测中雷达具有加 进行补偿，很有可能会导致EKF滤波估计值依然偏离了真 实状态值，无法达到预期目标跟踪效果。为了尽量避免单 性观测噪声，观测方程为：z仏)=肛 X(e)]+V4) 个残差补偿以及减少多个观测值的偏差，可以利用移动检 测窗口的残差平均值来代替此时的残差进行补偿。因此， 静差的补偿方法是利用存在静差的观测值减去移动检测窗 (35)式中，= r arctan □内的残差平均值来进行观测值的补偿，移动检测窗口内 的残差平均值即式(23)中Residual”*的大小,可以表示 L 丿厂,仏)+亡(©) ,arctan— ~\\」(36)为式(29)：Z*(A) = Z(.k~) —Residual „,u (29)在三维寻的制导的初始化中，采样时间少=0.01s, £ =4. 0 so其中，观测补偿量Residual“可以根据移动检测窗口 导弹的初始状态：x(0) = [3500,1500,1000, — 1100, — 150, — 50,10,10,10]TEKF滤波估计的初始化状态：的移动而发生实时变化，式(23)时间长度2也是会随着观 测值中静差的标记量的多少而发生变化，时间长度/越长， 补偿量就越精确，补偿效果就越好。2.3.3漂移(drift)的补偿er(0) = [3000,1000,800, — 950, — 100, — 100,0,0,0]7 初始化EKF滤波估计的状态协方差矩阵：Po = [10* X Is »0gX3 ； 0sx6 ,10' X /3 J通过|“|>0, lOTVeshoM可以检测到观测值中的 漂移，并标记为drift _ Flag = 1 o与坏值和静差不同，漂 实验仿真中，在传感器观测值中加入粗大误差(坏值、 移的偏移量不为一个常数，如果采用移动检测窗口中的残 静差和漂移)，对比优化前后EKF的跟踪状态情况。得到 差量对漂移的观测值进行补偿，观测值的偏移量依然跟真 实差距很大，这会导致EKF滤波后的状态估计量依然偏离 真实状态量，补偿结果并不理想.不利于动态目标跟踪。虽然漂移量不为具体的函数形式，但在局部位置偏移 量仍然可以近似为一个线性拟合的方程。因此，对于存在 漂移的观测值，可以通过局部线性拟合的方式对观测值进 行近似补偿。补偿过程可以表示为式(30)：Z*(A) = Z(.k) —drift (30)式中,drift为式(27)中线性方程近似漂移量。如果此时 介近似为0,则可以认为此时的是bias ,因此，当两 种粗大误差均存在的情况下，为了更佳地补偿效果，两种 粗大误差均可以采用线性拟合进行补偿。图2粗大误差检测结果3实验仿真三维非线性系统状态模型：X(&)=[匚(上)，G (上)，7\\(〃)，q(&),Vy(k) iVgCk) 9ax(k) »ay(^) 图2为三维寻的跟踪的俯仰角和水平方向偏向角随时 间变化的曲线。图2中，俯仰角的观测值在0.8 s、1.3 s和 3. 6 s有坏值点存在，水平方向偏向角在0.4 s、1.6 s、2.0 (31)(32)s和3.0 s存在坏值点，2.2-2.6 s存在静差，2.1〜2.2 s 三维系统运动状态方程可以表示为：X(& + 1) = FX(Q+GU(&)+W(&) 存在漂移。改进型EKF能将所有的粗大误差精确检测和分 类标记出来。精确检测之后，改进型EKF算法可以通过式 其中，F为状态转移矩阵，G为过程噪声驱动矩阵，矩子(28)〜(30)对不同的粗大误差进行分类补偿。・258・计算机测量与控制第27卷对传统EKF和改进型EKF进行50组仿真实验后，得 到每组实验的均方根误差(RMSE),经统计后,所得结果 如表1所示。表1传统EKF与改进型EKF滤波后的50组仿真实验均方根误差(RMSE)统计结果传统EKF的RMSE | 改进EKF的RMSE位置速度加速度位置速度加速度y/m -2000 a勺方根误差(RMSE)平均值40.136 927.693 7 |0.010 9|6.208 13.081 4勺方根误差(RMSE)标准差0.082 10.055 00.993 40. 924 6 |0-2000 l/a0.010 9——Real Track0根据表1中传统EKF和改进EKF在50次仿真实验下的位置、速度和加速度的均方根误差(RMSE)的数据，可 以绘制出如图3所示改进前后均方根误差比较图。O- RMSE of traditional EKF --9-RMSE of Modified EKFy/m 0 0 x/m图4三维寻的跟踪状态图20 30 4050Number of experiments结果的影响，从而提高跟踪的精确度。仿真结果表明，相 j 50—♦—RMSE of trad辻iona] EKF-V-RMSE of Modified EKF比于传统EKF,改进型EKF算法滤波抗干扰能力较强，滤 波性能较为稳定，实现了对动态系统目标的准确跟踪。参考文献：0j §0.02w 专 0.01O 420 30 4050Number of experiments―•-丽KE of traditional EKF RMSE of Modified EKF[1] Kalman R E. A New Approach to Linear Filtering and Predic tion Problems [J]. Journal of Basic Engineering Transactions,1960, 82： 35 -45.20 « 03050Number of experiments[2] Kalman R E, Bucy R S. New results in linear filtering and pre 图3传统EKF与改进型EKF滤波后的均方根误差比较图diction theory [J]. Journal of Basic Engineering Transactions,1961, 83 (1)： 95-10&[3] Senne K. Review of stochastic processes and filtering theory — Andrew H. Jazwinski [J]. IEEE Transactions on Automatic 由表1和图3可知，在50次的仿真实验中，改进型 EKF算法的位置和速度的均方根误差明显比优化前的均方 根误差要小，其中，改进型EKF滤波后位置均方根误差的 平均值比传统EKF小33. 9287,标准差比传统EKF小 0.9113。改进型EKF滤波后速度均方根误差的平均值比传 Control, 1972, 17 (5)： 752 -753.[4] 赵 佳，吕 弘，周智恺，等.基于扩展卡尔曼滤波的无人飞行 器姿态解算EJ1电子技术与软件工程，2018 (24)： 67 - 68.统EKF小24. 6123,标准差比传统EKF小0. 8696.这说明 改进型EKF算法滤波受到粗大误差干扰的影响更小，性能 [5] 陈清华，卢 宇，何志杰.基于改进EKF算法的锂电池SOC估 算方法[J1福建师范大学学报(自然科学版)，2018, 34(6)： 39 -44, 51.[6] Zhu Z, Meng Z, Zhang Z, et al. Robust particle filter for state estimation using measurements with different types of gross er 更稳定，跟踪的精确度更高。经过传统EKF和改进型EKF 算法后，实验仿真得到图4所示三维寻的跟踪状态图。根据图4所示寻的制导跟踪状态图可知，在粗大误差 干扰情况下，传统EKF的状态估计值已经严重偏离实际的 rors [J]. ISA transactions, 2017, 69: 281 - 295.[7] 张敏，袁辉.拉依达(PauTa)准则与异常值剔除EJ1郑 州工业大学学报，1997 (1)： 84 -88.状态值。相比传统EKF,改进型EKF滤波后的寻的制导跟 踪偏差明显较小，没有出现偏离实际状态值，这说明三维 [8] 熊艳艳，吴先球.粗大误差4种判别准则的比较和应用[J]. 大学物理实验，2010, 23 (1)： 66 -68.动态目标的跟踪精度较高。5 结束语本文针对动态系统目标跟踪观测过程中存在的坏值、 静差和漂移3种粗大误差，提出了一种基于粗大误差检测 和补偿的改进型EKF动态目标跟踪算法。该方法能实时检 [9] Tao S, Li B, Li N, et al. A Novel Approach for Moving Win dow Size Selection utilizing recursive PCA [A].第 37 届中国控 制会议论文集[C]. 2018： 710-715.[10] 刘晓鹏，刘坤会.F分布密度函数之性质[J]・应用概率统计， 2016, 21 (3)： 304 -314.测粗大误差并补偿观测量，能有效地减小粗大误差对滤波 因篇幅问题不能全部显示，请点此查看更多更全内容