目标跟踪_目标跟踪_多模型跟踪_跟踪滤波算法_多目标跟踪_卡尔曼跟踪

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%% %交互多模Kalman滤波在目标跟踪中的应用 %%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% global asj; T=2; %雷达扫描周期，也及采样周期 M=5; %仿真（滤波）次数 N=900/T; %总的采样点数 N1=100/T; %匀加速运动 N2=300/T; %匀减速运动 N3=400/T; %第一次转弯处采样起点 N4=600/T; %第一次匀速处采样起点 N5=610/T; %第二次转弯处采样起点 N6=660/T; %第二次匀速出采样起点 Delta=100; %测量噪声标准差 %对真实的轨迹和观测轨迹数据的初始化 Rx=zeros(N,1); Ry=zeros(N,1); Zx=zeros(N,M); Zy=zeros(N,M); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %下面是产生真实轨迹，读者可以修改数据，改变目标的真实运行状态和轨迹 %匀加速运动 t=2:T:100; x0=2000+0*t'; y0=10000-0*(t'-0)-0.15*((t'-0).^2)/2; %匀速运动 t=102:T:300; x1=x0(N1)+0*(t'-100); y1=y0(N1)-15*(t'-100); %1-匀减速运动 t=302:T:400; x2=x1(N2-N1)+0*t'; y2=y1(N2-N1)-15*(t'-300)+0.075*((t'-300).^2)/2; %2-慢转弯 t=402:T:600; x3=x2(N3-N2)+0.075*((t'-400).^2)/2; y3=y2(N3-N2)-(15/2)*(t'-400)+(0.075/2)*((t'-400).^2)/2; %3-匀速 t=602:T:610; vx=0.075*(600-400); x4=x3(N4-N3)+vx*(t'-600); y4=y3(N4-N3)+0*t'; %4-快转弯 t=612:T:660; x5=x4(N5-N4)+(vx*(t'-610)-0.3*((t'-610).^2)/2); y5=y4(N5-N4)-0.3*((t'-610).^2)/2; %5-匀速直线 t=662:T:900; vy=-0.3*(660-610); x6=x5(N6-N5)+0*t'; y6=y5(N6-N5)+vy*(t'-660); %最终将所有轨迹整合成为一个列向量，即真实轨迹书记，Rx为Real-x,Ry为Real-y的简写） Rx=[x0;x1;x2;x3;x4;x5;x6]; Ry=[y0;y1;y2;y3;y4;y5;y6]; %对每次蒙特卡洛仿真的滤波估计位置的初始化 Mt_Est_Px=zeros(M,N); Mt_Est_Py=zeros(M,N); %产生观测数据，要仿真M次，必须有M次的观测数据 nx=randn(N,M)*Delta; %产生观测噪声 ny=randn(N,M)*Delta; Zx=Rx*ones(1,M)+nx; %真实值的基础上叠加噪声，即构成九三级模拟的观测值 Zy=Ry*ones(1,M)+ny; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for m=1:M %滤波初始化 Mt_Est_Px(m,1)=Zx(1,m); %初始数据 Mt_Est_Py(m,1)=Zy(1,m); tMt_Est_Px(m,1)=Zx(1,m); %初始数据 tMt_Est_Py(m,1)=Zy(1,m); xn(1)=Zx(1,m); %滤波初值 xn(2)=Zx(2,m); yn(1)=Zy(1,m); yn(2)=Zy(2,m); %二点自启动法 vx=(Zx(2,m)-Zx(1,m))/2; vy=(Zy(2,m)-Zy(1,m))/2; %初始状态估计 X_est=[Zx(2,m);vx;0;Zy(2,m);vy;0];%状态初始化 P_est=[ Delta^2,Delta^2/T,0,0,0,0; Delta^2/T,2*Delta^2/(T^2),0,0,0,0; 0,0,0,0,0,0; 0,0,0,Delta^2,Delta^2/T,0; 0,0,0,Delta^2/T,2*Delta^2/(T^2),0; 0,0,0,0,0,0; ];%协方差初始化 Mt_Est_Px(m,2)=X_est(1);%记录画图点 Mt_Est_Py(m,2)=X_est(4); tX_est=[Zx(2,m);vx;0;Zy(2,m);vy;0];%状态初始化 tP_est=[ Delta^2,Delta^2/T,0,0,0,0; Delta^2/T,2*Delta^2/(T^2),0,0,0,0; 0,0,0,0,0,0; 0,0,0,Delta^2,Delta^2/T,0; 0,0,0,Delta^2/T,2*Delta^2/(T^2),0; 0,0,0,0,0,0; ];%协方差初始化 tMt_Est_Px(m,2)=tX_est(1);%记录画图点 tMt_Est_Py(m,2)=tX_est(4); %滤波开始 for i=1:2 tXn_est{i,1}=tX_est; tPn_est{i,1}=tP_est; end tu=[0.8,0.2];%模型概率初始化 %滤波开始 for i=1:3 Xn_est{i,1}=X_est; Pn_est{i,1}=P_est; end u=[0.8,0.1,0.1];%模型概率初始化 for r=3:N z=[Zx(r,m);Zy(r,m)]; %调用IMM算法 [X_est,P_est,Xn_est,Pn_est,u]=IMM(Xn_est,Pn_est,T,z,Delta,u); xn(r)=X_est(1); yn(r)=X_est(4); Mt_Est_Px(m,r)=X_est(1); Mt_Est_Py(m,r)=X_est(4); [tX_est,tP_est,tXn_est,tPn_est,tu]=tIMM(tXn_est,tPn_est,T,z,Delta,tu); txn(r)=tX_est(1); tyn(r)=tX_est(4); tMt_Est_Px(m,r)=tX_est(1); tMt_Est_Py(m,r)=tX_est(4); end %结束第一次滤波 end %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %滤波结果分析 err_x=zeros(N,1); err_y=zeros(N,1); delta_x=zeros(N,1); delta_y=zeros(N,1); delta_f=zeros(N,1); delta_z=zeros(N,1); %计算滤波的误差均值及标准差 exx=0; eyy=0; for r=1:N %估计误差均值吗，请对照书中的公式理解 ex=sum(Rx(r)-Mt_Est_Px(:,r)); ey=sum(Ry(r)-Mt_Est_Py(:,r)); err_x(r)=ex/M; err_y(r)=ey/M; eqx=sum((Rx(r)-Mt_Est_Px(:,r)).^2); eqy=sum((Ry(r)-Mt_Est_Py(:,r)).^2); eqzx=sum((Rx(r)-(Zx(r,:))').^2); eqzy=sum((Ry(r)-(Zy(r,:))').^2); teqx=sum((Rx(r)-tMt_Est_Px(:,r)).^2); teqy=sum((Ry(r)-tMt_Est_Py(:,r)).^2); % %估计误差标准差，请对照书中的公式理解 % delta_x(r)=sqrt(abs(eqx/M-(err_x(r)^2))); % delta_y(r)=sqrt(abs(eqy/M-(err_y(r)^2))); delta_x(r)=sqrt(eqx/M); delta_y(r)=sqrt(eqy/M); delta_f(r)=sqrt(eqx/M+eqy/M); delta_t(r)=sqrt(teqx/M+teqy/M); delta_z(r)=sqrt(eqzx/M+eqzy/M); %RMS exx=exx+eqx/M; eyy=eyy+eqy/M; end RMSE=sqrt(exx+eyy)/N %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %绘图 %轨迹图，（Rx,Ry)构成真实轨迹（ZX，ZY）为观测轨迹，如果蒙特卡洛仿真次数很多。 %该数据 %数海量的，导致观测轨迹看起来是一团（xn,yn)为一次滤波结果，读者可以用蒙特卡罗 %均值代替 figure(1); %plot(Rx,Ry,'k-',sum(Zx')'./M,sum(Zy')'./M,'g:',xn,yn,'r-.'); plot(Rx,Ry,'k-',Zx,Zy,'g:',xn,yn,'r-.'); legend('真实轨迹','观测样本','估计轨迹'); %均值 % figure(2); % subplot(2,1,1); % plot(err_x); % axis([1,N,-300,300]); % title('x方向估计误差均值'); % subplot(2,1,2); % plot(err_y); % axis([1,N,-300,300]); % title('y方向估计误差均值'); %标准差 figure(2); % subplot(2,1,1); hold on;box on; plot(delta_z,'g'); plot(delta_f,'k'); plot(delta_t,'b'); legend('观测误差','CV-CS-CT误差','CV-CS误差') title('均方根误差'); % subplot(2,1,2); % plot(delta_y); % title('y方向估计误差标准差'); xlabel('采样时间/T'); ylabel(' 误差/m'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %子函数IMM algorithm %X_est,P_est返回第M次仿真第m次仿真第r个采样点的滤波结果 %Xn_est,Pn_est记录每个模型对应的第M次仿真第R个采样点的滤波结果 %u为模型概率 function[X_est,P_est,Xn_est,Pn_est,u]=IMM(Xn_est,Pn_est,T,Z,Delta,u) %控制模型转换的马尔科夫链的转移概率矩阵 P=[0.95,0.025,0.025;0.025,0.95,0.025;0.025,0.025,0.95]; %所采用的三个模型参数，模型一为非机动，模型二，三均为机动模型(Q不同） %模型一 PHI{1,1}=[ 1,T,0,0,0,0; 0,1,0,0,0,0; 0,0,0,0,0,0; 0,0,0,1,T,0; 0,0,0,0,1,0; 0,0,0,0,0,0; ];%模型一 % PHI{2,1}=[ % 1,T,T^2/2,0,0,0; % 0,1,T,0,0,0; % 0,0,1,0,0,0; % 0,0,0,1,T,T^2/2; % 0,0,0,0,1,T; % 0,0,0,0,0,1 % ]; %模型二 L=1/2;%机动时间常数 PHI{2,1}=[ 1,T,[-1+L*T+exp(-L*T)]/L^2,0,0,0; 0,1,[1-exp(-L*T)],0,0,0; 0,0,exp(-L*T),0,0,0; 0,0,0,1,T,[-1+L*T+exp(-L*T)]/L^2; 0,0,0,0,1,[1-exp(-L*T)]; 0,0,0,0,0,exp(-L*T) ]; U=[ (1/L)*(-T+(1-exp(-L*T))/L+(L*T^2)/2); T-(1-exp(-L*T))/L; 1-exp(-L*T); ]; U=blkdiag(U,U); %模型三 w=0.000001; PHI{3,1}=[1,sin(w*T)/w,(1-cos(w*T))/w^2; 0,cos(w*T),sin(w*T)/w; 0,-w*sin(w*T),cos(w*T); ]; PHI{3,1}=blkdiag(PHI{3,1},PHI{3,1}); %模型三 G{1,1}=[ T^2/2,0; 1,0; 0,0; 0,T^2/2; 0,1; 0,0 ]; %模型一 % %G{2,1}=[ % % T^2/4,0; % T/2,0; % 1,0; % 0,T^2/4; % 0,T/2; % 0,1 % ];%模型二 q1=[1-exp(-2*L*T)+2*L*T+(2*L^3*T^3)/3-2*L^2*T^2-4*L*T*exp(-L*T)]/(2*L^5); q2=[exp(-2*L*T)+1-2*exp(-L*T)+2*L*T*exp(-L*T)-2*L*T+L^2*T^2]/(2*L^4); q4=q2; q3=[1-exp(-2*L*T)-2*L*T*exp(-L*T)]/(2*L^3); q7=q3; q5=[4*e