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

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

摘要：机动卡尔曼算法(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米。描述如下：