%長基線聲學(xué)定位系統(tǒng)一個應(yīng)答器

clear;close;clc

axis（[0?100?0?100]）;hold?on;
x=[80?20?60?90];y=[30?20?60?90];
K=atan（（y（4）-y（2））/（x（4）-x（2）））;
scatter（xy‘fill‘‘r‘）;
line（[x?x（1）?x（3）][y?y（1）?y（3）]）;
text（x（1）y（1）‘P‘）;text（（x（1）+x（2））/2（y（1）+y（2））/2‘DA‘）;
text（x（2）y（2）‘A‘）;text（（x（2）+x（3））/2（y（2）+y（3））/2‘AB‘）;
text（x（3）y（3）‘B‘）;text（（x（3）+x（4））/2（y（3）+y（4））/2‘BC‘）;
text（x（4）y（4）‘C‘）;text（（x（4）+x（1））/2（y（4）+y（1））/2‘DC‘）;
text（（x（1）+x（3））/2（y（1）+y（3））/2‘DB‘）;

DA=sqrt（（x（1）-x（2））^2+（y（1）-y（2））^2）;
DB=sqrt（（x（1）-x（3））^2+（y（1）-y（3））^2）;
DC=sqrt（（x（1）-x（4））^2+（y（1）-y（4））^2）;
AB=sqrt（（x（2）-x（3））^2+（y（2）-y（3））^2）;
BC=sqrt（（x（3）-x（4））^2+（y（3）-y（4））^2）;

x0=0;y0=0;
k=0;
while?1>0
????S1=sqrt（（x0-x（1））^2+（y0-y（1））^2）;
????S2=sqrt（（x0-x（2））^2+（y0-y（2））^2）;
????S3=sqrt（（x0-x（4））^2+（y0-y（4））^2）;
????r1=S1-DB;
????r2=S2-AB;
????r3=S3-BC;
????r=[r1?r2?r3]‘;
????D_r=[（x0-x（1））/S1（y0-y（1））/S1;
????????（x0-x（2））/S2（y0-y（2））/S3;
????????（x0-x（4））/S3（y0-y（4））/S3];
????A=D_r;
????v=-pinv（A‘*A）*A‘*r;
????a=x0;
????x0=x0+v（1）;y0=y0+v（2）;
????k=k+1;
????if?abs（a-x0）<0.1
????????break
????end
end

xA=x0+AB*cos（pi+K）;yA=y0+AB*sin（pi+K）;
xC=x0+BC*cos（K）;yC=y0+BC*sin（K）;
fprintf（‘船只的定位坐標\n?（xAyA）=（%f%f）\n?（xByB）=（%f%f）\n?（xCyC）=（%f%f）\n\n‘xAyAx0y0xCyC）;
fprintf（‘船只的實際坐標\n?（xAyA）=（%f%f）\n?（xByB）=（%f%f）\n?（xCyC）=（%f%f）\n‘x（2）y（2）x（3）y（3）x（4）y（4））;
xlabel（‘x‘）;ylabel（‘y‘）;
view（90-90）

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2019-02-25?19:02??長基線定位\
?????文件????????1489??2018-05-26?15:03??長基線定位\one_transponder.m
?????文件????????1205??2018-06-21?10:06??長基線定位\three_transponder.m
?????文件????????1146??2018-05-26?15:59??長基線定位\two_transponder.m