质心定位算法.zip_FZO_定位_定位算法_质心定位算法_质心算法

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;

for i=1:Node_number

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) % 目标真实位置与估计位置的偏差距离

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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']);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 子函数，计算两点间的距离

function dist=DIST(A,B)