基于IMM算法的目标跟踪.docx
《基于IMM算法的目标跟踪.docx》由会员分享，可在线阅读，更多相关《基于IMM算法的目标跟踪.docx（16页珍藏版）》请在冰点文库上搜索。
基于IMM算法的目标跟踪
基于交互式多模型方法的目标跟踪
高海南
目标建模
我们设定一个目标在二维平面内运动，其状态X（n）由位置、速度和加速度
组成，即X（n）4x（r）,X&yn（y）h&T。
假设采样间隔为T，目标检测概率PD=1，无虚警存在，在笛卡尔坐标系下目标的离散运动模型和观测模型（假定在采样时刻k）为：
Xk1二FXkGVk
Zki；=HkXkWk
目标在二维平面内运动模型如下：
1.CV:
近似匀速运动模型
CV模型将加速度看作是随机扰动（状态噪声），取目标状态
X（n）=[x（n）,X（n）,y（n）,y（n）]T。
则状态转移矩阵，干扰转移矩阵和观测矩阵分别为：
0
0
0
0
j
0
0
0〕
G=
，H
=1
1
T
0
T2/2
〔0
0
1
0一
0
1一
11
0
T一
_1T
01F=
00
00
00〕T2/20
2.CT:
匀速转弯模型
只考虑运动角速度已知的CT模型。
则状态转移矩阵，干扰转移矩阵和观
测矩阵分别为:
量测噪声协方差矩阵R由传感器决定
交互多模算法原理
假定有r个模型:
X（k+1）=FjX（k+GV）KjK,r
其中，Wjk是均值为零、协方差矩阵为Qj的白噪声序列。
用一个马尔可
夫链来控制这些模型之间的转换，马尔可夫链的转移概率矩阵为：
Pl1LPrl
P=MOM
PriLPr匚
测量模型为：
Zk=HjkXjkjk
IMM算法步骤可归纳如下：
1、输入交互
r
X°jk—1/k—1八Xi^lk/一・1jk-k1—/1
Pojk-1/k-1
r
—-ijk一1/k一1Pik-1k/—1Xk-k1—/1
i4_
—Xoj（k—1/k—1「Xi（k—1k/-尸lXoj（k-«/护
出（k—1/k-1=P{Mj（k-1Mj（k）Z：
」}=卩耳片（k-）Cj/
其中j"」ll,r，向是模型i转到模型j的转移概率，Cj为规一化常数，
r
Cj=為PjJik-1。
2、对应于模型Mjk，以Fjk-1/k-1，Pojk-1/k-1及Zk作为输
入进行Kalman滤波。
1）预测
Xjk/k一1=Fjkk-1?
0jk-k/1
2）预测误差方差阵
Pjk/k-1=Fjkk-P%jk-k/F1Tkk-,1
Gjk-1Qk-1G：
k-1
3）卡尔曼增益一
Kjk]=Pjk/k-1HTki（HkP%kk-1HTkRk
4）滤波
Xjk/k[=Xjk/k1KjkZkHXkj/kk
5）滤波误差方差阵
Pjk/k；」||l-KjkHkPjk/k-1
③、模型概率更新
k二P^Mjk/Zk[=Pfzk/Mjk,ZkJ?
P：
Mjk/ZkJ?
1r_
jkv必叫k一1jkCj/c
Ci_1
r
其中，C为归一化常数，且c=為用.jkCj,而上jk为观测Zk的似然函数,
j二
'（k）=P{Z（k）/Mj（k）,ZkAU—n^——exJ-1US/（k）u]
◎）|Sj（町I2丿
uk二Zk-HkXjk/k-1,Sjk二HkPjk/k-1HTkRk。
④、输出交互
r
Xk/k「Qk/kk
Pk/k八叫k]Pjkk：
；[农jkk?
X/k?
X/k?
kX/kk
三、仿真实验
设定目标运动起始位置坐标（x,丫）为（1000,1000）,初始速度为（10,10）,
3T
采样间隔T=1s,CT模型运动的角速度，即做顺时针匀速转弯运动。
270
x和y独立地进行观测，观测标准差为50米。
目标在1~150s运动模型为CV,151~270s运动模型为CT,271~400s运动模型为CV。
目标运动真实轨迹和测量轨迹如图1所示。
图1目标运动轨迹
在IMM滤波时，使用2个模型集，即CV、CT,假设我们已经知道CT模
■0.990.011
型的目标运动角速度w,Markov转移矩阵P^I°“。
进行蒙特卡洛仿
]0.010.99一
真，得到IMM滤波结果。
将此滤波结果与单独的CV、CT模型的标准卡尔曼滤波结果对比，如图2所示。
由图可知，CT模型滤波结果与真实值有较大偏差，在转弯时CV模型卡尔曼滤波结果偏离偏离真实值，而IMM算法能较好的跟踪
目标。
图2各种滤波结果图
为了定量分析滤波结果，我们将X、丫方向的CV、CT卡尔曼滤波、IMM滤波与真实值分别求位置偏差、均方根误差并进行进行对比，如图3、图4所示。
同时作出各个时刻CV、CT的模型概率，如图5所示。
可以看到在转弯时刻
（151~270S期间，CT模型概率大于CV模型概率，此时IMM滤波主要取决于CT模型，而在其他时刻，CT模型起主要作用，这与我们的经验知识一致。
IMM算法就是通过各模型概率的自动调整来完成对机动目标的跟踪，相对于单一模型滤波具有较理想的跟踪精度。
图3位置滤波偏差
图4位置滤波均方根误差
1、下面讨论不同的马尔科夫一步转移矩阵对跟踪结果的影响。
050.51
（1）P2=|时，模型概率和滤波结果如下图所示，此时CV、CT
0.50.5-
模型概率变化趋势不变，但相差不大，显然IMM算法优于单模型Kalman滤波
算法，但其精度低于当转移矩阵为P1时的结果。
图6转移矩阵为P2时概率变化图
图7转移矩阵为P2时滤波结果图
■0.10.91一
（2）更极端地，取P3=|时，模型概率和滤波结果如下图所示，
〔0.90.1一
此时CV、CT模型概率变化趋势总体不变，但相差甚微，而IMM算法总体上仍
优于单模型Kalman滤波算法，但其精度同样低于当转移矩阵为P1和P2时的结
果。
图8转移矩阵为P3时概率变化图
图9转移矩阵为P3时滤波结果图
综上所述，Markov链状态转移矩阵对角线元素越大，即由k-1时刻模型ml转移到k时刻模型ml概率越大，也就是模型的“惯性”越大，交互式多模型滤波结果精度越高，反之，精度越低。
2、CT模型角速度「对滤波结果的影响
不变，仿真得到如下结果，与图2对比可知，当角速度「越接近于真实值，跟踪精度越高，反之跟踪精度有所下降。
图10角速度*■=-pi/360时滤波结果图
通过编写程序和仿真实验结果可以体会到，IMM算法核心在于对做复杂机动运动的目标滤波时，IMM能够通过对各个模型滤波器的输入输出通过混合概率和似然函数计算进行加权综合处理，自动切换模型，实现对目标的较好跟踪。
IMM算法跟踪性能好坏取决于其使用的模型集，模型越精确，模型集越丰富，跟踪效果就越好，但带来了计算量增加的问题，有时反而降低性能。
clear;clc
N=400;T=1;
x0=[1000,10,1000,10]';xA=[];zA=[x0
（1）,x0（3）];
%model-1,匀速运动
A1=[1,T,0,0;
0,1,0,0;
0,0,1,T;
0,0,0,1];
G1=[TA2/2,0;
T,0;
0,TA2/2;
0,T];
Q1=[0.1A20;
00.1A2];
%model-2,匀速转弯模型A2=CreatCTF（-pi/270,T）;
G2=CreatCTT（T）;
Q2=[0.0144A20;
00.0144A2];
%产生真实数据
附：
IMM滤波程序
produce_data.m
x=x0;
fork=1:
150%匀速直线
x=A1*x+G1*sqrt（Q1）*[randn,randn]';
xA=[xAx];
end
fork=1:
120%匀速圆周转弯
x=A2*x+G2*sqrt（Q2）*[randn,randn]';xA=[xAx];
end
fork=1:
130%匀速直线
x=A1*x+G1*sqrt（Q1）*[randn,randn]';
xA=[xAx];
end
plot（xA（1,:
）,xA（3,:
）,'b-'）
save（'data','xA','A1','N','x0','G1','Q1','A2','G
2','Q2'）;
title（'运动轨迹'）
xlabel（'t（s）'）,ylabel（'位置（m）'）;
function[P_k,P_k_k_1,X_k]=kalman（F,T,H,Q,R,Z,X0,P0）
%%KALMAN滤波函数：
[P_k,K_k,X_k]=kalman（F,T,H,Q,R,Z,x0,p0）
%%模型为X（K+1）=F（k）X（k）+T（k）W（k）
%%输入参数：
%%F,T分别为：
系统状态转移矩阵、噪声矩阵
%%H为转换到笛卡尔坐标系下的观测矩阵
%%Q,R分别是W（k）、V（k）的协方差矩阵
%%Z为观测数据
%%x0，p0为一步滤波的起始条件
%%输出参数：
%%P_k为k时刻滤波误差协方差阵
%%P_k_k_1为对k时刻滤波误差协方差阵的估计
%%X_k为滤波更新值
%一步提前预测值和预测误差的协方差阵分别是：
X_k_k_1=F*X0;%k-1时刻对k时刻x值的预测
P_k_k_1=F*P0*F'+T*Q*T';%k-1时刻对k时刻p值的预测
K_k=P_k_k_1*H'*inv（H*P_k_k_1*H'+R）;%k时刻kalman滤波增益
X_k=X_k_k_1+K_k*（Z-H*X_k_k_1）;
P_k=P_k_k_1-P_k_k_1*H'*inv（H*P_k_k_1*H'+R）*H*P_k_k_1;
IMM_TEST.m
clearall;clc;
H=[1000;0010];
R=[25000;02500];
loaddata.mat
MC=50;
ex仁zeros（MC,N）;ex2=zeros（MC,N）;
ey仁zeros（MC,N）;ey2=zeros（MC,N）;
%蒙特卡罗仿真
formn=1:
MC
%模型初始化
Pi=[0.99,0.01;0.01,0.99];%转移概率
u1=1/2;u2=1/2;%2个模型间概率传播参数
U0=[u1,u2];
P0=[100000;0100;001000;0001;];%初始协方差X1_k_仁x0;X2_k_仁x0;%1-r（r=2）每个模型的状态传播参数P1=P0;P2=P0;%1-r（r=2）每个模型的状态传播参数
%CV模型卡尔曼滤波
fori=1:
400
zA（:
i）=H*xA（:
i）+sqrt（R）*[randn,randn]';[P_kv,P_k_k_1v,X1_kv]=kalman（A1,G1,H,Q1,R,zA（:
i）,x0,P0）;
X_kv（:
i）=X1_kv;
x0=X1_kv;P0=P_kv;
end
%CT模型卡尔曼滤波
A2=CreatCTF（-pi/270,1）;%改变角速度w
x0=[1000,10,1000,10]';
P0=[100000;0100;001000;0001];
fori=1:
400
[P_kt,P_k_k_1t,X1_kt]=kalman（A2,G2,H,Q2,R,zA（:
i）,x0,P0）;X_kt（:
i）=X1_kt;
x0=X1_kt;P0=P_kt;
end
ex1（mn,:
）=X_kv（1,:
）-xA（1,:
）;ey1（mn,:
）=X_kv（3,:
）-xA（3,:
）;
ex2（mn,:
）=X_kt（1,:
）-xA（1,:
）;ey2（mn,:
）=X_kt（3,:
）-xA（3,:
）;
%%%%%%%%%%IMM滤波初始化参数%%%%%%%%%%%
x0=[1000,10,1000,10]';
x1_k_仁x0;x2_k_1=x0;%1-r（r=2）每个模型的状态传播参数
P仁P0;P2=P0;%1-r（r=2）每个模型的状态传播参数
P0=[100000;
0100;
001000;
0001];
fork=1:
400%1-400秒
%混合概率
c1=Pi（1,1）*u1+Pi（2,1）*u2;
c2=Pi（1,2）*u1+Pi（2,2）*u2;
u1仁Pi（1,1）*u1/c1;u12=Pi（1,2）*u1/c2;
u2仁Pi（2,1）*u2/c1;u22=Pi（2,2）*u2/c2;
x1_m=x1_k_1*u11+x2_k_1*u21;
x2_m=x1_k_1*u12+x2_k_1*u22;
p1_k_仁（P1+（x1_k_1-x1_m）*（x1_k_1-x1_m）'）*u11+（P2+（x2_k_1-x1_m）*（x2_k_1-x1_m）'）*u21;
p2_k_1=（P1+（x1_k_1-x2_m）*（x1_k_1-x2_m）'）*u12+（P2+（x2_k_1-x2_m）*（x2_k_1-x2_m）'）*u22;
%状态预测
x1_pk1=A1*x1_m;x2_pk仁A2*x2_m;
p1_k=A1*p1_k_1*A1'+G1*Q1*G1';
p2_k=A2*p2_k_1*A2'+G2*Q2*G2';
%预测残差及协方差计算
zk=zA（:
k）;
v1=zk-H*x1_pk1;v2=zk-H*x2_pk1;
Sv仁H*p1_k*H'+R;Sv2=H*p2_k*H'+R;
like仁det（2*pi*Sv1）A（-0.5）*exp（-0.5*v1'*inv（Sv1）*v1）;
like2=det（2*pi*Sv2）A（-0.5）*exp（-0.5*v2'*inv（Sv2）*v2）;
%滤波更新
K仁p1_k*H'*inv（Sv1）;K2=p2_k*H'*inv（Sv2）;
Xk1=x1_pk1+K1*v1;xk2=x2_pk1+K2*v2;
P1=p1_k-K1*Sv1*K1';P2=p2_k-K2*Sv2*K2';
%模型概率更新
C=like1*c1+like2*c2;
U1=like1*c1/C;u2=like2*c2/C;
%估计融合
xk=xk1*u1+xk2*u2;
%迭代
x1_k_1=xk1;x2_k_仁xk2;
X_imm（:
k）=xk;um1（k）=u1;um2（k）=u2;
end
ex3（mn,:
）=X_imm（1,:
）-xA（1,:
）;
ey3（mn,:
）=X_imm（3,:
）-xA（3,:
）;
UM1（mn,:
）=um1;UM2（mn,:
）=um2;
end
EX1=sqrt（sum（ex1.A2,1）/MC）;EX2=sqrt（sum（ex2.A2,1）/MC）;EX3=sqrt（sum（ex1.A2,1）/MC）;
EY仁sqrt（sum（ey1A2,1）/MC）;EY2=sqrt（sum（ey2A2,1）/MC）;EY3=sqrt（sum（ey3.A2,1）/MC）;mex1=mean（ex1）;mey1=mean（ey1）;%CVmex2=mean（ex2）;mey2=mean（ey2）;%CTmex3=mean（ex3）;mey3=mean（ey3）;%IMM
Um仁mean（UM1）;Um2=mean（UM2）;
t=1:
400;
figure
（1）
plot（xA（1,t）,xA（3,t）,'k',zA（1,t）,zA（2,t）,'g-',xA（1,t）+mex3（t）,xA（3,t）+mey3（t）,...
'r',xA（1,t）+mex1（t）,xA（3,t）+mey1（t）,'b',xA（1,t）+mex2（t）,xA（3,t）+mey2（t）,'m'）;
legend（'真实值','测量值','IMM滤波','CV模型滤波','CT模型滤波',2）;
figure
（2）
subplot（2,1,1）
plot（t,mex1（t）,'b',t,mex2（t）,'m',t,mex3（t）,'r'）;
title（'X方向滤波误差'）
legend（'CV模型滤波','CT模型滤波','IMM滤波',3）;
subplot（2,1,2）
plot（t,mey1（t）,'b',t,mey2（t）,'m',t,mey3（t）,'r'）;
legend（'CV模型滤波','CT模型滤波','IMM滤波',4）;
title（'Y方向滤波误差'）
xlabel（'t（s）'）,ylabel（'位置误差（m）'）;
figure（3）
subplot（2,1,1）
plot（t,EX1（t）,'b',t,EX2（t）,'m',t,EX3（t）,'r'）;
title（'X方向RMSE'）
legend（'CV模型滤波','CT模型滤波','IMM滤波',2）;
subplot（2,1,2）
plot（t,EY1（t）,'b',t,EY2（t）,'m',t,EY3（t）,'r'）;
legend（'CV模型滤波','CT模型滤波','IMM滤波',2）;
title（'Y方向RMSE'）
xlabel（'t（s）'）,ylabel（'位置误差（m）'）;
figure（4）
plot（t,Um1（t）,'b-',t,Um2（t）,'m-'）;
legend（'CV模型概率','CT模型概率',1）;
title（'CV、CT模型概率变化'）
升级会员 