目标定位跟踪算法及仿真程序

目标定位跟踪算法及仿真程序 质心算法是最简单的定位算法，如图2-1 所示，四个小圆为观测站，实线三角形是目标真实的位置，假设四个圆形观测站都探测到目标的存在，则根据质心定位算法，目标的位置 （ x ,y ） 可 以 表 示为：44321xxxxx，44321yyyyy，这里观测站得位置为),(ii yx，同理，当观测站数目为N时，这时候的质心定位算法可以表示为： NiiNiiyNxNyx1111 图1 质心定位 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 质心定位算法Matlab 程序 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% fu nction main % 定位初始化 Length=100; % 场地空间，单位：米 Width=100; % 场地空间，单位：米 d=50; % 目标离观测站50 米以内都能探测到，反之则不能 Node_nu mber=6; % 观测站的个数 for i=1:Node_nu mber % 观测站的位置初始化，这里位置是随机给定的 Node(i).x =Width*rand; Node(i).y =Length*rand; end % 目标的真实位置，这里也随机给定 Target.x =Width*rand; Target.y =Length*rand; % 观测站探测目标 X=[]; for i=1:Node_nu mber if DIST(Node(i),Target)<=d X=[X;Node(i).x,Node(i).y]; end end N=size(X,1); % 探测到目标的观测站个数 Est_Target.x=sum(X(:,1))/N; % 目标估计位置x Est_Target.y=sum(X(:,2))/N; % 目标估计位置y Error_Dist=DIST(Est_Target,Target) % 目标真实位置与估计位置的偏差距离 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 画图 figure hold on;box on;axis([0 100 0 100]); % 输出图形的框架 for i=1:Node_number h1=plot(Node(i).x,Node(i).y,'ko','MarkerFace','g','MarkerSize',10); text(Node(i).x+2,Node(i).y,['Node ',num2str(i)]); end h2=plot(Target.x,Target.y,'k^','MarkerFace','b','MarkerSize',10); h3=plot(Est_Target.x,Est_Target.y,'ks','MarkerFace','r','MarkerSize',10); line([Target.x,Est_Target.x],[Target.y,Est_Target.y],'Color','k'); circle(Target.x,Target.y,d); legend([h1,h2,h3],'Observation Station','Target Postion','Estimate Postion'); xlabel(['error=',num2str(Error_Dist),'m']); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 子函数，计算两点间的...