【目标跟踪】基于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方向位置估计的标准差曲线’);