基于IMM算法的目跟踪.docx

基于IMM算法的目跟踪西安交通大学 多源信息融合-IMM算法 基于交互式多模型方法的目标跟踪 高海南 3110038011 一、 目标建模 我们设定一个目标在二维平面内运动，其状态X(n)由位置、速度和加速度&组成，即X(n)=x(n),x(n),(y)n,(y)n&T。假设采样间隔为T，目标检测概率PD=1，无虚警存在，在笛卡尔坐标系下目标的离散运动模型和观测模型 (假定在采样时刻k)为： X(k+1)=FX(k)+GV(k) Z(k)=H(k)X(k)+W(k) 目标在二维平面内运动模型如下： 1. CV:近似匀速运动模型 CV模型将加速度看作是随机扰动，取目标状态 && X(n)=x(n),x(n),y(n),y(n)T。则状态转移矩阵，干扰转移矩阵和观测矩阵分别为：é1ê0F=êê0êë0T00ùéT2/20ùêú100ú00ú，H=é1000ùú,G=êê0010úê001TúT2/2úëû êúú001ûTûë02. CT：匀速转弯模型 只考虑运动角速度w已知的CT模型。则状态转移矩阵，干扰转移矩阵和观测矩阵分别为： sin(wT)é1êwê0cos(wT)F=êê1-cos(wT)ê0w2êêsin(wT)ë00-0101-cos(wT)ùúéT2/20ùw2úêúsin(wT)ú00úH=é1000ùG=êê0010úê0sin(wT)úT2/2úëûúêúwTûúë0cos(wT)úû量测噪声协方差矩阵R由传感器决定。二、 交互多模算法原理 假定有r个模型： 1 西安交通大学 多源信息融合-IMM算法 X(k+1)=F,=j1,K , r )+G(j)kjX(kjV其中，Wj(k)是均值为零、协方差矩阵为Qj的白噪声序列。用一个马尔可夫链来控制这些模型之间的转换，马尔可夫链的转移概率矩阵为： ép11L P=êêMOêëpr1Lpr1ùMúú prúrû测量模型为： Z(k)=Hj(k)XIMM算法步骤可归纳如下： 、输入交互 j()k+W(j) k Xoji(1=åXk-(k-1/k-)i=1r1k/-)m1ij(k-k1-/)1Poj(k-1/k-)1 =åmij(k-1/k-)1Pi(k-1k/-i=1ri(k-1éX)+ëoj(k-1X)-k1-/)k1-/)ùû1T1 oj(k-1/k-)i(k-1k/-éX-X1ùûëmij(k-1/k-)1=PMi(k-)1M/i(k)Zk,-1=pijmi(k-)c1j/其中j=1,K,r，pij是模型i转到模型j的转移概率，cj为规一化常数，cj=åpijmi(k-1)。 i=1roj(k-1/k-1)，Poj(k-1/k-1)及Z(k)作为输、对应于模型Mj(k)，以X入进行Kalman滤波。 1）预测 j(k/k-10j(k-1 X ,-)1Xk-/) 1)=Fj(kk2）预测误差方差阵 j0jPX,-)1PX)=Fj(kk%(k/k-1%(k- +Gj(k-1)Q(k-T)F1j(k1GT)j(k-)11k-/k-,)13）卡尔曼增益 -1jTjTéùk/k-1HkHkPkk/-1Hk+Rk Kj(k)=PX)()ë()X)()()û %(%(4）滤波 jj(k/k)=Xj(k/-k(éZ()-k)1+Kj()kkHX/-k)ùk1 X()ëû5）滤波误差方差阵 jjùI-KkHkP1 PX()()%(k/k)=é%jëûX(k/k-)、模型概率更新 2 西安交通大学 多源信息融合-IMM算法 mj(k)=PMj(k)/Zk=PZ(k)/Mj(k),Zk-1PMj(k)/Zk-1 r1=Lj(k)åpijmi(k-1)=Lj(k)cj/cci=1其中，c为归一化常数，且c=åLj(k)cj，而Lj(k)为观测Z(k)的似然函数，j=1rLj(k)=PZ(k)/Mj(k),Zk-1=1(2p)n/2ì1T-1üexp-Sk()íýjjj1/22îþSj(k)jTj(k/k-1)，S(k)=H(k)P%j(k)=Z(k)-H(k)Xk/k-1H()(k)+R(k)。 jX、输出交互 (k/k)=åXj(k/k)m(k) Xjj=1rXj( P(k/k)=åmj(k)Pj(k/)k+éëj=1r/k)k-X(jXé/kk)ù(ûëk/)k-(X)/T ù kkû三、 仿真实验 设定目标运动起始位置坐标为，初始速度为，采样间隔T=1s ，CT模型运动的角速度w=-p270，即做顺时针匀速转弯运动。x和y独立地进行观测，观测标准差为50米。目标在1150s运动模型为CV，151270s运动模型为CT， 271400s运动模型为CV。目标运动真实轨迹和测量轨迹如图1所示。 图1 目标运动轨迹 3 西安交通大学 多源信息融合-IMM算法 在IMM滤波时，使用2个模型集，即CV、CT，假设我们已经知道CT模é0.990.01ùP1=型的目标运动角速度w，Markov转移矩阵ê0.010.99ú。进行蒙特卡洛仿ëû真，得到IMM滤波结果。将此滤波结果与单独的CV、CT模型的标准卡尔曼滤波结果对比，如图2所示。由图可知，CT模型滤波结果与真实值有较大偏差，在转弯时CV模型卡尔曼滤波结果偏离偏离真实值，而IMM算法能较好的跟踪目标。 图2 各种滤波结果图 为了定量分析滤波结果，我们将X、Y方向的CV、CT卡尔曼滤波、IMM滤波与真实值分别求位置偏差、均方根误差并进行进行对比，如图3、图4所示。同时作出各个时刻CV、CT的模型概率，如图5所示。可以看到在转弯时刻期间，CT模型概率大于CV模型概率，此时IMM滤波主要取决于CT模型，而在其他时刻，CT模型起主要作用，这与我们的经验知识一致。IMM算法就是通过各模型概率的自动调整来完成对机动目标的跟踪，相对于单一模型滤波具有较理想的跟踪精度。 4 西安交通大学 多源信息融合-IMM算法 图3 位置滤波偏差 图4 位置滤波均方根误差 5 西安交通大学 多源信息融合-IMM算法 图5 各时刻CV、CT模型概率 1、下面讨论不同的马尔科夫一步转移矩阵对跟踪结果的影响。 é0.50.5ùP2=ê时，模型概率和滤波结果如下图所示，此时CV、CTúë0.50.5û模型概率变化趋势不变，但相差不大，显然IMM算法优于单模型Kalman滤波算法，但其精度低于当转移矩阵为P1时的结果。 图6 转移矩阵为P2时概率变化图 6 西安交通大学 多源信息融合-IMM算法 图7 转移矩阵为P2时滤波结果图 é0.10.9ù更极端地，取P3=êú时，模型概率和滤波结果如下图所示，0.90.1ëû此时CV、CT模型概率变化趋势总体不变，但相差甚微，而IMM算法总体上仍优于单模型Kalman滤波算法，但其精度同样低于当转移矩阵为P1和P2时的结果。 图8 转移矩阵为P3时概率变化图 7 西安交通大学 多源信息融合-IMM算法 图9 转移矩阵为P3时滤波结果图 综上所述，Markov链状态转移矩阵对角线元素越大，即由k-1时刻模型m1转移到k时刻模型m1概率越大，也就是模型的“惯性”越大，交互式多模型滤波结果精度越高，反之，精度越低。 2、CT模型角速度w对滤波结果的影响 é0.990.01ùpP1=w=-取Markov转移矩阵，而角速度，其他参数均ê0.010.99ú360ëû不变，仿真得到如下结果，与图2对比可知，当角速度w越接近于真实值，跟踪精度越高，反之跟踪精度有所下降。 图10 角速度w=-pi/360时滤波结果图 8 西安交通大学 多源信息融合-IMM算法 通过编写程序和仿真实验结果可以体会到，IMM算法核心在于对做复杂机动运动的目标滤波时，IMM能够通过对各个模型滤波器的输入输出通过混合概率和似然函数计算进行加权综合处理，自动切换模型，实现对目标的较好跟踪。IMM算法跟踪性能好坏取决于其使用的模型集，模型越精确，模型集越丰富，跟踪效果就越好，但带来了计算量增加的问题，有时反而降低性能。 9 西安交通大学 多源信息融合-IMM算法 附：IMM滤波程序 produce_data.m 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=T2/2, 0; T, 0; 0, T2/2; 0, T ; Q1=0.12 0; 0 0.12; %model-2,匀速转弯模型 A2=CreatCTF(-pi/270,T); G2=CreatCTT(T); Q2=0.01442 0; 0 0.01442; % 产生真实数据 x = x0; for k = 1:150%匀速直线 x = A1*x + G1*sqrt(Q1)*randn,randn' xA =xA x; end for k = 1:120%匀速圆周转弯 x = A2*x + G2*sqrt(Q2)*randn,randn' xA =xA x; end for k = 1:130%匀速直线 x = A1*x + G1*sqrt(Q1)*randn,randn' xA =xA x; end plot(xA(1,:),xA(3,:),'b-') save('data','xA','A1','N','x0','G1','Q1','A2','G2','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+ TW % 输入参数： % 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; 10 西安交通大学 多源信息融合-IMM算法 function F=CreatCTF(w,t) %CT模型的状态转移矩阵 f1=1; f2=sin(w*t)/w; f3=(1-cos(w*t)/w; f4=cos(w*t); f5=sin(w*t); F= f1 f2 0 -f3 ; 0 f4 0 -f5 ; 0 f3 f1 f2 ; 0 f5 0 f4; function T=CreatCTT(t) % 函数作用：产生匀加速模型的噪声矩阵T % 输入参数 ：t为时间间隔 % 输出参数： 噪声矩阵T T=t2/2 0 ; t 0 ; 0 t2/2 ; 0 t ; %噪声矩阵 IMM_TEST.m clear all;clc; H = 1 0 0 0;0 0 1 0; R = 2500 0;0 2500; load data.mat MC=50; ex1=zeros(MC,N);ex2=zeros(MC,N); ey1=zeros(MC,N);ey2=zeros(MC,N); %蒙特卡罗仿真 for mn=1:MC %模型初始化 Pi =0.99,0.01;0.01,0.99;%转移概率 u1=1/2; u2=1/2;%2个模型间 概率传播参数 U0 = u1,u2; P0 = 100 0 0 0;0 1 0 0;0 0 100 0;0 0 0 1;%初始协方差 X1_k_1=x0;X2_k_1=x0; %1-r(r=2)每个模型的状态传播参数 P1=P0;P2=P0; %1-r(r=2)每个模型的状态传播参数 %CV模型卡尔曼滤波 for i=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 = 100 0 0 0;0 1 0 0 ;0 0 100 0;0 0 0 1; for i=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; 11 西安交通大学 多源信息融合-IMM算法 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_1=x0;x2_k_1=x0; %1-r(r=2)每个模型的状态传播参数 P1=P0;P2=P0; %1-r(r=2)每个模型的状态传播参数 P0 = 100 0 0 0; 0 1 0 0 ; 0 0 100 0; 0 0 0 1; for k = 1:400%1-400秒 %混合概率 c1=Pi(1,1)*u1+Pi(2,1)*u2; c2=Pi(1,2)*u1+Pi(2,2)*u2; u11=Pi(1,1)*u1/c1;u12=Pi(1,2)*u1/c2; u21=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_1=(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_pk1=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; Sv1=H*p1_k*H'+R;Sv2=H*p2_k*H'+R; like1=det(2*pi*Sv1)(-0.5)*exp(-0.5*v1'*inv(Sv1)*v1); like2=det(2*pi*Sv2)(-0.5)*exp(-0.5*v2'*inv(Sv2)*v2); %滤波更新 K1=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_1=xk2; 12 西安交通大学 多源信息融合-IMM算法 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.2,1)/MC);EX2=sqrt(sum(ex2.2,1)/MC);EX3=sqrt(sum(ex1.2,1)/MC); EY1=sqrt(sum(ey1.2,1)/MC);EY2=sqrt(sum(ey2.2,1)/MC);EY3=sqrt(sum(ey3.2,1)/MC); mex1=mean(ex1);mey1=mean(ey1);%CV mex2=mean(ex2);mey2=mean(ey2);%CT mex3=mean(ex3);mey3=mean(ey3);%IMM Um1=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模型概率变化')13