IMMKF.rar_IMMF；kalman_IMMKf算法_monthxel_图像目标跟踪_跟踪

% 基于IMM算法的目标跟踪 clear all; clc; echo off; %=============================== %建立模型 %=============================== % 仿真参数 echo_num=100; %仿真迭代次数 Tsample=1; %采样时间 model2_w2=3*2*pi/360; %模型2转弯率3度 model3_w3=-3*2*pi/360; %模型3转弯率-3度 H=[1,0,0,0;0,0,1,0]; %模型量测矩阵 G=[Tsample^2/2,0;Tsample,0;0,Tsample^2/2;0,Tsample]; %模型过程噪声加权矩阵 r=200; %20 2000 R=[r,0;0,r]; %模型量测噪声协方差矩阵 Q=[10,0;0,10]; %模型过程噪声协方差矩阵 F1=[1,Tsample,0,0;0,1,0,0;0,0,1,Tsample;0,0,0,1]; %模型1状态转移矩阵 F2=[1,sin(model2_w2*Tsample)/model2_w2,0,(cos(model2_w2*Tsample)-1)/model2_w2; 0,cos(model2_w2*Tsample),0,sin(model2_w2*Tsample); 0,(1-cos(model2_w2*Tsample))/model2_w2,1,sin(model2_w2*Tsample)/model2_w2; 0,-sin(model2_w2*Tsample),0,cos(model2_w2*Tsample)]; %模型2状态转移矩阵 左转弯 F3=[1,sin(model3_w3*Tsample)/model3_w3,0,(cos(model3_w3*Tsample)-1)/model3_w3; 0,cos(model3_w3*Tsample),0,sin(model3_w3*Tsample); 0,(1-cos(model3_w3*Tsample))/model3_w3,1,sin(model3_w3*Tsample)/model3_w3; 0,-sin(model3_w3*Tsample),0,cos(model3_w3*Tsample)]; %模型3状态转移矩阵 右转弯 model2_w4=3*2*pi/360; %模型2转弯率3度 model3_w5=-3*2*pi/360; %模型3转弯率-3度 F4=[1,sin(model2_w4*Tsample)/model2_w4,0,(cos(model2_w4*Tsample)-1)/model2_w4; 0,cos(model2_w4*Tsample),0,-sin(model2_w4*Tsample); 0,(1-cos(model2_w4*Tsample))/model2_w4,1,sin(model2_w4*Tsample)/model2_w4; 0,sin(model2_w4*Tsample),0,cos(model2_w4*Tsample)]; %模型2状态转移矩阵 F5=[1,sin(model3_w5*Tsample)/model3_w5,0,(cos(model3_w5*Tsample)-1)/model3_w5; 0,cos(model3_w5*Tsample),0,-sin(model3_w5*Tsample); 0,(1-cos(model3_w5*Tsample))/model3_w5,1,sin(model3_w5*Tsample)/model3_w5; 0,sin(model3_w5*Tsample),0,cos(model3_w5*Tsample)]; %模型3状态转移矩阵 x0=[1000,200,1000,200]'; % 初始状态 % 产生量测数据 %[z_s,z_true_s]=targets(); randn('state',sum(100*clock)); % Shuffle the pack! %rng('shuffle'); x_s = zeros(4,echo_num); z_s = zeros(2,echo_num); %含噪声量测数据 z_true_s = zeros(2,echo_num); %真值数据 measureNoise = zeros(2,echo_num); measureNoise = sqrt(R)*randn(2,echo_num); %产生量测噪声 x_s(:,1)=x0; z_s(:,1)=H*x_s(:,1)+measureNoise(:,1); z_true_s(:,1)=H*x_s(:,1); for a=2:echo_num if (a>=20)&&(a<=40) x_s(:,a)=F4*x_s(:,a-1); %20--40s左转 elseif (a>=60)&&(a<=80) x_s(:,a)=F5*x_s(:,a-1); %60--80s右转 else x_s(:,a)=F1*x_s(:,a-1); %匀速直线运动 end; z_s(:,a)=H*x_s(:,a)+measureNoise(:,a); z_true_s(:,a)=H*x_s(:,a); end; %=============================== % IMM %=============================== %初始化 x1_IMM_s = zeros(4,1); %模型1IMM算法状态估计值 x2_IMM_s = zeros(4,1); %模型2IMM算法状态估计值 x3_IMM_s = zeros(4,1); %模型3IMM算法状态估计值 x_pro_IMM_s = zeros(4,echo_num); %IMM算法模型综合状态估计值 P_IMM_s=zeros(4,4,echo_num); %IMM算法模型综合状态协方差矩阵 P1_IMM_s=zeros(4,4); P2_IMM_s=zeros(4,4); P3_IMM_s=zeros(4,4); %IMM算法各模型协方差矩阵 r1_IMM_s=zeros(2,1); r2_IMM_s=zeros(2,1); r3_IMM_s=zeros(2,1); S1_IMM_s=zeros(2,2); S2_IMM_s=zeros(2,2); S3_IMM_s=zeros(2,2); %初始化 x_pro_IMM_s(:,1)=x0; pij=[0.9,0.05,0.05; 0.1,0.8,0.1; 0.05,0.15,0.8]; %模型转移概率矩阵 %pij=[0.6,0.2,0.2;0.2,0.6,0.2;0.25,0.15,0.6]; %模型转移概率矩阵 u_IMM_s=zeros(3,echo_num); u_IMM_s(:,1)=[0.3,0.3,0.4]'; %IMM算法模型概率 x1_IMM_s=x0;x2_IMM_s=x0;x3_IMM_s=x0; %IMM算法各模型初始状态 P0=diag([1000,500,1000,500]); %初始状态协方差矩阵 P1_IMM_s=P0;P2_IMM_s=P0;P3_IMM_s=P0; P_IMM_s(:,:,1)=P0; %main loop for t=1:echo_num-1 %第一步Interacting（只针对IMM算法） c_j=pij'*u_IMM_s(:,t); ui1_x=(1/c_j(1))*pij(:,1).*u_IMM_s(:,t); ui2_x=(1/c_j(2))*pij(:,2).*u_IMM_s(:,t); ui3_x=(1/c_j(3))*pij(:,3).*u_IMM_s(:,t); %计算模型混合概率 % 计算各模型滤波初始化条件 x01_s=x1_IMM_s*ui1_x(1)+x2_IMM_s*ui1_x(2)+x3_IMM_s*ui1_x(3); x02_s=x1_IMM_s*ui2_x(1)+x2_IMM_s*ui2_x(2)+x3_IMM_s*ui2_x(3); x03_s=x1_IMM_s*ui3_x(1)+x2_IMM_s*ui3_x(2)+x3_IMM_s*ui3_x(3); %各模型滤波初始状态 P01=(P1_IMM_s+[x1_IMM_s-x01_s]*[x1_IMM_s-x01_s]')*ui1_x(1)+... (P2_IMM_s+[x2_IMM_s-x01_s]*[x2_IMM_s-x01_s]')*ui1_x(2)+... (P3_IMM_s+[x3_IMM_s-x01_s]*[x3_IMM_s-x01_s]')*ui1_x(3); P02=(P1_IMM_s+[x1_IMM_s-x02_s]*[x1_IMM_s-x02_s]')*ui2_x(1)+... (P2_IMM_s+[x2_IMM_s-x02_s]*[x2_IMM_s-x02_s]')*ui2_x(2)+... (P3_IMM_s+[x3_IMM_s-x02_s]*[x3_IMM_s-x02_s]')*ui2_x(3); P03=(P1_IMM_s+[x1_IMM_s-x03_s]*[x1_IMM_s-x03_s]')*ui3_x(1)+... (P2_IMM_s+[x2_IMM_s-x03_s]*[x2_IMM_s-x03_s]')*ui3_x(2)+... (P3_IMM_s+[x3_IMM_s-x03_s]*[x3_IMM_s-x03_s]')*ui3_x(3); %各模型滤波初始状态协方差矩阵 %第二步--卡尔曼滤波 %模型1卡尔曼滤波 [x1_IMM_s,P1_IMM_s,r1_IMM_s,S1_IMM_s]=Kalman(x01_s,P01,z_s(:,t+1),F1,G,Q,H,R); %模型2卡尔曼滤波 [x2_IMM_s,P2_IMM_s,r2_IMM_s,S2_IMM_s]=Kalman(x02_s,P02,z_s(:,t+1),F2,G,Q,H,R); %模型3卡尔曼滤波 [x3_IMM_s,P3_IMM_s,r3_IMM_s,S3_IMM_s]=Kalman(x03_s,P03,z_s(:,t+1),F3,G,Q,H,R); %第三步--模型概率更新 [u_IMM_s(:,t+1)]=Model_P_up(r1_IMM_s,r2_IMM_s,r3_IMM_s,S1_IMM_s,S2_IMM_s,S3_IMM_s,c_j); %第四步--模型综合 [x_pro_IMM_s(:,t+1),P_IMM_s(:,:,t+1)]=Model_mix(x1_IMM_s,x2_IMM_s,x3_IMM_s,P1_IMM_s,P2_IMM_s,P3_IMM_s,u_IMM_s(:,t)); end %=============================== %绘图 %=============================== %目标轨迹 figure(1) plot(z_true_s(1,:),z_true_s(2,:),'g',x_pro_IMM_s(1,:),x_pro_IMM_s(3,:),'r',z_s(1,:),z_s(2,:),'x'); %grid on; hold on % plot(x_pro_IMM_s(1,:),x_pro_IMM_s(3,:),'r'); % plot(z_s(1,:),z_s(2,:),'*'); %hold off title('目标运动轨迹'); xlabel('x_s/m'); ylabel('y/m'); legend('真实值','滤波值','量测值'); text(z_s(1,1)+500,z_s(2,1),'t=1'); % 位置误差 figure(2) subplot(2,1,1); t=1:echo_num; plot(t,abs(x_pro_IMM_s(1,t)-x_s(1,t)));grid on title('x坐标位置跟踪误差'); xlabel('t/s'); ylabel('x_s-error/m'); subplot(2,1,2); t=1:echo_num; plot(t,abs(x_pro_IMM_s(3,t)-x_s(3,t)));grid on title('y坐标位置跟踪误差'); xlabel('t/s'); ylabel('y-error/m'); % 速度误差 figure(3) subplot(2,1,1); t=1:echo_num; plot(t,abs(x_pro_IMM_s(2,t)-x_s(2,t)));grid on title('x坐标速度跟踪误差'); xlabel('t/s'); ylabel('vx-error/m'); subplot(2,1,2); t=1:echo_num; plot(t,abs(x_pro_IMM_s(4,t)-x_s(4,t)));grid on title('y坐标速度跟踪误差'); xlabel('t/s'); ylabel('vy-error/m'); % 模型概率 figure(4) plot(t,u_IMM_s(1,t),'g:',t,u_IMM_s(2,t),'r-.',t,u_IMM_s(3,t),'b--','LineWidth',2);grid on title('IMM算法模型概率曲线'); xlabel('t/s'); ylabel('模型概率'); legend('模型1','模型2','模型3');