target-tracking-algorithm-simulation.zip_ca tracking target_ca模型

clear; clc; T=1; num=50; N0=400/T; N1=600/T; a=1/60; SIGMA=10000 SIGMAa=0.25; % N3=610/T;N4=660/T;N5=900/T; x=zeros(N1,1); y=zeros(N1,1); vx=zeros(N1,1); vy=zeros(N1,1); x(1)=-10000;y(1)=2000; vx(1)=15;vy(1)=0; ax=0;ay=0; var=100; for i=1:N1-1 if(i>N0-1&i<=N1-1) ax=-0.075;ay=0.075; vx(i+1)=vx(i)+ax*T; vy(i+1)=vy(i)+ax*T; else ax=0;ay=0; vx(i+1)=vx(i); vy(i+1)=vy(i); end x(i+1)=x(i)+vx(1)*T; y(i+1)=y(i)+vy(i)*T; end rex(num,N1)=0; rey(num,N1)=0; % for m=1:num nx=100*randn(N1,1); ny=100*randn(N1,1); zx=x+nx; zy=y+ny; rex(m,1)=-10000; rey(m,1)=2000; rex(m,2)=-9960; rey(m,2)=2000; ki=0; low=1;high=0; u=0;ua=0; e=0.8; xks(1)=zx(1); yks(1)=zy(1); xks(2)=zx(2); yks(2)=zy(2); o=[1,T,(-1+a*T+exp(-a*T))/(a*a),0,0,0; 0,1,(1-exp(-a*T))/a,0,0,0; 0,0,exp(-a*T),0,0,0; 0,0,0,1,T,(-1+a*T+exp(-a*T))/(a*a); 0,0,0,0,1,(1-exp(-a*T))/a; 0,0,0,0,0,exp(-a*T)]; % o=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1]; Q=2*a*SIGMAa*[(1-exp(-2*a*T)+2*a*T+2*a^3*T^3/3-2*a^2*T^2-4*a*T*exp(-a*T))/(2*a^5),(exp(-2*a*T)+1-2*exp(-a*T)+2*a*T*exp(-a*T)-2*a*T+a^2*T^2)/(2*a^4),(1-exp(-2*a*T)-2*a*T*exp(-a*T))/(2*a^3),0,0,0; (exp(-2*a*T)+1-2*exp(-a*T)+2*a*T*exp(-a*T)-2*a*T+a^2*T^2)/(2*a^4),(4*exp(-a*T)-3-exp(-2*a*T)+2*a*T)/(2*a^3),(exp(-2*a*T)+1-2*exp(-a*T))/(2*a^2),0,0,0; (4*exp(-a*T)-3-exp(-2*a*T)+2*a*T)/(2*a^3),(exp(-2*a*T)+1-2*exp(-a*T))/(2*a^2),(1-exp(-2*a*T))/(2*a),0,0,0; 0,0,0,(1-exp(-2*a*T)+2*a*T+2*a^3*T^3/3-2*a^2*T^2-4*a*T*exp(-a*T))/(2*a^5),(exp(-2*a*T)+1-2*exp(-a*T)+2*a*T*exp(-a*T)-2*a*T+a^2*T^2)/(2*a^4),(1-exp(-2*a*T)-2*a*T*exp(-a*T))/(2*a^3); 0,0,0,(exp(-2*a*T)+1-2*exp(-a*T)+2*a*T*exp(-a*T)-2*a*T+a^2*T^2)/(2*a^4),(4*exp(-a*T)-3-exp(-2*a*T)+2*a*T)/(2*a^3),(exp(-2*a*T)+1-2*exp(-a*T))/(2*a^2); 0,0,0,(4*exp(-a*T)-3-exp(-2*a*T)+2*a*T)/(2*a^3),(exp(-2*a*T)+1-2*exp(-a*T))/(2*a^2),(1-exp(-2*a*T))/(2*a)]; h=[1,0,0,0,0,0;0,0,0,1,0,0]; g=[T^2/2,0;T,0;0,T^2/2;0,T]; q=[10000,0;0,10000]; % perr=[var^2,var^2/T,0,0;var*var/T,2*var^2/(T^2),0,0;0,0,var^2,var^2/T;0,0,var^2/T,2*var^2/(T^2)]; perr=[SIGMA,SIGMA/T,0,0,0,0;SIGMA/T,2*SIGMA/(T^2)+SIGMAa*(2-a^2*T^2+2*a^3*T^3/3-2*a*exp(-a*T))/(a^4*T^2),SIGMAa*(exp(-a*T)+a*T-1)/(a^2*T),0,0,0; 0,SIGMAa*(exp(-a*T)+a*T-1)/(a^2*T),SIGMAa,0,0,0;0,0,0,SIGMA,SIGMA/T,0; 0,0,0,SIGMA/T,2*SIGMA/(T^2)+SIGMAa*(2-a^2*T^2+2*a^3*T^3/3-2*a*exp(-a*T))/(a^4*T^2),SIGMAa*(exp(-a*T)+a*T-1)/(a^2*T); 0,0,0,0,SIGMAa*(exp(-a*T)+a*T-1)/(a^2*T),SIGMAa]; vx=(zx(2)-zx(1))/2;vy=(zy(2)-zy(1))/2; xk=[zx(1);vx;ax;zy(1);vy;ay]; % for r=3:N1 z=[zx(r);zy(r)]; xk1=o*xk; perr1=o*perr*o'+Q; k1=perr1*h'*inv(h*perr1*h'+SIGMA*eye(2)); % k1=perr1*h'*inv(h*perr1*h'+q); xk=xk1+k1*(z-h*xk1); perr=(eye(6)-k1*h)*perr1; xks(r)=xk(1,1); yks(r)=xk(4,1); vxks(r)=xk(2,1); vyks(r)=xk(5,1); xk1s(r)=xk1(1,1); yk1s(r)=xk1(4,1); vxk1s(r)=xk1(2,1); vyks1(r)=xk1(5,1); perr11(r)=perr(1,1); perr12(r)=perr(1,2); perr22(r)=perr(2,2); rex(m,r)=xks(r); rey(m,r)=yks(r); end end rey(1,:) ex=0;ey=0; eqx=0;eqy=0; ex1(N1,1)=0;ey1(N1,1)=0; qx(N1,1)=0;qy(N1,1)=0; for i=1:N1 for j=1:num ex=ex+x(i)-rex(j,i); ey=ey+y(i)-rey(j,i); %eqx=eqx+(x(i)-rex(j,i))^2; eqx=eqx+(x(i)-rex(j,i))^2; eqy=eqy+(y(i)-rey(j,i))^2; end % eqx/num-(ex1(i)^2) %sqrt((double(eqx)/num-(ex1(i)^2))); ex1(i)=ex/num; ey1(i)=ey/num; qx(i)=sqrt((double(eqx)/num-(ex1(i)^2))); qy(i)=sqrt((double(eqy)/num-(ey1(i)^2))); % qy(i)=(eqy/num-(ey1(i)^2))^0.5; ex=0;eqx=0;ey=0;eqy=0; end figure(1); plot(x,y,'k-',zx,zy,'g:',xks,yks,'r-.'); legend('true trace','observation samples','estimated samples'); figure(2); plot(zx,zy); legend('observation samples'); figure(3) plot(xks,yks);legend('estimated trace'); figure(4);plot(x,ex1);legend('the error at x anix'); figure(5);plot(x,qx); legend('the square error at x anix'); figure(6);plot(y,ey1);legend('the error at y anix'); figure(7);plot(y,qy);