Inverse Kinematics Algorithm solving problem using Optimisation Methods

Hello,

After writing the forward kinematics equations of 3DoF Manipulator:

X = l1 * cos(THETA1) + l2 * cos(THETA1 + THETA2) + l3 * cos(THETA1 + THETA2 + THETA3)

Y = l1 * sin (THETA1) + l2 * sin(THETA1 + THETA2) + l3 * sin(THETA1 + THETA2 + THETA3)

PHI = THETA1 + THETA2 + THETA3*

Constrained as: TETA1 in [-68,103]°; TETA2 in [-130,130]°; TETA1 in [-90,90]°; In other meanings, the INPUTS are X/Z/PHI and the Unknows to be solved are the THETAs.

I have used lsqnonlin function to solve the Inverse Kinematix problem, with the following code:

x0 = [deg2rad(0),deg2rad(0),deg2rad(0)];
lb = [deg2rad(-68),deg2rad(-130),deg2rad(-90)];
ub = [deg2rad(130),deg2rad(130),deg2rad(90)];

tetas = rad2deg(lsqnonlin(@(t)fsystem(t,l1,l2,l3,X0Pitch,Z0Pitch,Xwall,Zwall,phi),x0,lb,ub,options))
X_calculated = forwardX(deg2rad(tetas),l1,l2,l3,X0Pitch);
Z_calculated = forwardZ(deg2rad(tetas),l1,l2,l3,Z0Pitch); 
ErrorX = abs(X_calculated-Xwall)
ErrorZ = abs(Z_calculated-Zwall)
ErrorPhi = sum(tetas)

And the function used is:

    function Diff = fsystem(tetas,l1,l2,l3,X0Pitch,Z0Pitch,Xwall,Zwall,phi)
tet1 = tetas(1); tet2 = tetas(2); tet3 = tetas(3);%tet3 = tetas(3);
Diff(1) =  abs(X0Pitch + l1*cos(tet1)+l2*cos(tet1+tet2)+l3 - Xwall) ; 
Diff(2) =  abs(Z0Pitch + l1*sin(tet1)+l2*sin(tet1+tet2) - Zwall) ; 
Diff(3) =  tet1 + tet2 + tet3 - phi; 
end

I used also to compute the Errors of the three variables (teta1,teta2,teta3, and PHI) using the forward kinematics in order to validate the algorithm. The found 3 variables validate the equation (1) and (2) but not the equation (3) ( ErrorX and ErrorZ are enough close to zero, however ErrorPHI is not and is far to be close to zero )

Looks like my algorithm solve the 2 first equations and ignore totally the third one. Does anyone has an idea why ? Or does anybody has used another method to solve such Non-Linear Constrained equations system ? I'd be very grateful for your helps.

Thanks in advance.