编写的源程序交互多模算法用于目标多机动蒙特卡罗法仿真跟踪滤波器.docx

%*交互多模算法*%IMM算法%Radar%N %*%function IMM(N)N=100;T=2;r=10000;R=r 0;0 r;A1=1 T 0 0; 0 1 0 0;0 0 1 T;0 0 0 1;H1=1 0 0 0; 0 0 1 0;G1=T/2 0;1 0;0 T/2;0 1;A2=1 T 0 0 T2/2 0; 0 1 0 0 T 0 0 0 1 T 0 T 2/2; 0 0 0 1 0 T 0 0 0 0 1 0; 0 0 0 0 0 1;H2=1 0 0 0 0 0 ;0 0 1 0 0 0 ;G2=T2/4 0; T2/2 0 0 T2/4;0 T2/2 1 0;0 1;q1=0;q2=0.05;q3=0.02;Q1=q1 0; 0 q1;Q2=q2 0; 0 q2;Q3=q3 0; 0 q3;P=0.95 0.025 0.025 %模型状态转移矩阵 0.025 0.95 0.025 0.025 0.025 0.95;U=1 0 0;%模拟目标真实运动轨迹x0=2000;y0=10000;v0=-15;alx=0.075;aly=0.075;a2x=-0.3;a2y=-0.3;t1=400;K1=t1/T;t2=600;K2=(t2-t1)/T;t3=610;K3=(t3-t2)/T;t4=660;K4=(t4-t3)/T;t5=1500;K5=(t5-t4)/T;% 0-400k1=1:K1;x1=x0*ones(1,length(k1);y1=y0 v0*k1*T;0 -600k2=1:K2;x2=x1(K1) alx*(k2*T).2/2;y2=y1(K1) v0*k2*T aly*(k2*T).2/2;0-610k3=1:K3;x3=x2(K2) alx*(t2-t1)*(k3*T);y3=y2(K2)*ones(1,length(k3);a0-660k4=1:K4;x4=x3(K3) alx*(t2-t1)*(k4*T) alx*(k4*T).2/2;y4=y3(K3) a2y*(k4*T).2/2;f0-1500k5=1:K5;x5=x4(K4)*ones(1,length(k5);y5=y4(K4) a2y*(t4-t3)*(k5*T);x=x1 x2 x3 x4 x5;y=y1 y2 y3 y4 y5;K=K1 K2 K3 K4 K5;xx=zeros(1,K);yy=zeros(1,K);ex1=zeros(1,K);ey1=zeros(1,K);ex2=zeros(1,K);ey2=zeros(1,K);%for i=1:N zx=randn(1,K)*100 x; zy=randn(1,K)*100 y; Z=zx;zy; % LX=zx(2) (zx(2)-zx(1)/T zy(2) (zy(2)-zy(1)/T; LP=r r/T 0 0 r/T 2*r/(T*T) 0 0 0 0 r r/T 0 0 r/T 2*r/(T*T); xx(1)=xx(1) zx(1);yy(1)=yy(1) zy(1); ex1(1)=ex1(1) x(1)-zx(1);ex2(1)=ex2(1) (x(1)-zx(1)2; ey1(1)=ey1(1) y(1)-zy(1);ey2(1)=ey2(1) (y(1)-zy(1)2; % LX1=LX,0,0; LX2=LX,0,0; LX3=LX,0,0; LP1=LP,zeros(4,2);zeros(2,6); LP2=LP,zeros(4,2);zeros(2,6); LP3=LP,zeros(4,2);zeros(2,6); for k=2:K xx(k)=xx(k) LX(1); yy(k)=yy(k) LX(3); ex1(k)=ex1(k) x(k)-LX(1); ex2(k)=ex2(k) (x(k)-LX(1)2; ey1(k)=ey1(k) y(k)-LX(3); ey2(k)=ey2(k) (y(k)-LX(3)2; % C_=P*U; U=P.*(U*ones(1,3)./(ones(3,1)*C_); X01=LX1*U(1,1) LX2*U(2,1) LX3*U(3,1); X02=LX1*U(1,2) LX2*U(2,2) LX3*U(3,2); X03=LX1*U(1,3) LX2*U(2,3) LX3*U(3,3); P01=U(1,1)*(LP1 (LX1-X01)*(LX1-X01) U(2,1)*(LP2 (LX2-X01)*(LX2-X01) U(3,1)*(LP3 (LX3-X01)*(LX3-X01); P02=U(1,2)*(LP1 (LX1-X02)*(LX1-X02) U(2,2)*(LP2 (LX2-X02)*(LX2-X02) U(3,2)*(LP3 (LX3-X02)*(LX3-X02); P03=U(1,3)*(LP1 (LX1-X03)*(LX1-X03) U(2,3)*(LP2 (LX2-X03)*(LX2-X03) U(3,3)*(LP3 (LX3-X03)*(LX3-X03); % X01=X01(1:4); P01=P01(1:4,1:4); LX1,LP1,E1=kfilter(A1,H1,G1,Q1,R,X01,P01,Z,k); LX1=LX1,0,0; LP1=LP1,zeros(4,2);zeros(2,6); LX2,LP2,E2=kfilter(A2,H2,G2,Q2,R,X02,P02,Z,k); LX3,LP3,E3=kfilter(A2,H2,G2,Q3,R,X03,P03,Z,k); E=E1 E2 E3; C=E*C_; U=(E.*C_)/C; % LX=LX1*U(1) LX2*U(2) LX3*U(3); LP=U(1)*(LP1 (LX1-LX)*(LX1-LX) U(2)*(LP2 (LX2-LX)*(LX2-LX) U(3)*(LP3 (LX3-LX)*(LX3-LX); endend%for k=1:K ex_(k)=ex1(k)/N; ex(k)=sqrt(ex2(k)/N-ex_(k)2); ey_(k)=ex1(k)/N; ey(k)=sqrt(ey2(k)/N-ey_(k)2); xx(k)=xx(k)/N; yy(k)=yy(k)/N;end figure(1)