基于IMM算法的目标跟踪

基于交互式多模型方法的目标跟踪

高海南

一、 目标建模

我们设定一个目标在二维平面内运动，其状态()X n 由位置、速度和加速度

组成，即()[(),(),(),()]T

X n x n x

n y n y n =&&。假设采样间隔为T ，目标检测概率1D P =，

无虚警存在，在笛卡尔坐标系下目标的离散运动模型和观测模型 (假定在采样时刻k )为：

()()()1X k FX k GV k +=+ ()()()()Z k H k X k W k =+

目标在二维平面内运动模型如下： 1. CV:近似匀速运动模型

C V 模型将加速度看作是随机扰动（状态噪声），取目标状态

()[(),(),(),()]T X n x n x n y n y n =&&。则状态转移矩阵，干扰转移矩阵和观测矩阵分别为：

221

00/20010000,0010/20

0010

T

T F G T T T ⎡⎤

⎡⎤⎢⎥

⎢⎥⎢

⎥⎢

⎥==⎢⎥⎢⎥⎢⎥⎢⎥

⎣⎦⎣⎦，10000010H ⎡⎤=⎢⎥⎣⎦

2. CT ：匀速转弯模型

只考虑运动角速度ω已知的CT 模型。则状态转移矩阵，干扰转移矩阵和观测矩阵分别为：

2

2()1()()sin()1()

()sin()()10000

10

sin T cos T cos T T cos T sin T T cos F T ωωωωωωωωωωωω⎡⎤

⎢⎥⎢

⎥⎢⎥=⎢⎥⎢⎥⎢

⎥⎢⎥⎣⎦

---22/20000/20T G T T ⎡⎤⎢⎥

⎢⎥=⎢⎥⎢⎥⎣⎦10000010H ⎡⎤=⎢⎥⎣⎦

量测噪声协方差矩阵R 由传感器决定。

二、 交互多模算法原理

假定有r 个模型：

()()()1,1,,j j j k F k G V

k j r +=+=X X K 其中，()j k W 是均值为零、协方差矩阵为j Q 的白噪声序列。用一个马尔可夫链来控制这些模型之间的转换，马尔可夫链的转移概率矩阵为：

1111r r r r p p p p ⎡⎤

⎢⎥=⎢⎥⎢⎥⎣⎦

P L

M O

M L

测量模型为：

()()()()j j

j k H k k W k =+Z X

IMM 算法步骤可归纳如下： ①、输入交互

()

()()

()()()(){

()()()}

()()(){}()

1

11ˆˆ1/11/11/1

1/1

ˆ1/11/11/1ˆˆˆ1/11/11/1

1/11/,1/

r

o j

i ij i oj r

i i ij i T

oj i oj k ij i i ij i j k k k k k k k k k k k k k k k k k k k k k k P M k M k p k c μμμμ==---=--

----⎡=----

+--⎣⎤⎡⎤------

--⎦⎣

⎦--=-=-

∑∑X

X P P X X X X Z

其中1,

,j r =，ij p 是模型i 转到模型j 的转移概率，j c 为规一化常数，

()1

1r

j ij i i c p k μ==-∑。

②、对应于模型()j M k ，以()ˆ1/1oj k k --X

，()1/1oj k k --P 及()k Z 作为输入进行Kalman 滤波。

1）预测

()()()0ˆˆ/1,11/1j j j k k F k k

k k -=---X X 2）预测误差方差阵

()()()()

()()()0/1,11/

1,1

111

j j T

j j X X T j j k k F k k k k F k

k G k k G k -=--

--+--

-P P Q %%

3）卡尔曼增益

()(

)()()()()()1/1/1j T j T

j X X k k k H k H k k k H k k -⎡⎤=--+⎣⎦K P P R %% 4）滤波

()()()()()()ˆˆˆ//1/1j j j j k k k k k k H k k k ⎡⎤=-+--⎣⎦X X K Z X 5）滤波误差方差阵

()()()()

//1j j

j X X k k k H k k k ⎡⎤=--⎣⎦P I K P %% ③、模型概率更新

()(){}()(){}(){}()()()111

//,/1

1/k k k j j j j r

j ij i j j i k P M k P k M k P M k k p k k c c c μμ--====Λ-=Λ∑Z Z Z Z

其中，c 为归一化常数，且()1

r

j j j c k c ==Λ∑，而()j k Λ为观测()k Z 的似然函数，

()()(){}()

()

()111/2

/2

1

1/,exp 22k T j j j j j n j k P k M k k k π--⎧⎫

Λ==

-⎨⎬⎩⎭Z Z υS υS ()()()()()()()()()ˆ/1/1j j T j j X

k k H k k k k H k k k H k k =--=-+υZ X S P R ，%。 ④、输出交互

()()()1ˆˆ//r

j j

j k k k k k μ==∑X X ()()()()

(

){

()()}

1

ˆˆˆˆ//////

r

T

j j j j j k k k k k k k k k k k k k

μ=⎡⎤⎡⎤=+--⎣⎦⎣⎦∑P P X X X

X

三、 仿真实验

设定目标运动起始位置坐标（x ，y ）为（1000,1000），初始速度为（10,10），采样间隔T=1s ，CT 模型运动的角速度270

π

ω-

=，即做顺时针匀速转弯运动。

x 和y 独立地进行观测，观测标准差为50米。目标在1~150s 运动模型为CV ，151~270s 运动模型为CT ， 271~400s 运动模型为CV 。目标运动真实轨迹和测量轨迹如图1所示。

图1 目标运动轨迹