牛顿法求解Stewart平台运动学正解

function  Newton()
format long
x0=[0;0;20;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;
p1=[12;-2;0];
p2=[12;2;0];
p3=[-4.26794919;11.39230485;0];
p4=[-7.73205081;9.39230485;0];
p5=[-7.73205081;-9.39230485;0];
p6=[-4.26794919;-11.39230485;0];
r1=[12;-14;5];
r2=[12;14;5];
r3=[6.12435565;17.39230485;5];
r4=[-18.12435565;3.39230485;5];
r5=[-18.12435565;-3.39230485;5];
r6=[6.12435565;-17.39230485;5];
l1=20;
l2=25;
l3=26;
l4=20;
l5=20;
l6=20;
f1=(norm(X+R*p1-r1))^2-l1^2;
f2=(norm(X+R*p2-r2))^2-l2^2;
f3=(norm(X+R*p3-r3))^2-l3^2;
f4=(norm(X+R*p4-r