【运动学】 Singer模型算法机动目标跟踪【含Matlab源码 1157期】.zip

%**********利用Singer模型算法对机动目标进行跟踪************* function [xx5,yy5,ex5,exv5]=singer(T,r,N) clc clear close all % %***************仿真条件******************** T=2; %雷达扫描周期 r=10000; %量测误差方差 N=50;%Monte Carlo仿真次数 %alpha=1/60;%机动时间常数的倒数，即机动频率 F=[1 T (1/2)*T^2 0 0 0; 0 1 T 0 0 0; 0 0 1 0 0 0; 0 0 0 1 T (1/2)*T^2; 0 0 0 0 1 T; 0 0 0 0 0 1];%状态转移矩阵 H=[1 0 0 0 0 0; 0 0 0 1 0 0];%量测矩阵 sigmax=r;%X方向量测噪声方差 sigmay=r;%Y方向量测噪声方差 R=[sigmax 0; 0 sigmay];%量测噪声协方差 %sigmaax=0.01;%X方向目标加速度方差 %sigmaay=0.01;%Y方向目标加速度方差 qq11=T^5/20; qq12=T^4/8; qq13=T^3/6; qq22=T^3/3; qq23=T^2/2; qq33=T; qq44=T^5/20; qq45=T^4/8; qq46=T^3/6; qq55=T^3/3; qq56=T^2/2; qq66=T; Q=[qq11 qq12 qq13 0 0 0; qq12 qq22 qq23 0 0 0; qq13 qq23 qq33 0 0 0; 0 0 0 qq44 qq45 qq46; 0 0 0 qq45 qq55 qq56; 0 0 0 qq46 qq56 qq66];%过程噪声协方差 for j=1:N [x,y,zx,zy,NN]=target_movement; load target_movement_out z=[zx';zy']; X=[z(1,3) (z(1,3)-z(1,2))/T (z(1,3)-2*z(1,2)+z(1,1))/T^2 z(2,3) (z(2,3)-z(2,2))/T (z(2,3)-2*z(2,2)+z(2,1))/T^2]';%状态向量初始化 %滤波协方差初始化 P11=R(1,1); P12=R(1,1)/T; P13=R(1,1)/T^2; P22=2*R(1,1)/T^2; P23=3*R(1,1)/T^3; P33=6*R(1,1)/T^4; P44=R(2,2); P45=R(2,2)/T; P46=R(2,2)/T^2; P55=2*R(2,2)/T^2; P56=3*R(2,2)/T^3; P66=6*R(2,2)/T^4; P=[P11 P12 P13 0 0 0; P12 P22 P23 0 0 0; P13 P23 P33 0 0 0; 0 0 0 P44 P45 P46; 0 0 0 P45 P55 P56; 0 0 0 P46 P56 P66]; MX(:,3)=X; EX(j,3)=(X(1)-x(3)).^2;%x方向位置初始方差 EXv(j,3)=(X(2)-vvx(3)).^2;%x方向速度初始方差 EY(j,3)=(X(4)-y(3)).^2;%y方向位置初始方差 EYv(j,3)=(X(5)-vvy(3)).^2;%y方向速度初始方差 for i=4:NN x1=F*X; z1=H*x1; P1=F*P*F'+Q; S=H*P1*H'+R; v=z(:,i)-z1; W=P1*H'*inv(S); X=x1+W*v; P=P1-W*S*W'; Mv=v'*inv(S)*v; MX(:,i)=X; MEX(:,i,j)=MX(:,i); EX(j,i)=(X(1)-x(i)).^2;%x方向位置初始方差 EXv(j,i)=(X(2)-vvx(i)).^2;%x方向速度初始方差 EY(j,i)=(X(4)-y(i)).^2;%y方向位置初始方差 EYv(j,i)=(X(5)-vvy(i)).^2;%x方向速度初始方差 end end EEX=sqrt(sum(EX)/N); EEY=sqrt(sum(EY)/N); exv5=sqrt(sum(EXv)/N); eyv5=sqrt(sum(EYv)/N); xx5=MX(1,:); yy5=MX(4,:); ex5=EEX; ey5=EEY; % ex5=EEY; i=1:NN; k=3:1:NN; l=4:1:NN; figure(1) plot(x,y,'--r'); hold on plot(zx,zy,'-b'); hold on plot(xx5(k),yy5(k),'-g'); title('动目标跟踪Singer') xlabel('x方向') ylabel('y方向') legend('目标运动真实轨迹','目标运动量测轨迹','跟踪轨迹Singer') figure(2) hold on plot(k,EEX(k),'-r'); title('x方向位置协方差Singer') xlabel('跟踪步数') ylabel('x方向') figure(3) hold on plot(k,EEY(k),'-r'); title('y方向位置协方差Singer') xlabel('跟踪步数') ylabel('y方向') figure(4) hold on plot(k,exv5(k),'-r'); title('x方向速度协方差Singer') xlabel('跟踪步数') ylabel('x方向') figure(5) hold on plot(k,eyv5(k),'-r'); title('y方向速度协方差Singer') xlabel('跟踪步数') ylabel('y方向')