卡尔曼滤波在目标跟踪中的应用

卡尔曼滤波在目标跟踪中的应用［摘要］机动卡尔曼算法(VD算法)在扩展卡尔曼滤波诸算法中原理较为简单，目标跟踪效果也较好．一．模型建立（１）非机动模型（匀速直线运动）系统模型)()()1(kGWkXkX其中)()()()()(kVkykVkxkXyx;10001000010001TT;10200102TTG;)()()(kWkWkWyx;0)]([kWE;kjTQjWkWE)]()([。测量模型为:)()()(kVkHXkZ;其中01000001H;)(kV为零均值,协方差阵为R白噪声,与)(kW不相关。（２）机动模型系统模型);(*)()1(kWGkXkXmmmmm其中)()()()()()()(kakakVkykVkxkXmymymymmxmm;1000000100000100020100000100200122TTTTTTm;10012040020422TTTTGm;0)]([kWEm,kjmmmQjWkWET)]()([。观测模型与机动模型的相同,只是H矩阵为mH,000100000001mH二．kalman滤波算法作为一般的kalman滤波算法其序贯算法可以描述如下：)1/1(ˆ)1/(ˆkkXkkXTTGkGQkkPkkP)1()1/)1()1/(1])1/([)1/()(RHkkHPHkkPkKTT)]1/()()[()1/(ˆ)/(ˆkkHXkZkKkkXkkX)1/()()1/()/(kkHPkKkkPkkP起始估计值为221/ˆ2/2221/xxxyyyzzzTzzzTX起始估计的估计误差为(2)(1)(2)(1)2(2/2)(2)(1)(2)(1)2xxxxyyyyvvvTuTvvvTuTX起始估计的估计误差协方差矩阵为22222222222222/002/004(2/2)00/200/4xyuxxuyTTTTTTTTP三．VD算法描述VD算法采用两种模型,即非机动模型和机动模型,无机动时滤波器工作于正常模式(低阶模型),用一机动检测器监视机动,一旦检测到机动,模型中立即增加一个状态变量,用机动模型跟踪直至下一次判决而退回到正常的非机动模型，由于采用机动的VD算法，因此涉及到的一个关键环节就是机动检测问题。滤波器开始工作于正常模式,其输出的新息序列为)(kv,令)()1()(kkk;)()()()(1kvkSkvkT其中,)(kS是)(kv的协方差矩阵.取1)1(作为检测机动的有效窗口长度,机动检测的方法为:如果Thk)(,则认为目标在1k开始有一恒定的加速度加入,这时目标模型应由低阶模型转向高阶模型。由高阶机动模型退回低阶非机动模型的检测方法是检测加速度估计值是否有统计显著性意义。令kjaajk1)()(;)/(ˆ)]/()[/(ˆ)(1kkakkPkkakmaTa其中)/.(ˆkka是加速度分量的估计值,)/(kkPma是协方差矩阵的对应块,如果Taka)(则加速度估计无显著性意义,滤波器退出机动模型。前面提到了起始状态的初始化，下面讲一下当检测到机动时，滤波器的初始化问题。当在第K次检测到机动时,滤波器假定在1k开始有一恒定的加速度,在窗内的状态估计应修正如下:首先,加速度在k的估计为)]1/(ˆ)([2)/(ˆ2kkZkZTkkaxxmx;)]1/(ˆ)([2)/(ˆ2kkZkZTkkayymy。xZˆ是对测量的预测值,)1/(ˆ)1/(ˆkkxkkZx;)1/(ˆ)1/(ˆkkykkZy;在k的位置估计为:)()1/(ˆkZkkxxm;)()1/(ˆkZkkyym;在k的速度估计为:)1/1()1/1()/(kkTakkVkkVxxmx)1/1()1/1()/(kkTakkVkkVyymy;协方差矩阵修正为:1111)/(RkkPm11122)/(RTkkPm112152)/(RTkkPm)]1/1()1/1(2)1/1([4)/(222121111255kkPTkkTPkkPRTkkPm)]1/1(4)1/1()1/1(44)/(122211211222kkPTkkPkkPTRTkkPm)]1/1(6)1/1(2)1/1(44)/(1222211311325kkPTkkPTkkPTRTkkPm四．仿真实验假定有一二座标雷达对一平面上运动的目标进行观测，目标在t0-400秒沿着y轴作恒速直线运动，运动速度为-15米/秒，目标的起始点为(2000米,10000米)，在t=400-600秒向轴x方向做090的慢转弯，加速度为yxuu0.075米/秒2，完成慢转弯后加速度将降为零，从t=610秒开始做90度的快转弯，加速度为0.3米/秒2，在660秒结束转弯，加速度降至零。雷达扫描周期T=2秒，X和Y独立地进行观测，观测噪声的标准差均为100米。描述如下：15002000250030003500400045005000-2000020004000600080001000012000其中，程序算法中各参数为:加权衰减因子85.0,机动检测门限25hT;退出机动的检测门限49.9aT;在跟踪的开始,首先采用非机动模型,从第20次采样开始,激活机动检测器。1500200025003000350040004500-2000020004000600080001000012000真实轨迹观测轨迹50次滤波轨迹0100200300400-100-50050X坐标滤波误差均值曲线0100200300400-50050100Y坐标滤波误差均值曲线0100200300400050100150X坐标滤波误差标准差曲线0100200300400050100150Y坐标滤波误差标准差曲线通过上图，可以看到：VD算法有4次机动,分别对应目标的2次加速运动,和2次匀速运动,符合目标真实轨迹变化。只是在模型出现机动的时候,会出现大的误差。在模型的调整过程中,可以明显发现:机动检测门限hT,退出机动的检测门限aT,加权衰减因子对算法的有效滤波有很大的影响，当目标快转弯时,会出现大的误差,这时候可以通过改变机动检测门限来减小。