目标定位算法（一）之非测距的定位算法
文章目录
- 非测距的定位算法
-
- 1.质心定位算法
- 2.加权质心定位算法
- 3.网格定位算法
非测距的定位算法
这类定位算法依靠观测站的物理部署和简单的探测到目标的“有”和“无”的二进制探测信息对目标进行估计。主要有质心定位算法、加权质心定位算法和网络定位算法。
1.质心定位算法
多质点系统，每个质点的位置为p(xi,yi,zi)考虑到每个质点的质量一样，那么质心的计算方法为：
将观测站的位置看为质点的位置，按照上式就可以计算出目标位置的估计值。当观测站分步稀疏时，这个定位的误差会较大。
例：假定有6个观测站，随机部署在长宽均为100米的场地上，每个观测站完全相同，探测距离均为50米。一个目标随机出现在场地内，被观测站观测到，并计算估计位置。Matlab仿真如下：
function CentroidLocalization %质心定位算法
%定位初始化
Length=100; %场地空间，单位：米
Width=100; %场地空间，单位：米
d=50; %观测站探测能力50m
N=6; %观测站的个数
for i=1:N %观测站的位置初始化
Node(i).x=Width*rand;
Node(i).y=Length*rand;
end
%目标的真实位置，这里也随机给出
Target.x=Width*rand;
Target.y=Length*rand;
X=[ ]; %用于保存探测到目标的探测站的位置
for i=1:N
if getDist(Node(i),Target)<=d %调用距离子函数，getDist()函数必须写在另一个m文件中，见末尾
X=[X;Node(i).x, Node(i).y]; %保存探测到目标的探测站的位置
end
end
M=size(X,1); %探测到目标的观测站个数
if M>0
Est_Target.x=sum(X(:,1))/M; %质心的估计位置x
Est_Target.y=sum(X(:,2))/M; %质心的估计位置y
Error_Dist=getDist(Est_Target,Target) %质心估计值与实际值的误差
end
%开始画图
figure
hold on;box on;axis([0 100 0 100]);
for i=1:N
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');
%画出目标周围，以探测能力50m为半径的圆形d范围；circle()函数写在circle.m文件中
circle(Target.x,Target.y,d);
%表明h1,h2,h3是什么
legend([h1,h2,h3],'Observation Station','Target Postion','Estimate Postion');
xlabel(['error=',num2str(Error_Dist),'m']);
%计算两点距离函数，必须写在另一个m文件中
function dist=getDist(A,B)
dist=sqrt((A.x-B.x)^2+(A.y-B.y)^2);
end
%画圆函数，必须写在另一个m文件中
function circle(x0,y0,r)
sita=0:pi/20:2*pi;
plot (x0+r*cos(sita),y0+r*sin(sita));
end
运行结果：
两次运行结果，误差分别为8.6m和28.0m，两次定位误差相差很大，定位误差与观测站的位置及部署疏密关系密切。
2.加权质心定位算法
探测目标时，传感器可以根据探测到目标信号的强度，如声音信号强度或无线接收信号强度（RSSI），粗略地判断目标的远近，并将这个强度作为一个权值，用在质心定位算法中：
一般地，权值与距离有一定的比例关系，如RSSI遇距离近似有反比的关系，建模时可以粗略地认为：
注：r为目标与观测站的距离；n为噪声，表示观测站测得的RSSI和声音信号的强度是受到噪声干扰的。噪声的大小给定方法可以参考信噪比（Signal to Noise Ratio，SNR）参数：
注：S表示信号值，这里表示接收到的信号强度值；N表示噪声的方差。假如信噪比为10dB，那么 N = S / 2 N=S/ \sqrt{2} \quad N=S/2
例：在一个100m*100m的场地上随机部署6个观测站，每个观测站探测的距离为50m，且能探测到目标距离观测站的声音信号的强弱。观测站对出现在场地内的目标进行探测，MATLAB仿真：
function WeightCentroidLocalization %加权质心定位算法
%定位初始化
Length=100; %场地空间，单位：米
Width=100; %场地空间，单位：米
d=50; %观测站探测能力50m
Node_number=6; %观测站的个数
SNR=50; %信噪比
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=[ ];W=[ ]; %权值
for i=1:Node_number
dd=getdist(Node(i),Target);
Q=dd/(10^(20/SNR)); %根据信噪比公式求噪声方差
if dd<=d
X=[X;Node(i).x, Node(i).y];
W=[W,1/((dd+sqrt(Q)*randn)^2)]; %根据公式计算权值
end
end
%权值归一化
W=W./sum(W);
N=size(X,1); %探测到目标的观测站个数
sumx=0;sumy=9;
for i=1:N
sumx=sumx+X(i,1)*W(i);
sumy=sumy+X(i,2)*W(i);
end
Est_Target.x=sumx; %目标的估计位置x
Est_Target.y=sumy; %目标估计位置y
Error_Dist=getdist(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');
%画出目标周围，以探测能力50m为半径的圆形d范围；circle()函数写在circle.m文件中
circle(Target.x,Target.y,d);
%表明h1,h2,h3是什么
legend([h1,h2,h3],'Observation Station','Target Postion','Estimate Postion');
xlabel(['error=',num2str(Error_Dist),'m']);
%计算两点距离函数，必须写在另一个m文件中
function dist=getdist(A,B)
dist=sqrt((A.x-B.x)^2+(A.y-B.y)^2);
end
%画圆函数，必须写在另一个m文件中
function circle(x0,y0,r)
sita=0:pi/20:2*pi;
plot (x0+r*cos(sita),y0+r*sin(sita));
end
运行结果：
3.网格定位算法
将观测站均匀地部署在检测场地，并整齐排列成队列网格的形式，称为网格定位算法。
例：假定目标在100m*100m的场地内，每隔20m放置一个观测站，观测站的探测距离都是50m，当有多个观测站探测到目标时，以探测到目标信号最强的3个基站的位置的质心作为目标的估计位置，MATLAB仿真如下：
function CridLocalization %网格定位算法
%定位初始化
Length=100; %场地空间，单位：米
Width=100; %场地空间，单位：米
Xnum=5; %观测站在水平方向的个数
Ynum=5; %观测站在垂直方向的个数
divX=Length/Xnum/2;divY=Width/Ynum/2;%为了在正中间查看观测站分步调节量
d=50; %观测站探测能力50m
Target.x=Width * (Xnum-1)/Xnum * rand;
Target.y=Length * (Ynum-1)/Ynum * rand;
DIST=[ ]; %放置观测站与目标之间距离的集合
for j=1:Ynum %观测站的网格部署
for i=1:Xnum
Station((j-1) * Xnum+i).x=(i-1) * Length/Xnum;
Station((j-1) * Xnum+i).y=(j-1) * Width/Ynum;
dd=getdist(Station((j-1) * Xnum+i) ,Target);
DIST=[DIST dd];
end
end
%找出探测到目标信号最强的3个观测站，也就是离目标最近的观测站
[set,index] =sort( DIST); % set为排列好的从小到大的数值集合, index 为索引集合
NI=index(1:3); %最近的3个,即index 1-3个元素
Est_Target.x=0;Est_Target.y=0;
if set(NI(3))运行结果：
