基于粗大误差检测和补偿的改进型EKF动态目标跟踪算法

(温州大学 电气数字化设计技术国家地方联合工程实验室，浙江 温州 325035)

0 引言

卡尔曼滤波理论[1-2]是目前最重要的最优估计理论之一，因其目标跟踪算法实用性强，滤波效率高，状态估计结果较为优化，在寻的制导、路径识别、视频监控等领域得到广泛应用。卡尔曼滤波算法在线性系统中为最优估计算法，但工业中实际的系统往往为非线性系统，因此，针对非线性系统提出了扩展卡尔曼滤波算法(Extended Kalman Filter，EKF)[3]。扩展卡尔曼滤波的思想是基于非线性方程的线性化。扩展卡尔曼滤波状态估计的精确度与系统方程非线性程度密切相关。系统的非线性程度越小，滤波结果越精确。

目前，国内外学者基于扩展卡尔曼滤波解决了很多工业中的工程问题。赵佳等为解决无人飞行器姿态传感器低成本高精度的要求，引入扩展卡尔曼滤波算法，有效地消除了无人机飞行器运动过程对姿态解算精度的影响[4]。陈清华等针对锂电池剩余电量(SOC)无法精确估算的问题，提出了一种基于改进Thevenin模型的EKF算法[5]，实现了锂电池SOC的精确估算。

上述扩展卡尔曼滤波算法是基于动态目标跟踪过程中观测值只含随机误差的情况，但是实际观测值中难免会出现各种粗大误差(gross errors)[6]，比如坏值(outlier)、静差(bias)和漂移(drift)等[6]。受到粗大误差的影响，动态目标跟踪的滤波状态容易出现异常，这直接影响状态估计的准确性。本文提出一种基于粗大误差观测值检测和补偿的改进型EKF动态目标跟踪算法，通过对粗大误差观测值的精确检测和优化分类补偿，弥补了传统的EKF算法对于实际应用中的不足。实验仿真结果表明，改进型EKF算法能够实现粗大误差观测值下的动态系统目标跟踪，且具有较好的准确性。

1 扩展卡尔曼滤波算法

非线性离散系统的动态方程如下：

X(k+1)=f[k,X(k)]+G(k)W(k)

(1)

Z(k)=h[k,X(k)]+V(k)

(2)

式(1)为系统状态方程，式(2)为观测方程。X(k)是n维系统状态向量；Z(k)是m维观测向量；f[k,X(k)]和h[k,X(k)]是X(k)和Z(k)对应的非线性系统函数；G(k)是系统噪声驱动矩阵；W(k)则是零均值系统白噪声矩阵；V(k)是零均值观测白噪声，且{W(k)}和{V(k)}互不相关。将式(1)中X(k+1)做一阶泰勒展开，得：

W(k)+φ(k)

(3)

(4)

(5)

将式(2)中的Z(k)做一阶泰勒展开，得：

V(k)=H(k)X(k)+y(k)+V(k)

(6)

(7)

(8)

通过式(1)～(8)的变换，n维的非线性状态方程和m维的非线性观测方程可以转化为下式：

X(k+1)=F(k+1|k)X(k)+G(k)W(k)+φ(k)

(9)

Z(k)=H(k)X(k)+y(k)+V(k)

(10)

式(9)和式(10)是将非线性方程线性化之后的一阶线性方程，与传统的卡尔曼滤波方程相比，状态转移矩阵F(k+1|k)和观测矩阵H(k)都是根据f[k,X(k)]和h[k,X(k)]的Jacobian矩阵代替。根据传统线性卡尔曼滤波的递推方程，对系统进行滤波处理，得到非线性系统近似的最优估计值。

与卡尔曼滤波的基本方程[1]类比，可以得到扩展卡尔曼滤波递推方程：

(11)

P(k+1|k)=F(k+1|k)P(k|k)

FT(k+1|k)+Q(k+1)

(12)

K(k+1)=P(k+1|k)HT(k+1|k)

[H(k+1)P(k+1|k)HT(k+1)+R(k+1)]-1

(13)

(14)

P(k+1)=[I-K(k+1)H(k+1)]P(k+1|k)

(15)

式中，P(k+1|k)表示根据上一个k时刻的协方差值来估计k+1时刻的协方差值。P(k+1)表示k+1时刻更新的协方差值。

2 观测值的粗大误差

2.1 粗大误差的类型

观测数据中通常出现的粗大误差大致有3种，分别是坏值(outlier)，静差(bias)和漂移(drift)，如图1所示。

图1 存在粗大误差情况下的观测值

坏值：也称异常值或离群值，如图1(a)所示，坏值是观测数据中明显偏离其他观测数据的异常观测值，通常情况下为个别观测数据。假设第n个观测变量在ki时刻出现了坏值，那么ki时存在坏值的观测值可以被记为式(16)的形式。

Z(ki)=h[ki,X(ki)]+outliern,ki+V(ki)

(16)

其中:outliern,ki表示在n个观测变量在ki时刻出现的坏值与实际值的偏差量大小。

静差：属于稳态误差的一种。如图1(b)所示，由于测控设备本身的问题，导致观测值与真实值之间存在误差近似为常数的现象，偏差量可正可负，可能发生在所有观测数据中，也可能发生在部分观测数据中。假设在n个观测变量在ki至kj时刻存在静差，则存在静差的观测值可以被记为式(17)所示的形式。

Z(k)=h[k,X(k)]+biasn,ki～kj+V(k)

(17)

其中:k=ki,ki+1,...,kj，biasn,ki～kj表示在n个观测变量在ki至kj时刻存在静差的观测值与实际值的偏差量大小。

漂移：如图1(c)所示，与前两种粗大误差不同，观测数据的漂移表现为观测值与真实值之间存在偏差量为非线性函数的异常情况。假设第n个观测变量在ki至kj时刻存在漂移，则存在漂移的观测值可以被记为式(18)所示的形式。

Z(k)=h[k,X(k)]+driftn,ki～kj+V(k)

(18)

其中:k=ki,ki+1,...,kj，driftn,ki～kj是复杂变化的函数，通常情况下为非线性的形式。

2.2 粗大误差的检测

对于动态目标识别中传感器得到观测数据中存在的粗大误差值，需要对其进行准确检测，从而进行相应的补偿。

2.2.1 坏值(outlier)的检测

对于坏值的检测：坏值是一组观测数据中存在的孤立异常值，因此需要对每个时刻每个观测数据进行检测，从而判断坏值存在与否。

坏值的检测通常可采用依拉达准则[7]。假设动态目标传感器观测值数据与扩展卡尔曼滤波方程中预估的观测数据偏差量的绝对值为Residualn,k，即残差，可表示为式(19)。

(19)

(20)

根据贝塞尔法求得标准差σ。

(21)

T为设定的检测区间长度，当Residual服从正态分布时，一组观测数据中，Residual<3σ的占比为99.73%，这说明观测数据中残差Residual<3σ的情况是占大多数的。

若Residual>3σ，此时出现该观测数据的可能性为0.07%，可信度低，为高度异常的坏值，可以确定该观测值为坏值，将坏值标记为outlier_Flag=1，非坏值标记为outlier_Flag=0。当每次检测出一个坏值时，就对坏值进行一次标记和补偿。

需要注意的是，依拉达准则只适用于观测次数较多的情况(观测次数n>10)[8]。

2.2.2 静差(bias)的检测

静差与坏值不同，静差的存在使得连续多个观测值与真实值之间的差值近似为常数，且差值可正可负。静差可能发生在所有的观测值中，也可能发生在部分观测值中。因此，准确检测静差时不能只对k时刻第n个观测值进行检测，而是对k时刻前的第T-l时刻观测值到第T时刻观测值进行检测，其中，l为设定的检测区间长度，可长可短。先设定个静差检测移动窗口[9]，长度为l。那么对于该静差检测移动窗口，则标准差和残差的平均值分别为：

(22)

(23)

(24)

由于对应于非线性系统的随机误差服从正态分布，因此当存在静差的观测值与原正态分布进行比较时就是存在一个平移量为bias的正态分布。bias为静差偏移量，是一个常数,可正可负。式(25)和式(26)分别表示正常观测时和存在静差时的残差情况。

Residual(k)～N(0,σ2)

(25)

Residual(k)～N(μ,σ2),μ=bias

(26)

式中，μ为一段时间内残差的平均值。

在静差检测移动窗口内，根据求得的标准差和残差，可以对观测值中是否存在静差进行判断。如果移动窗口移动3个时间点，3次移动窗口中残差的平均值可以近似表示为一个常数量，即残差的平均值μ=bias，这里μ=bias表示一个近似的常数，并且移动窗口内的标准差满足依拉达法则，即Residual>3σ。移动窗口内标准差的浮动范围小于一定的阈值，即|σ|

2.2.3 漂移(drift)的检测

与坏值和静差不同，漂移表现为连续多个观测值与真实值的偏差量很大且难以用具体的函数来表示。

漂移的检测是基于静差检测基础之上的，当移动检测窗口的观测值中已经检测出Residual>3σ且残差的平均值μ不为一个常数时，如果满足|σ|≥Threshold，即标准差大于一定的阈值[8]，则检测到该时间的观测值为漂移，标记为drift_Flag=1，否则，标记为drift_Flag=0，对于漂移，可以采用局部线性拟合的方式，在局部构建一个线性方程近似漂移量，如(27)所示:

drift=p1k+p2

(27)

其中:drift表示k时刻的观测漂移偏差量，p1表示局部线性区线性方程的斜率大小，p2表示局部线性区线性方程的截距大小。

2.3 粗大误差的补偿

粗大误差值已经可以通过上述讨论的方法进行检测，下一步就需要对已经检测到的不同类型的粗大误差进行分类补偿。

2.3.1 坏值(outlier)的补偿

根据依拉达法则，可以检测出此时的观测值是否存在坏值，并将其标记为outlier_Flag=1。

对于标记为outlier_Flag=1的观测值，可以通过式(21)中的残差Residualn,ki来对此时的观测值进行补偿，即此时的观测值减去残差，使得最后观测值得以修正。若用Zrec(k)表示补偿后的观测值，则可得到式(28):

Zrec(k)=Z(k)-Residualn,k

(28)

2.3.2 静差(bias)的补偿

(29)

2.3.3 漂移(drift)的补偿

通过|μ|>0,|σ|>Threshold可以检测到观测值中的漂移，并标记为drift_Flag=1。与坏值和静差不同，漂移的偏移量不为一个常数，如果采用移动检测窗口中的残差量对漂移的观测值进行补偿，观测值的偏移量依然跟真实差距很大，这会导致EKF滤波后的状态估计量依然偏离真实状态量，补偿结果并不理想，不利于动态目标跟踪。

虽然漂移量不为具体的函数形式，但在局部位置偏移量仍然可以近似为一个线性拟合的方程。因此，对于存在漂移的观测值，可以通过局部线性拟合的方式对观测值进行近似补偿。补偿过程可以表示为式(30):

Zrec(k)=Z(k)-drift

(30)

式中,drift为式(27)中线性方程近似漂移量。如果此时p1近似为0，则可以认为此时的drift是bias，因此，当两种粗大误差均存在的情况下，为了更佳地补偿效果，两种粗大误差均可以采用线性拟合进行补偿。

3 实验仿真

三维非线性系统状态模型：

X(k)=[rx(k),ry(k),rz(k),vx(k),

vy(k),vz(k),ax(k),ay(k),az(k)]T

(31)

三维系统运动状态方程可以表示为:

X(k+1)=FX(k)+GU(k)+W(k)

(32)

其中，F为状态转移矩阵，G为过程噪声驱动矩阵，矩阵如下所示:

(33)

(34)

在此三维系统中，导弹对对动态目标采用纯方位观测，观测为俯仰角和水平方向偏向角，实际观测中雷达具有加性观测噪声，观测方程为:

Z(k)=h[X(k)]+V(k)

(35)

式中，

(36)

在三维寻的制导的初始化中，采样时间Δt=0.01 s，t=4.0 s。

导弹的初始状态:

x(0)=[3500,1500,1000,-1100,-150,-50,10,10,10]T

EKF滤波估计的初始化状态:

ex(0)=[3000,1000,800,-950,-100,-100,0,0,0]T

初始化EKF滤波估计的状态协方差矩阵:

P0=[104×I6,06×3;03×6,102×I3]

实验仿真中，在传感器观测值中加入粗大误差(坏值、静差和漂移)，对比优化前后EKF的跟踪状态情况。得到经粗大误差检测前后的观测值如图2所示。

图2 粗大误差检测结果

图2为三维寻的跟踪的俯仰角和水平方向偏向角随时间变化的曲线。图2中，俯仰角的观测值在0.8 s、1.3 s和3.6 s有坏值点存在，水平方向偏向角在0.4 s、1.6 s、2.0 s和3.0 s存在坏值点，2.2～2.6 s存在静差，2.1～2.2 s存在漂移。改进型EKF能将所有的粗大误差精确检测和分类标记出来。精确检测之后，改进型EKF算法可以通过式子(28)～(30)对不同的粗大误差进行分类补偿。

对传统EKF和改进型EKF进行50组仿真实验后，得到每组实验的均方根误差(RMSE)，经统计后，所得结果如表1所示。

表1 传统EKF与改进型EKF滤波后的50组仿真实验均方根误差(RMSE)统计结果

根据表1中传统EKF和改进EKF在50次仿真实验下的位置、速度和加速度的均方根误差(RMSE)的数据，可以绘制出如图3所示改进前后均方根误差比较图。

图3 传统EKF与改进型EKF滤波后的均方根误差比较图

由表1和图3可知，在50次的仿真实验中，改进型EKF算法的位置和速度的均方根误差明显比优化前的均方根误差要小，其中，改进型EKF滤波后位置均方根误差的平均值比传统EKF小33.9287，标准差比传统EKF小0.9113。改进型EKF滤波后速度均方根误差的平均值比传统EKF小24.6123，标准差比传统EKF小0.8696.这说明改进型EKF算法滤波受到粗大误差干扰的影响更小，性能更稳定，跟踪的精确度更高。经过传统EKF和改进型EKF算法后，实验仿真得到图4所示三维寻的跟踪状态图。

图4 三维寻的跟踪状态图

根据图4所示寻的制导跟踪状态图可知，在粗大误差干扰情况下，传统EKF的状态估计值已经严重偏离实际的状态值。相比传统EKF，改进型EKF滤波后的寻的制导跟踪偏差明显较小，没有出现偏离实际状态值，这说明三维动态目标的跟踪精度较高。

5 结束语

本文针对动态系统目标跟踪观测过程中存在的坏值、静差和漂移3种粗大误差，提出了一种基于粗大误差检测和补偿的改进型EKF动态目标跟踪算法。该方法能实时检测粗大误差并补偿观测量，能有效地减小粗大误差对滤波结果的影响，从而提高跟踪的精确度。仿真结果表明，相比于传统EKF，改进型EKF算法滤波抗干扰能力较强，滤波性能较为稳定，实现了对动态系统目标的准确跟踪。