目標定位跟踪算法及仿真程序.docxVIP

目标定位跟踪算法及仿真程序质心算法是最简单的定位算法，如图2-1所示，四个小圆为观测站，实线三角形是目标真实的位置，假设四个圆形观测站都探测到目标的存在，则根据质心定位算法，目标的位置（x,y）可以表示为：，，这里观测站得位置为，同理，当观测站数目为N时，这时候的质心定位算法可以表示为： 图1 质心定位%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%质心定位算法Matlab程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%function main% 定位初始化Length=100; % 场地空间，单位：米Width=100; % 场地空间，单位：米d=50; % 目标离观测站50米以内都能探测到，反之则不能Node_number=6; % 观测站的个数for i=1:Node_number % 观测站的位置初始化，这里位置是随机给定的 Node(i).x=Width*rand; Node(i).y=Length*rand;end% 目标的真实位置，这里也随机给定Target.x=Width*rand;Target.y=Length*rand;% 观测站探测目标X=[]; for i=1:Node_number if DIST(Node(i),Target)=d X=[X;Node(i).x,Node(i).y]; endendN=size(X,1); % 探测到目标的观测站个数Est_Target.x=sum(X(:,1))/N; % 目标估计位置xEst_Target.y=sum(X(:,2))/N; % 目标估计位置yError_Dist=DIST(Est_Target,Target) % 目标真实位置与估计位置的偏差距离%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 画图figurehold 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)]);endh2=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]);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 子函数，计算两点间的距离function dist=DIST(A,B)dist=sqrt( (A.x-B.x)^2+(A.y-B.y)^2 );% 子函数， 以目标为中心画圆function circle(x0,y0,r)sita=0:pi/20:2*pi;plot(x0+r*cos(sita),y0+r*sin(sita)); % 中心在(x0,y0），半径为r%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%执行程序，得到仿真结果：更多目标定位跟踪算法和程序请参考以下书籍：目 录第一章 目标跟踪概述 11.1 多传感器探测的目标跟踪架构 11.2 目标定位算法简介 21.3 目标跟踪过程描述 21.4 跟踪模型的建立 4第二章 常用目标定位算法 72.1 质心定位算法程序 72.2 加权质心定位算法程序 92.3 最小二乘/极大似然定位算法 122.3.1 测距技术 122.3.2 定位技术 142.3.3最小均方误差的二维定位方法程序 162.3.4 最小均方误差的三维定位方法程序 172.3.5 最小二乘/极大似然用于目标跟踪(连续定位)程序 192.3.6 最小二乘/极大似然用于纯方位目标跟踪(连续定位)程序 22第三章 卡尔曼滤波 243.1 Kalman滤波 243.1.1 Kalman滤波原理 243.1.2 Kalman滤波在目标跟