dingwein.m

求解定位方程组。并直接利用三元方程求出X,Y,Z时的误差子函数

% 利用二元距离方程来求出X、Y。Z用测深仪来测;直接利用三元方程求出X,Y,Z时的误差子函数
%function YY=dingwein   %直接求利用二元方程求出X,Y%
global xx
global A
global R
global Z0
% xx为船上声呐的位置数据 %
% R为应答器到船上声呐的距离 %
% X0为布放时应答器的位置 %
% A 为矢量水听器测得的角度,为在平面上的角度，以正北为基准，
        %顺时针为正，逆时针为负， 范围为(-pi,pi)%

%load ss;  %不用global定义变量，而是直接导入数据，再赋给xx,A,R,X0%
xx=[-3000 4000 30;3000 4000 30]; %测量得到的数据
R=[sqrt(25000000+(5000-30)^2);sqrt(25000000+(5000-30)^2)];
%a=[asin(sqrt(9/25));asin((-1)*sqrt(9/25))];
X0=[21 -15 5000];

if xx(1,1)>xx(2,1)  %将在X-Y平面上靠近Y轴的点放在前面
    xx=[xx(2,1:3);xx(1,1:3)];
    %a=[a(2,1);a(1,1)];
    R=[R(2,1);R(1,1)]; 
end
%aa=a(2,1)-a(1,1);  %求组成的平面三角形的内角
%qq=fc3(X0);
Z0=X0(1,3);
X00=X0(1,1:2);
yy=fsolve('fcn',X00,1.0e-10);
YY=[yy Z0];