基于IMM算法的目标跟踪.docx

1、基于IMM算法的目标跟踪基于交互式多模型方法的目标跟踪高海南目标建模我们设定一个目标在二维平面内运动，其状态 X(n)由位置、速度和加速度组成，即X(n) 4x(r), X& yn (y)h & T。假设采样间隔为T，目标检测概率PD = 1， 无虚警存在，在笛卡尔坐标系下目标的离散运动模型和观测模型 (假定在采样时 刻k)为：X k 1 二 FX k GV kZ ki； = H k X k W k目标在二维平面内运动模型如下：1. CV:近似匀速运动模型C V模型将加速度看作是随机扰动(状态噪声)，取目标状态X(n)=x(n),X(n), y(n), y(n)T。则状态转移矩阵，干扰转移矩阵

2、和观测矩阵分别为：0000j000,G =，H=11T0T2/20010 一01 一1 10T 一_1 T0 1 F =0 00 00 0 T2/2 02. CT:匀速转弯模型只考虑运动角速度已知的CT模型。则状态转移矩阵，干扰转移矩阵和观测矩阵分别为:量测噪声协方差矩阵R由传感器决定交互多模算法原理假定有r个模型:X(k+1) = FjX( k + G V )K jK , r其中，Wj k是均值为零、协方差矩阵为 Q j的白噪声序列。用一个马尔可夫链来控制这些模型之间的转换，马尔可夫链的转移概率矩阵为：Pl 1 L PrlP = MO M,Pri L P r 匚测量模型为：Z k = Hj

3、k X j k j kIMM算法步骤可归纳如下：1、输入交互rXj k1 /k 1八 X i lk/一 1j k- k1/ 1Poj k -1 /k - 1r -ij k 一1 /k 一 1 Pi k- 1k/ 1 X k- k1/ 1i 4 _Xoj (k1/k 1Xi(k 1k/-尸lXoj(k- /护出(k1 /k- 1=PMj(k- 1Mj(k) Z：=卩耳片(k- ) Cj /其中jll,r，向是模型i转到模型j的转移概率，Cj为规一化常数，rCj =為 PjJi k -1 。2、对应于模型Mj k，以Fj k-1/k -1，Poj k-1/k-1及Z k作为输入进行Kalman滤波

4、。1)预测Xj k/k 一 1=Fj k k- 1?0j k- k/ 12)预测误差方差阵Pjk/k-1=Fjkk- P%jk- k/ F1Tkk-, 1Gj k - 1 Q k - 1G： k - 13) 卡尔曼增益 一K j k = Pj k/k - 1 H T k i(H k P% k k - 1H Tk R k4)滤波Xj k/ k= X jk/k1 K j k Z k H Xkj / k k5)滤波误差方差阵Pj k/k；|l -Kj k H k Pjk/k-1、模型概率更新k 二PMj k / Z k=Pfz k /Mj k , Z kJ?P：M j k / ZkJ?1 r _j

5、k v 必叫 k 一1 j k Cj /cC i _1r其中，C为归一化常数，且c =為用.j k Cj,而上j k为观测Z k的似然函数,j 二(k ) = P Z (k )/M j(k ),ZkAUn exJ-1 US/(k )u ) |Sj(町 I 2 丿u k 二Z k -H k Xj k/k -1 , Sj k 二H k Pjk/k-1HT k R k。、输出交互rX k/k Q k/kkP k/ k 八叫 k Pj k k：；农j k k?X /k ? X /k ?kX / k k三、仿真实验设定目标运动起始位置坐标（x, 丫）为（1000,1000）,初始速度为（10,10）,3

6、T采样间隔T=1s , CT模型运动的角速度 ，即做顺时针匀速转弯运动。270x和y独立地进行观测，观测标准差为 50米。目标在1150s运动模型为CV, 151270s运动模型为CT, 271400s运动模型为CV。目标运动真实轨迹和测量 轨迹如图1所示。图1目标运动轨迹在IMM滤波时，使用2个模型集，即CV、CT,假设我们已经知道CT模0.99 0.011型的目标运动角速度w, Markov转移矩阵P I “。进行蒙特卡洛仿0.01 0.99 一真，得到IMM滤波结果。将此滤波结果与单独的 CV、CT模型的标准卡尔曼滤 波结果对比，如图2所示。由图可知，CT模型滤波结果与真实值有较大偏差，

7、 在转弯时CV模型卡尔曼滤波结果偏离偏离真实值，而 IMM算法能较好的跟踪目标。图2各种滤波结果图为了定量分析滤波结果，我们将 X、丫方向的CV、CT卡尔曼滤波、IMM 滤波与真实值分别求位置偏差、均方根误差并进行进行对比，如图3、图4所示。 同时作出各个时刻 CV、CT的模型概率，如图 5所示。可以看到在转弯时刻(151270S期间，CT模型概率大于CV模型概率，此时IMM滤波主要取决于 CT模型，而在其他时刻，CT模型起主要作用，这与我们的经验知识一致。IMM 算法就是通过各模型概率的自动调整来完成对机动目标的跟踪， 相对于单一模型 滤波具有较理想的跟踪精度。图3位置滤波偏差图4位置滤波均

8、方根误差1、下面讨论不同的马尔科夫一步转移矩阵对跟踪结果的影响。05 0.51（1）P2= | 时，模型概率和滤波结果如下图所示， 此时CV、CT0.5 0.5-模型概率变化趋势不变，但相差不大，显然 IMM算法优于单模型Kalman滤波算法，但其精度低于当转移矩阵为 P1时的结果。图6转移矩阵为P2时概率变化图图7转移矩阵为P2时滤波结果图0.1 0.91 一（2）更极端地，取P3 = | 时，模型概率和滤波结果如下图所示，0.9 0.1 一此时CV、CT模型概率变化趋势总体不变，但相差甚微，而 IMM算法总体上仍优于单模型Kalman滤波算法，但其精度同样低于当转移矩阵为 P1和P2时的结

9、果。图8转移矩阵为P3时概率变化图图9转移矩阵为P3时滤波结果图综上所述，Markov链状态转移矩阵对角线元素越大，即由 k-1时刻模型ml 转移到k时刻模型ml概率越大，也就是模型的“惯性”越大，交互式多模型滤 波结果精度越高，反之，精度越低。2、CT模型角速度对滤波结果的影响不变，仿真得到如下结果，与图2对比可知，当角速度越接近于真实值，跟踪 精度越高，反之跟踪精度有所下降。图10角速度* =-pi/360时滤波结果图通过编写程序和仿真实验结果可以体会到，IMM算法核心在于对做复杂机 动运动的目标滤波时，IMM能够通过对各个模型滤波器的输入输出通过混合概 率和似然函数计算进行加权综合处理，

10、自动切换模型，实现对目标的较好跟踪。 IMM算法跟踪性能好坏取决于其使用的模型集，模型越精确，模型集越丰富， 跟踪效果就越好，但带来了计算量增加的问题，有时反而降低性能。clear ;clcN = 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.1A2 0;0 0.1A2;%model-2,匀速转弯模型 A2=CreatCTF(-pi/270,T);G2=Crea

11、tCTT(T);Q2=0.0144A2 0;0 0.0144A2;%产生真实数据附：IMM滤波程序produce_data.mx = x0;for k = 1:150% 匀速直线x = A1*x + G1*sqrt(Q1)*ra ndn,randn ;xA =xA x;endfor k = 1:120% 匀速圆周转弯x = A2*x + G2*sqrt(Q2)*ra ndn ,ra ndn ; xA =xA x;endfor k = 1:130% 匀速直线x = A1*x + G1*sqrt(Q1)*ra ndn,randn ;xA =xA x;endplot(xA(1,:),xA(3,:),

12、b-)save(data,xA,A1,N,x0,G1,Q1,A2,G2,Q2);title(运动轨迹)xlabel(t(s),ylabel(位置(m);fun ctio n P_k,P_k_k_1,X_k=kalma n( 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 为观测数

13、据% 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*

14、H * i nv(H*P_k_k_1*H + R) * H * P_k_k_1;IMM_TEST.mclear all;clc;H = 1 0 0 0;0 0 1 0;R = 2500 0;0 2500;load data.matMC=50;ex 仁 zeros(MC,N);ex2=zeros(MC,N);ey仁 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

15、;0 0 100 0;0 0 0 1;% 初始协方差 X1_k_仁x0;X2_k_仁x0; %1-r(r=2)每个模型的状态传播参数 P1=P0;P2=P0; %1-r(r=2)每个模型的状态传播参数%CV模型卡尔曼滤波for i=1:400zA(:,i) = H*xA(:,i) + sqrt(R)*ra ndn ,ra ndn ; P_kv,P_k_k_1v,X1_kv=kalma n(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);% 改变角速度

16、 wx0 = 1000,10,1000,10;P0 = 100 0 0 0;0 1 0 0 ;0 0 100 0;0 0 0 1;for i=1:400P_kt,P_k_k_1t,X1_kt=kalma n(A2,G2,H,Q2,R,zA(:,i),x0,P0); X_kt(:,i)=X1_kt;x0=X1_kt;P0=P_kt;endex1(m n,:)=X_kv(1,:)-xA(1,:);ey1(m n,:)=X_kv(3,:)-xA(3,:);ex2(m n,:)=X_kt(1,:)-xA(1,:);ey2(m n,:)=X_kt(3,:)-xA(3,:);%IMM 滤波初始化参数 %

17、%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 = 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;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

18、+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;%预测残差及协方差计

19、算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* in v(Sv1)*v1);like2=det(2*pi*Sv2)A(-0.5)*exp(-0.5*v2*i nv(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;%模型概率更新

20、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;endex3( mn ,:)=X_imm(1,:)-xA(1,:);ey3(m n,:)=X_imm(3,:)-xA(3,:);UM1( mn ,:)=um1;UM2( mn ,:)=um2;endEX1=sqrt(sum(ex1.A2,1)/MC);EX2=sqrt(sum(ex2.A2,1)/MC);EX3=sqrt(sum(ex1.

21、A2,1)/MC);EY仁sqrt(sum(ey1A2,1)/MC);EY2=sqrt(sum(ey2A2,1)/MC);EY3=sqrt(sum(ey3.A2,1)/MC); mex1=mea n( ex1);mey1=mea n( ey1);%CV mex2=mea n( ex2);mey2=mea n( ey2);%CT mex3=mea n(ex3);mey3=mea n( ey3);%IMMUm 仁mean (UM1);Um2=mea n(UM2);t=1:400;figure(1)plot(xA(1,t),xA(3,t),k ,zA(1,t),zA(2,t),g-,xA(1,t)+

22、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);lege nd(真实值,测量值,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);

23、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模型概率变化)