【目标跟踪】基于VD算法结合卡尔曼滤波结合实现对目标的跟踪预测附matlab代码

%---------------------------------------------%%% VD算法clear all; close all; clc;%% 参数设置%% 产生真实轨迹N1=400/T;N2=600/T;N3=610/T;N4=660/T;N=1000/T;x=zeros(N,1);y=zeros(N,1);vx=zeros(N,1);vy=zeros(N,1);x(1)=2000;y(1)=10000;       %位置、速度、加速度真实值数组vx(1)=0;vy(1)=-15;ax=0;ay=0;var=100; for i=1:N-1     if(i>N1-1&&i<=N2-1)         ax=0.075;ay=0.075;         vx(i+1)=vx(i)+ax*T;         vy(i+1)=vy(i)+ay*T;     elseif(i>N2-1&&i<=N3-1)         ax=0;ay=0;         vx(i+1)=vx(i)+ax*T;         vy(i+1)=vy(i)+ay*T;     elseif(i>N3-1&&i<=N4-1)         ax=-0.3;ay=-0.3;         vx(i+1)=vx(i)+ax*T;         vy(i+1)=vy(i)+ay*T;     else         ax=0;ay=0;         vx(i+1)=vx(i)+ax*T;         vy(i+1)=vy(i)+ay*T;     end     x(i+1)=x(i)+vx(i)*T+0.5*ax*T^2;     y(i+1)=y(i)+vy(i)*T+0.5*ay*T^2; end rex=zeros(M,N);%保留x方向位置每次滤波结果 rey=zeros(M,N);%保留y方向位置每次滤波结果 revx=zeros(M,N);%保留x方向速度每次滤波结果 revy=zeros(M,N);%保留y方向速度每次滤波结果  %% 卡尔曼滤波VD算法 %滤波50次 for m=1:M     %% 观测数据生成     noise=mvnrnd([0,0],[10000,500;500,10000],N); % sigma     zx=x+noise(:,1);     zy=y+noise(:,2);     %% 滤波初始化     ki=0;                  %机动模型持续时间     low=1;high=0;          %low表示非机动模型是否初始化、high表示机动模型中是否初始化     u=0;ua=0;              %分别表示非机动模型和机动模型中的判决量     o=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];   %状态转移矩阵     h=[1 0 0 0;0 0 1 0];           %测量矩阵     g=[T/2 0;1 0;0 T/2;0 1];      %扰动噪声矩阵     q=[0,0;0,0];                    %扰动噪声协方差阵     R=[10000,500;500,10000];           %测量噪声协方差矩阵     % 按照论文要求产生初始值     zx0=x(1)-vx(1)*T+var*randn(1,1);zy0=y(1)-vy(1)*T+var*randn(1,1);    %kalman滤波开始  end ex=0;ey=0; eqx=0;eqy=0; ex1=N:1;ey1=N:1; qx=N:1;qy=N:1; %计算滤波均值和均方差 for i=1:N     for j=1:M         ex=ex+x(i)-rex(j,i);         ey=ey+y(i)-rey(j,i);         eqx=eqx+(x(i)-rex(j,i))^2;         eqy=eqy+(y(i)-rey(j,i))^2;     end     ex1(i)=ex/M;%x方向位置的误差值     ey1(i)=ey/M;%y方向位置的误差值     qx(i)=(eqx/M-(ex1(i)^2))^0.5;%x方向位置的根方差     qy(i)=(eqy/M-(ey1(i)^2))^0.5;%y方向位置的根方差     ex=0;eqx=0;eqy=0;     ey=0; end  for i=1:N     for j=1:M         ex=ex+vx(i)-revx(j,i);         ey=ey+vy(i)-revy(j,i);         eqx=eqx+(vx(i)-revx(j,i))^2;         eqy=eqy+(vy(i)-revy(j,i))^2;     end     ex1(i)=ex/M;%x方向位置的误差值     ey1(i)=ey/M;%y方向位置的误差值     qvx(i)=(eqx/M-(ex1(i)^2))^0.5;%x方向位置的根方差     qvy(i)=(eqy/M-(ey1(i)^2))^0.5;%y方向位置的根方差     ex=0;eqx=0;eqy=0;     ey=0; end %绘图 figure plot(x,y,'k-',zx,zy,'g:',xks,yks,'r-'); legend('真实轨迹', '观测样本', '估计轨迹'); figure subplot(221); plot(qvx); title('x方向速度估计的标准差曲线'); subplot(222) plot(qvy); title('y方向速度估计的标准差曲线'); subplot(223) plot(qx); title('x方向位置估计的标准差曲线'); subplot(224) plot(qy); title('y方向位置估计的标准差曲线');