基于IMM算法的目标跟踪

基于交互式多模型方法的目标跟踪高海南一、 目标建模我们设定一个目标在二维平面内运动，其状态()X n 由位置、速度和加速度组成，即()[(),(),(),()]TX n x n xn 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 =&&。

则状态转移矩阵，干扰转移矩阵和观测矩阵分别为：22100/20010000,0010/200010TT F G T T T ⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥==⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦，10000010H ⎡⎤=⎢⎥⎣⎦2. CT ：匀速转弯模型只考虑运动角速度ω已知的CT 模型。

则状态转移矩阵，干扰转移矩阵和观测矩阵分别为：22()1()()sin()1()()sin()()1000010sin 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 Vk j r +=+=X X K 其中，()j k W 是均值为零、协方差矩阵为j Q 的白噪声序列。

用一个马尔可夫链来控制这些模型之间的转换，马尔可夫链的转移概率矩阵为：1111r r r r p p p p ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦P LM OM L测量模型为：()()()()j jj k H k k W k =+Z XIMM 算法步骤可归纳如下： ①、输入交互()()()()()()(){()()()}()()(){}()111ˆˆ1/11/11/11/1ˆ1/11/11/1ˆˆˆ1/11/11/11/11/,1/ro ji ij i oj ri i ij i Toj 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 μμμμ==---=------⎡=----+--⎣⎤⎡⎤--------⎦⎣⎦--=-=-∑∑XX P P X X X X Z其中1,,j r =，ij p 是模型i 转到模型j 的转移概率，j c 为规一化常数，()11rj 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 kk k -=---X X 2）预测误差方差阵()()()()()()()0/1,11/1,1111j j Tj j X X T j j k k F k k k k F kk G k k G k -=----+---P P Q %%3）卡尔曼增益()()()()()()()1/1/1j T j Tj 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 jj X X k k k H k k k ⎡⎤=--⎣⎦P I K P %% ③、模型概率更新()(){}()(){}(){}()()()111//,/11/k k k j j j j rj 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 为归一化常数，且()1rj j j c k c ==Λ∑，而()j k Λ为观测()k Z 的似然函数，()()(){}()()()111/2/211/,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 Xk k H k k k k H k k k H k k =--=-+υZ X S P R ，%。

④、输出交互()()()1ˆˆ//rj jj k k k k k μ==∑X X ()()()()(){()()}1ˆˆˆˆ//////rTj j j j j k k k k k k k k k k k k kμ=⎡⎤⎡⎤=+--⎣⎦⎣⎦∑P P X X XX三、 仿真实验设定目标运动起始位置坐标（x ，y ）为（1000,1000），初始速度为（10,10），采样间隔T=1s ，CT 模型运动的角速度270πω-=，即做顺时针匀速转弯运动。

x 和y 独立地进行观测，观测标准差为50米。

目标在1~150s 运动模型为CV ，151~270s 运动模型为CT ， 271~400s 运动模型为CV 。

目标运动真实轨迹和测量轨迹如图1所示。

图1 目标运动轨迹在IMM 滤波时，使用2个模型集，即CV 、CT ，假设我们已经知道CT 模型的目标运动角速度w ，Markov 转移矩阵⎥⎦⎤⎢⎣⎡=99.001.001.099.01P 。

进行蒙特卡洛仿真，得到IMM 滤波结果。

将此滤波结果与单独的CV 、CT 模型的标准卡尔曼滤波结果对比，如图2所示。

由图可知，CT 模型滤波结果与真实值有较大偏差，在转弯时CV 模型卡尔曼滤波结果偏离偏离真实值，而IMM 算法能较好的跟踪目标。

图2 各种滤波结果图为了定量分析滤波结果，我们将X 、Y 方向的CV 、CT 卡尔曼滤波、IMM 滤波与真实值分别求位置偏差、均方根误差并进行进行对比，如图3、图4所示。

同时作出各个时刻CV 、CT 的模型概率，如图5所示。

可以看到在转弯时刻（151~270s ）期间，CT 模型概率大于CV 模型概率，此时IMM 滤波主要取决于CT 模型，而在其他时刻，CT 模型起主要作用，这与我们的经验知识一致。

IMM 算法就是通过各模型概率的自动调整来完成对机动目标的跟踪，相对于单一模型滤波具有较理想的跟踪精度。

图3 位置滤波偏差图4 位置滤波均方根误差图5 各时刻CV 、CT 模型概率1、下面讨论不同的马尔科夫一步转移矩阵对跟踪结果的影响。

（1）⎥⎦⎤⎢⎣⎡=5.05.05.05.02P 时，模型概率和滤波结果如下图所示，此时CV 、CT 模型概率变化趋势不变，但相差不大，显然IMM 算法优于单模型Kalman 滤波算法，但其精度低于当转移矩阵为P1时的结果。

图6 转移矩阵为P2时概率变化图图7 转移矩阵为P2时滤波结果图（2）更极端地，取⎥⎦⎤⎢⎣⎡=1.09.09.01.03P 时，模型概率和滤波结果如下图所示，此时CV 、CT 模型概率变化趋势总体不变，但相差甚微，而IMM 算法总体上仍优于单模型Kalman 滤波算法，但其精度同样低于当转移矩阵为P1和P2时的结果。

图8 转移矩阵为P3时概率变化图图9 转移矩阵为P3时滤波结果图综上所述，Markov 链状态转移矩阵对角线元素越大，即由k-1时刻模型m1转移到k 时刻模型m1概率越大，也就是模型的“惯性”越大，交互式多模型滤波结果精度越高，反之，精度越低。

2、CT 模型角速度ω对滤波结果的影响取Markov 转移矩阵⎥⎦⎤⎢⎣⎡=99.001.001.099.01P ，而角速度360πω-=，其他参数均不变，仿真得到如下结果，与图2对比可知，当角速度ω越接近于真实值，跟踪精度越高，反之跟踪精度有所下降。

图10 角速度ω=-pi/360时滤波结果图通过编写程序和仿真实验结果可以体会到，IMM算法核心在于对做复杂机动运动的目标滤波时，IMM能够通过对各个模型滤波器的输入输出通过混合概率和似然函数计算进行加权综合处理，自动切换模型，实现对目标的较好跟踪。

IMM算法跟踪性能好坏取决于其使用的模型集，模型越精确，模型集越丰富，跟踪效果就越好，但带来了计算量增加的问题，有时反而降低性能。

附：IMM滤波程序produce_data.mclear ;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=[T^2/2, 0;T, 0;0, T^2/2;0, T] ;Q1=[0.1^2 0;0 0.1^2];%model-2,匀速转弯模型A2=CreatCTF(-pi/270,T); G2=CreatCTT(T);Q2=[0.0144^2 0;0 0.0144^2];% 产生真实数据x = x0;for k = 1:150%匀速直线x = A1*x + G1*sqrt(Q1)*[randn,randn]';xA =[xA x];endfor k = 1:120%匀速圆周转弯x = A2*x + G2*sqrt(Q2)*[randn,randn]';xA =[xA x];endfor k = 1:130%匀速直线x = A1*x + G1*sqrt(Q1)*[randn,randn]';xA =[xA x];endplot(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;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为时间间隔%% 输出参数：噪声矩阵TT=[t^2/2 0 ;t 0 ;0 t^2/2 ;0 t ;]; %噪声矩阵IMM_TEST.mclear all;clc;H = [1 0 0 0;0 0 1 0];R = [2500 0;0 2500];load data.matMC=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:400zA(:,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);%改变角速度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: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;endex1(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;X_imm(:,k)=xk;um1(k)=u1;um2(k)=u2;endex3(mn,:)=X_imm(1,:)-xA(1,:);ey3(mn,:)=X_imm(3,:)-xA(3,:);UM1(mn,:)=um1;UM2(mn,:)=um2;endEX1=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);%CVmex2=mean(ex2);mey2=mean(ey2);%CTmex3=mean(ex3);mey3=mean(ey3);%IMMUm1=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模型概率变化')。