function Newton()
format long
x0=[0;0;200;0;0;0];
x1=x0-inv(myJacobi(x0))*myfun(x0);
while norm(x1-x0)>1e-3
x0=x1;
x1=x0-inv(myJacobi(x0))*myfun(x0);
end
x1
end
function f=myfun(x)
syms x1 x2 x3 a b g
X=[x1;x2;x3];
RX=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
RY=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
RZ=[cos(g) -sin(g) 0;sin(g) cos(g) 0; 0 0 1];
R=RZ*RY*RX;
B1P1=[ 121 -140 0 120 -20 0; %初始参数矩阵%B1、P1
120 140 0 120 20 0;
61.243557 173.92305 0 -42.679489 113.92304 0;
-181.24356 33.92305 0 -77.320511 93.923042 0;
-181.24356 -33.92305 0 -77.320511 -93.923042 0;
61.243557 -173.92305 0 -42.679489 -113.92304 0];