长基线定位.zip

利用长基线定位方法对水面船只进行定位模拟，分别考虑三种情况。第一种是只有一个应答器，对船只进行三次探测，得到的数据进行船只定位；第二种是两个应答器，对船只进行定位，前两种方法只能对船只进行距离探测，未涉及三维信息；第三种是利用三个应答器对船只进行探测、定位，得出船只具体的位置信息。

%长基线声学定位系统一个应答器

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