目标定位算法(一)之非测距的定位算法

非测距的定位算法

这类定位算法依靠观测站的物理部署和简单的探测到目标的“有”和“无”的二进制探测信息对目标进行估计。主要有质心定位算法加权质心定位算法网络定位算法

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))<d                       %检查3个中最大那个数是否在观测站可探测距离范围之内
    for i=1:3
        Est_Target.x= Est_Target.x+Station(NI(i)).x/3;
        Est_Target.y= Est_Target.y+Station(NI(i)).y/3;
    end
end

%开始画图
figure
hold on;box on;axis([0-divX,Length-divX,0-divY,Width-divX])
xx=[Station(NI(1)).x,Station(NI(2)).x,Station(NI(3)).x];
yy=[Station( NI(1)).y,Station( NI(2)).y,Station(NI(3)).y];
fill(xx,yy,'y');
for j=1:Ynum
    for i=1:Xnum
        h1=plot(Station((j-1) * Xnum+i).x,Station((j-1) * Xnum+i).y,'-ko','MarkerFace','g');
        text(Station((j-1) * Xnum+i).x+1,Station((j-1) * Xnum+i).y, num2str((j-1) * Xnum+i));
    end
end
Error_Est=getdist( Est_Target,Target)
h2=plot(Target.x,Target.y,'k^','MarkerFace','b' ,'MarkerSize', 10) ;
h3=plot(Est_Target.x,Est_Target.y,'ks','MarkerFace','r','MarkerSize',10);
legend([h1,h2,h3] ,'Observation Station', 'Target Postion' ,'Estimate Postion');
xlabel(['error=',num2str(Error_Est),'m']);

%计算两点距离函数,必须写在另一个m文件中
function dist=getdist(A,B)
dist=sqrt((A.x-B.x)^2+(A.y-B.y)^2);
end

运行结果:
在这里插入图片描述

  • 0
    点赞
  • 50
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值