6x6 matrix Newton-Raphson help needed!!!

Hi Matlab experts,

Please help me, I want to build a script using Newton Raphson method to calculate 6 variables: X, Y, Z, alpha, beta, gama as matrix below. But I cannot succeed. Please help correct me. Many thanks!

syms X Y Z alpha beta gama;
%#codegen
deg2rad=pi/180;
theta_b=15*deg2rad;
theta_p=15*deg2rad;
R_b=1;
R_p=0.9;
Q0=1; %initial leg length
p=[X;Y;Z];
B1=[R_b*cos(15*deg2rad);R_b*sin(15*deg2rad);0];
B2=[R_b*cos(105*deg2rad);R_b*sin(105*deg2rad);0];
B3=[R_b*cos(135*deg2rad);R_b*sin(135*deg2rad);0];
B4=[R_b*cos(225*deg2rad);R_b*sin(225*deg2rad);0];
B5=[R_b*cos(255*deg2rad);R_b*sin(255*deg2rad);0];
B6=[R_b*cos(345*deg2rad);R_b*sin(345*deg2rad);0];

Bx1=B1(1,:);
Bx2=B2(1,:);
Bx3=B3(1,:);
Bx4=B4(1,:);
Bx5=B5(1,:);
Bx6=B6(1,:);
 
By1=B1(2,:);
By2=B2(2,:);
By3=B3(2,:);
By4=B4(2,:);
By5=B5(2,:);
By6=B6(2,:);
 
Bz1=B1(3,:);
Bz2=B2(3,:);
Bz3=B3(3,:);
Bz4=B4(3,:);
Bz5=B5(3,:);
Bz6=B6(3,:);
 
P1=[R_p*cos(75*deg2rad);R_b*sin(75*deg2rad);0];
P2=[R_p*cos(165*deg2rad);R_b*sin(165*deg2rad);0];
P3=[R_p*cos(195*deg2rad);R_b*sin(195*deg2rad);0];
P4=[R_p*cos(285*deg2rad);R_b*sin(285*deg2rad);0];
P5=[R_p*cos(315*deg2rad);R_b*sin(315*deg2rad);0];
P6=[R_p*cos(405*deg2rad);R_b*sin(405*deg2rad);0];

Px1=P1(1,:);
Px2=P2(1,:);
Px3=P3(1,:);
Px4=P4(1,:);
Px5=P5(1,:);
Px6=P6(1,:);

Py1=P1(2,:);
Py2=P2(2,:);
Py3=P3(2,:);
Py4=P4(2,:);
Py5=P5(2,:);
Py6=P6(2,:);

Pz1=P1(3,:);
Pz2=P2(3,:);
Pz3=P3(3,:);
Pz4=P4(3,:);
Pz5=P5(3,:);
Pz6=P6(3,:);

L1=1.1560;
L2=1.7842;
L3=1.8500;
L4=1.1317;
L5=0.8071;
L6=0.9340;

f1=((X+((cos(gama)*cos(beta))*Px1)+(((cos(gama)*sin(beta)*sin(alpha))-(sin(gama)*cos(alpha)))*Py1)+(((cos(gama)*sin(beta)*cos(alpha))+(sin(gama)*sin(alpha)))*Pz1)-Bx1)^2+(Y+((sin(gama)*cos(beta))*Px1)+(((sin(gama)*sin(beta)*sin(alpha))+(cos(gama)*cos(alpha)))*Py1)+(((sin(gama)*sin(beta)*cos(alpha))-(cos(gama)*sin(alpha)))*Pz1)-By1)^2+(Z+((-sin(beta))*Px1)+((cos(beta)*sin(alpha))*Py1)+((cos(beta)*cos(alpha))*Pz1)-Bz1)^2)^0.5-Q0-L1;
f2=((X+((cos(gama)*cos(beta))*Px1)+(((cos(gama)*sin(beta)*sin(alpha))-(sin(gama)*cos(alpha)))*Py1)+(((cos(gama)*sin(beta)*cos(alpha))+(sin(gama)*sin(alpha)))*Pz1)-Bx1)^2+(Y+((sin(gama)*cos(beta))*Px1)+(((sin(gama)*sin(beta)*sin(alpha))+(cos(gama)*cos(alpha)))*Py1)+(((sin(gama)*sin(beta)*cos(alpha))-(cos(gama)*sin(alpha)))*Pz1)-By1)^2+(Z+((-sin(beta))*Px1)+((cos(beta)*sin(alpha))*Py1)+((cos(beta)*cos(alpha))*Pz1)-Bz1)^2)^0.5-Q0-L2;
f3=((X+((cos(gama)*cos(beta))*Px1)+(((cos(gama)*sin(beta)*sin(alpha))-(sin(gama)*cos(alpha)))*Py1)+(((cos(gama)*sin(beta)*cos(alpha))+(sin(gama)*sin(alpha)))*Pz1)-Bx1)^2+(Y+((sin(gama)*cos(beta))*Px1)+(((sin(gama)*sin(beta)*sin(alpha))+(cos(gama)*cos(alpha)))*Py1)+(((sin(gama)*sin(beta)*cos(alpha))-(cos(gama)*sin(alpha)))*Pz1)-By1)^2+(Z+((-sin(beta))*Px1)+((cos(beta)*sin(alpha))*Py1)+((cos(beta)*cos(alpha))*Pz1)-Bz1)^2)^0.5-Q0-L3;
f4=((X+((cos(gama)*cos(beta))*Px1)+(((cos(gama)*sin(beta)*sin(alpha))-(sin(gama)*cos(alpha)))*Py1)+(((cos(gama)*sin(beta)*cos(alpha))+(sin(gama)*sin(alpha)))*Pz1)-Bx1)^2+(Y+((sin(gama)*cos(beta))*Px1)+(((sin(gama)*sin(beta)*sin(alpha))+(cos(gama)*cos(alpha)))*Py1)+(((sin(gama)*sin(beta)*cos(alpha))-(cos(gama)*sin(alpha)))*Pz1)-By1)^2+(Z+((-sin(beta))*Px1)+((cos(beta)*sin(alpha))*Py1)+((cos(beta)*cos(alpha))*Pz1)-Bz1)^2)^0.5-Q0-L4;
f5=((X+((cos(gama)*cos(beta))*Px1)+(((cos(gama)*sin(beta)*sin(alpha))-(sin(gama)*cos(alpha)))*Py1)+(((cos(gama)*sin(beta)*cos(alpha))+(sin(gama)*sin(alpha)))*Pz1)-Bx1)^2+(Y+((sin(gama)*cos(beta))*Px1)+(((sin(gama)*sin(beta)*sin(alpha))+(cos(gama)*cos(alpha)))*Py1)+(((sin(gama)*sin(beta)*cos(alpha))-(cos(gama)*sin(alpha)))*Pz1)-By1)^2+(Z+((-sin(beta))*Px1)+((cos(beta)*sin(alpha))*Py1)+((cos(beta)*cos(alpha))*Pz1)-Bz1)^2)^0.5-Q0-L5;
f6=((X+((cos(gama)*cos(beta))*Px1)+(((cos(gama)*sin(beta)*sin(alpha))-(sin(gama)*cos(alpha)))*Py1)+(((cos(gama)*sin(beta)*cos(alpha))+(sin(gama)*sin(alpha)))*Pz1)-Bx1)^2+(Y+((sin(gama)*cos(beta))*Px1)+(((sin(gama)*sin(beta)*sin(alpha))+(cos(gama)*cos(alpha)))*Py1)+(((sin(gama)*sin(beta)*cos(alpha))-(cos(gama)*sin(alpha)))*Pz1)-By1)^2+(Z+((-sin(beta))*Px1)+((cos(beta)*sin(alpha))*Py1)+((cos(beta)*cos(alpha))*Pz1)-Bz1)^2)^0.5-Q0-L6;

F=[f1;f2;f3;f4;f5;f6];

G=[diff(f1,X) diff(f1,Y) diff(f1,Z) diff(f1,alpha) diff(f1,beta) diff(f1,gama);diff(f2,X) diff(f2,Y) diff(f2,Z) diff(f2,alpha) diff(f2,beta) diff(f2,gama);diff(f3,X) diff(f3,Y) diff(f3,Z) diff(f3,alpha) diff(f3,beta) diff(f3,gama);diff(f4,X) diff(f4,Y) diff(f4,Z) diff(f4,alpha) diff(f4,beta) diff(f4,gama);diff(f5,X) diff(f5,Y) diff(f5,Z) diff(f5,alpha) diff(f5,beta) diff(f5,gama);diff(f6,X) diff(f6,Y) diff(f6,Z) diff(f6,alpha) diff(f6,beta) diff(f6,gama)];
H=inv(G);
F0=[0;0;0;0;0;0];
n=10;
e=10^-4;

for i=1:n
F1=F0-H*F;
F0=F1;
end
X
Y
Z
alpha
beta
gama

[Jan]

When I run the code, I get the error message for deg2rad, because the input is missing. Replace

cos(15*deg2rad)

by

cos(deg2rad(15))

or even better by

cosd(15)

The pile of variables with an index hidden in the name is a bad idea. The code would be much easier, if you use arrays instead. Compare

B1=[R_b*cos(15*deg2rad);R_b*sin(15*deg2rad);0];
B2=[R_b*cos(105*deg2rad);R_b*sin(105*deg2rad);0];
B3=[R_b*cos(135*deg2rad);R_b*sin(135*deg2rad);0];
B4=[R_b*cos(225*deg2rad);R_b*sin(225*deg2rad);0];
B5=[R_b*cos(255*deg2rad);R_b*sin(255*deg2rad);0];
B6=[R_b*cos(345*deg2rad);R_b*sin(345*deg2rad);0];

with

v = [15, 135, 225, 255, 345];
B = R_b * [cosd(v); sind(v); zeros(size(v))];

Multiplying by the inverse is less accurate than using \ . See: doc inv

[Jack Dong]

Thank you Jan for the advice!