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

一． 模型建立
(1) 非机动模型（匀速直线运动） 系统模型
)()()1(k GW k X k X +Φ=+
其中
⎥⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢
⎢⎣⎡=)()()()()(k V k y k V k x k X y x ； ⎥
⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=Φ10001000010001
T T
； ⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎣⎡=1020
0102T T G
⎥
⎦⎤
⎢⎣⎡=)()()(k W k W k W y x ； 0)]([=k W E ； kj T Q j W k W E δ=)]()([ 测量模型为:
)()()(k V k HX k Z +=;
其中 ⎥⎦
⎤
⎢⎣⎡=01000001H )(k V 为零均值,协方差阵为R 白噪声,与)(k W 不相关。

(2) 机动模型 系统模型
);(*)()1(k W G k X k X m m m m m +Φ=+
其中
⎥⎥
⎥
⎥
⎥⎥⎥
⎥
⎦⎤
⎢⎢⎢⎢⎢⎢⎢⎢⎣⎡=)()()()()()()(k a k a k V k y k V k x k X m y m y
m y m m x m
m ；⎥⎥⎥
⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢
⎢⎢⎢⎣⎡=Φ100
00
00100000100020100000100200
122
T T T T T T m
；⎥⎥⎥
⎥⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣⎡=10012040020422T T T T G m 0)]([=k W E m , kj m m m Q j W k W E T
δ=)]()([
观测模型与机动模型的相同,只是H 矩阵为m H 。

⎥⎦
⎤
⎢⎣⎡=000100000001m
H
二．Kalman 滤波算法
作为一般的Kalman 滤波算法其算法可以描述如下：
)1/1(ˆ)1/(ˆ--Φ=-k k X k k X
T T G k GQ k k P k k P )1()1/)1()1/(-+Φ--Φ=- 1])1/([)1/()(-+--=R H k k HP H k k P k K T T )]1/()()[()1/(ˆ)/(ˆ--+-=k k HX k Z k K k k X k k X
)1/()()1/()/(---=k k HP k K k k P k k P
起始估计值为
()()()()()()()221/ˆ2/2221/x x x y y y z z z T z z z T ⎡⎤⎢⎥-⎡⎤⎣⎦⎢⎥=⎢⎥⎢⎥⎢⎥⎡⎤-⎣⎦⎣⎦
X 起始估计的估计误差为
(2)(1)(2)(1)2(2/2)(2)
(1)(2)(1)2x x x x y y y y v v v T u T v v v T
u T -⎡⎤
⎢⎥-⎢⎥
⋅+⎢⎥
=⎢⎥-⎢⎥
-⎢⎥⋅+⎢⎥⎣⎦
X 起始估计的估计误差协方差矩阵为
2222
22
222
22222/00
2/004(2/2)00/200
/4x
y
u
x x u y T
T T
T T T T
T ⎡⎤σσ⎢⎥σσ⎢⎥σ+⎢⎥
=⎢
⎥σσ⎢⎥⎢⎥σσ⎢⎥σ+⎢⎥⎣
⎦
P 三．VD 算法描述
VD 算法采用两种模型,即非机动模型和机动模型,无机动时滤波器工作于正
常模式(低阶模型),用机动检测器监视机动,一旦检测到机动,模型中立即增加一个状态变量,用机动模型跟踪直至下一次判决而退回到正常的非机动模型。

由于采用机动的VD 算法，因此涉及到的一个关键环节就是机动检测问题。

滤波器开始工作于正常模式,其输出的新息序列为)(k v ,令
)()1()(k k k δαμμ+-= )()()()(1k v k S k v k T -=δ
其中,)(k S 是)(k v 的协方差矩阵。

取1)1(--=∆α作为检测机动的有效窗口长度,机动检测的方法为：
1.如果Th k ≥)(μ,则认为目标在1-∆-k 开始有一恒定的加速度加入,这时目标模型应由低阶模型转向高阶模型。

由高阶机动模型退回低阶非机动模型的检测方法是检测加速度估计值是否有统计显著性意义。

令 ∑-∆-=
k
j a
a j k 1
)()(δ
μ
其中，)/(ˆ)]/()[/(ˆ)(1k k a k k P k k a
k m a T a -=δ，)/.(ˆk k a 是加速度分量的估计值,)/(k k P m a 是协方差矩阵的对应块。

2．如果Ta k a <)(μ，则加速度估计无显著性意义,滤波器退出机动模型。

前面提到了起始状态的初始化，下面讲一下当检测到机动时，滤波器的初始化问题。

当在第K 次检测到机动时,滤波器假定在1-∆-k 开始有一恒定的加速度,
在窗内的状态估计应修正如下: 首先,加速度在∆-k 的估计为
)]1/(ˆ)([2)/(ˆ2-∆-∆--∆-=
∆-∆-k k Z k Z T k k a x x m x )]1/(ˆ)([2)/(ˆ2-∆-∆--∆-=∆-∆-k k Z k Z T
k k a y y m y x
Z ˆ是对测量的预测值, )1/(ˆ)1/(ˆ-∆-∆-=-∆-∆-k k x k k Z x )1/(ˆ)1/(ˆ-∆-∆-=-∆-∆-k k y k k Z y
在∆-k 的位置估计为:
)()1/(ˆ∆-=-∆-∆-k Z k k x
x m )()1/(ˆ∆-=-∆-∆-k Z k k y
y m 在∆-k 的速度估计为:
)1/1()1/1()/(-∆--∆-+-∆--∆-=∆-∆-k k Ta k k V k k V x x m x
)1/1()1/1()/(-∆--∆-+-∆--∆-=∆-∆-k k Ta k k V k k V y y m y
协方差矩阵修正为:
1111)/(R k k P m =∆-∆-；11122)/(R T k k P m
=
∆-∆-；112152)/(R T
k k P m
=∆-∆- )]1/1()1/1(2)1/1([4
)/(222121111255-∆--∆-+-∆--∆-+-∆--∆-+=
∆-∆-k k P T k k TP k k P R T k k P m )]1/1(4)1/1()1/1(44)/(122211211222-∆--∆-+-∆--∆-+-∆--∆-+=∆-∆-k k P T k k P k k P T R T k k P m
)]1/1(6)1/1(2)1/1(44)/(1222211311325-∆--∆-+-∆--∆-+-∆--∆-+=∆-∆-k k P T
k k P T k k P T R T k k P m
四．仿真实验
假定有一二坐标雷达对一平面上运动的目标进行观测，目标在=t 0-400秒沿着y 轴作恒速直线运动，运动速度为-15米/秒，目标的起始点为(2000米,10000米)，在t= 400-600秒向轴x 方向做090的慢转弯，加速度为==y x u u 0.075米/秒2，完成慢转弯后加速度将降为零，从t=610秒开始做90度的快转弯，加速度为0.3米/秒2，在660秒结束转弯，加速度降至零。

雷达扫描周期T=2秒，X 和Y 独立地进行观测，观测噪声的标准差均为100米。

描述如下：
程序算法中各参数为:加权衰减因子85.0=α, 机动检测门限25=h T ；退出机动的检测门限49.9=a T 。

在跟踪的开始,首先采用非机动模型,从第20次采样开始,激活机动检测器。

0100200300400
-100
-500
50X 坐标 滤波误差均值曲线0100200300400
-50
50
100Y 坐标 滤波误差均值曲线
0100200300400
50100
150X 坐标 滤波误差标准差曲线0100200300400
50
100
150Y 坐标 滤波误差标准差曲线
通过上图，可看到：VD 算法有4次机动,分别对应目标的2次加速运动,和2次匀速运动,符合目标真实轨迹变化。

只是在模型出现机动的时候,会出现大的误差。

在模型的调整过程中,可以明显发现：机动检测门限h T ,退出机动的检测门限a T ,加权衰减因子 对算法的有效滤波有很大的影响，当目标快转弯时,会出现大的误差,这时候可以通过改变机动检测门限来减小。