利用fsolve函数求解局部坐标系在全局坐标系中的位姿--一种奇怪的方法

function position_after_3_point_coord()
format short
options = optimset('MaxFunEvals',300,'MaxIter',300)
x=fsolve(@myfun,[0;0;0],options) %调用函数
end

function f=myfun(x)
syms a b g
A0=[119.806374;149.341095;4.584520];% A点在全局系中的坐标
B0=[372.825714;343.291016;32.235802];% B点在全局系中的坐标
C0=[148.859406;373.197510;20.230499];% C点在全局系中的坐标
D0=[246.3183;246.3180;18.3967];% 局部系原点在全局系中的坐标
A1_x=norm([A0(1,1)-D0(1,1);A0(2,1)-D0(2,1);A0(3,1)-D0(3,1)]);%求解A点到原点距离
A1=[-(A1_x);0;0];%A点在局部系中的坐标
B1_x=norm([B0(1,1)-D0(1,1);B0(2,1)-D0(2,1);B0(3,1)-D0(3,1)]);%求解B点到原点距离
B1=[B1_x;0;0];%B点在局部系中的坐标
C1_y=norm([C0(1,1)-D0(1,1);C0(2,1)-D0(2,1);C0(3,1)-D0(3,1)]);%求解C点到原点距离
C1=[0;C1_y;0];%C点在局部系中的坐标
RZ=[cos(g) -sin(g) 0;sin(g) cos(g) 0; 0 0 1];
RY=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
RX=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
R=RZ*RY*RX;%局部系与全局系的坐标变换矩阵，表示局部系在全局系中的位姿
f1=(norm(D0+R*A1))^2-(norm(A0))^2;%闭环矢量方程
f2=(norm(D0+R*B1))^2-(norm(B0))^2;%闭环矢量方程
f3=(norm(D0+R*C1))^2-(norm(C0))^2;%闭环矢量方程
f=[f1;f2;f3];
a=x(1);
b=x(2);
g=x(3);
f=eval(f);
end