fc4.m

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

function YY=fc4(X) %函数表达式文件：三元方程（已化简）%
% 定义一个函数，x为船上声呐的位置数据 %
% R为应答器到船上声呐的距离 %
% X0为布放时应答器的位置 %
% A 为矢量水听器测得的角度 %
global xx
global A
global R
global aa

y1=(X(1)-xx(1,1))^2+(X(2)-xx(1,2))^2+(X(3)-xx(1,3))^2-R(1,1)^2; %方程1
y2=(X(1)-xx(2,1))^2+(X(2)-xx(2,2))^2+(X(3)-xx(2,3))^2-R(2,1)^2; %方程2
%xxx=(sin(A)).^2.*(R(1).^2-(X(3)-xx(1,3)).^2)-(X(1)-xx(1,1)).^2;

l11=sqrt((xx(1,1)-xx(2,1))^2+(xx(1,2)-xx(2,2))^2);
%l12=R(1,1)^2-(X(3)-xx(1,3))^2;
%l13=R(2,1)^2-(X(3)-xx(2,3))^2;
%y3=l12+l13-2*sqrt(l12*l13)*cos(aa)-l11^2;
r1=(X(1)-xx(1,1))^2+(X(2)-xx(1,2))^2;
r2=(X(1)-xx(2,1))^2+(X(2)-xx(2,2))^2;
y3=r1+r2-2*sqrt(r1)*sqrt(r2)*cos(aa)-l11^2; %方程3
YY=[y1 y2 y3];