%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%?基于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