TOA Estimate定位算法

基于TOA(到达时间法）测距的定位算法程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 基于TOA的目标定位算法
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function TOAEstimate
Length=100; 
Width=100;  
Node_number=3; 
for i=1:Node_number
    Node（i）.x=Width*rand; 
    Node（i）.y=Length*rand;
    Node（i）.D=Node（i）.x^2+Node（i）.y^2; 
end
Target.x=Width*rand;
Target.y=Length*rand;
BroadcastPacket=0; 
ultrasonicV=340; 
sendData（BroadcastPacket）;
delaytime=10;  
delay（delaytime）;
sendUltraPlus（）;
uT=[];  
for i=1:Node_number
    recvUltraPlus（）; 
   
    [d]=DIST（Node（i）Target）;  
    uT（i）=GetTimeLength（d）;
end
Zd=[];
for i=1:Node_number
    Zd（i）=uT（i）*ultrasonicV; 
end
H=[];b=[];
for i=2:Node_number
    H=[H;2*（Node（i）.x-Node（1）.x）2*（Node（i）.y-Node（1）.y）];  
    b=[b;Zd（1）^2-Zd（i）^2+Node（i）.D-Node（1）.D];
end
Estimate=inv（H‘*H）*H‘*b; 
Est_Target.x=Estimate（1）;Est_Target.y=Estimate（2）;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
hold on;box on;axis（[0 120 0 120]）; % 输出图形的框架
for i=1:Node_number
    h1=plot（Node（i）.xNode（i）.y‘ko‘‘MarkerFace‘‘g‘‘MarkerSize‘10）;
    text（Node（i）.x+2Node（i）.y[‘Node ‘num2str（i）]）;
end
h2=plot（Target.xTarget.y‘k^‘‘MarkerFace‘‘b‘‘MarkerSize‘10）;
h3=plot（Est_Target.xEst_Target.y‘ks‘‘MarkerFace‘‘r‘‘MarkerSize‘10）;
line（[Target.xEst_Target.x][Target.yEst_Target.y]‘Color‘‘k‘）;
legend（[h1h2h3]‘Observation Station‘‘Target Postion‘‘Estimate Postion‘）;
[Error_Dist]=DIST（Est_TargetTarget）;
xlabel（[‘error=‘num2str（Error_Dist）‘m‘]）;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [dist]=DIST（AB）
dist=sqrt（（A.x-B.x）^2+（A.y-B.y）^2）;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function time=GetTimeLength（d）
ultrasonicV=340;
time=d/ultrasonicV;
Q=5e-6;
time=time+sqrt（Q）*randn; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function sendData（BroadcastData）
disp（‘The TargetNode send wireless data success !\n‘）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function sendUltraPlus（）
disp（‘The TargetNode send ultrasonic plus success !\n‘）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function recvUltraPlus（）
disp（‘The ObserNode receive ultrasonic plus success !\n‘）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function delay（delaytime）
disp（‘System delay for sometime!\n‘）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2012-10-24 08:56  TOAEstimate\
     文件        2509  2012-08-19 10:13  TOAEstimate\TOAEstimate.m