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

卡尔曼滤波在目标跟踪中的应用
［摘要］机动卡尔曼算法(VD算法)在扩展卡尔曼滤波诸算法中原理较为简单，目标跟踪效
果也较好．

一．模型建立
（１） 非机动模型（匀速直线运动）
系统模型
)()()1(kGWkXkX
其中











)()()()()(kVkykVkxkX

y
x

; 10001000010001TT; 10200102TTG;





)()()(kWkWkW

y
x

; 0)]([kWE; kjTQjWkWE)]()([。

测量模型为:
)()()(kVkHXkZ
;

其中 01000001H;
)(kV为零均值,协方差阵为R白噪声,与)(kW
不相关。

（２） 机动模型
系统模型
);(*)()1(kWGkXkXmmmmm
其中











)()()()()()()(kakakVkykVkxkX
m
y

m
y

m
y

m
m
x

m

m
;1000000100000100020100000100200122TTTTTTm;10012040020422TTTTGm;

0)]([kWE
m
, kjmmmQjWkWET)]()([。

观测模型与机动模型的相同,只是H矩阵为mH,





000100000001mH

二．kalman滤波算法
作为一般的kalman滤波算法其序贯算法可以描述如下：
)1/1(ˆ)1/(ˆkkXkkX
TT
GkGQkkPkkP)1()1/)1()1/(

1])1/([)1/()(RHkkHPHkkPkKTT
)]1/()()[()1/(ˆ)/(ˆkkHXkZkKkkXkkX
)1/()()1/()/(kkHPkKkkPkkP
起始估计值为







221/ˆ2/2221/xxxyyyz
zzTzzzT





X

起始估计的估计误差为
(2)(1)(2)(1)2(2/2)(2)(1)(2)(1)2xxxxyyyyvvvTuTvvvTuT








X

起始估计的估计误差协方差矩阵为
22
22
2

2

2

22

22
2

2
2

/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。

x
Z
ˆ
是对测量的预测值,

)1/(ˆ)1/(ˆkkxkkZx;
)1/(ˆ)1/(ˆkkykkZy;
在k的位置估计为:
)()1/(ˆkZkkxxm;
)()1/(ˆkZkkyym;
在k的速度估计为:

)1/1()1/1()/(kkTakkVkkV
xxmx

)1/1()1/1()/(kkTakkVkkVyymy;
协方差矩阵修正为:
1111
)/(RkkPm

1112
2
)/(RTkkPm

11215
2
)/(RTkkPm

)]1/1()1/1(2)1/1([4)/(222121111255kkPTkkTPkkPRTkkP
m
)]1/1(4)1/1()1/1(44)/(122211211222kkPTkkPkkPTRTkkP
m

)]1/1(6)1/1(2)1/1(44)/(1222211311325kkPTkkPTkkPTRTkkP
m

四．仿真实验
假定有一二座标雷达对一平面上运动的目标进行观测，目标在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
-2000
0
2000
4000
6000
8000
10000
12000

其中，程序算法中各参数为:
加权衰减因子85.0, 机动检测门限25hT; 退出机动的检测门限49.9aT;
在跟踪的开始,首先采用非机动模型,从第20次采样开始,激活机动检测器。
1500200025003000350040004500
-2000
0
2000
4000
6000
8000
10000
12000
真实轨迹
观测轨迹
50次滤波轨迹

0100200300400-100-50050X坐标 滤波误差均值曲线0100200300400
-50
0
50
100
Y坐标 滤波误差均值曲线
0100200300400050100150X坐标 滤波误差标准差曲线0100200300400
0
50
100
150
Y坐标 滤波误差标准差曲线
通过上图，可以看到：VD算法有4次机动,分别对应目标的2次加速运动,和2次匀速
运动,符合目标真实轨迹变化。只是在模型出现机动的时候,会出现大的误差。在模型的调整
过程中,可以明显发现:机动检测门限hT,退出机动的检测门限aT,加权衰减因子对算法
的有效滤波有很大的影响，当目标快转弯时,会出现大的误差,这时候可以通过改变机动检测
门限来减小。