[MatLab] Sistema si secondo grado con Matlab

[mat092]

Salve a tutti, ho un problema con un esercitazione di meccanica applicata alle macchine. Il professore ha chiesto di costruire (virtualmente) un quadrilatero articolato e attraverso matlab di calcolare due dei 4 angoli (due li conosco) in una determinata posizione. Ho le equazioni di chiusura (non lineari) che sono due, e due sono le incognite. Dunque uso il metodo delle tangenti (Newton-Raphson) ma quando inserisco vettori di partenza diversi (x0) i risultati finali mi vengono diversi e ciò non può essere perchè qualsiasi vettore di partenza io utilizzi mi dovrà convergere alla stessa soluzione. Forse ho sbagliato a calcolare lo Jacobiano non so, aiutatemi...Questo è il mio script:

function x = NR(x0,tol,itmax)
 
r(1)=1.97;
r(2)=3.12;
r(3)=3.49;
r(4)=6.14;
theta(1)= pi/4;
theta(4)= pi;


J(1,1) = -r(2)*sin( x0(1));
J(1,2) = -r(3)*sin( x0(2));
J(2,1) =  r(2)*cos( x0(1));
J(2,2) =  r(3)*cos( x0(2));

for i= 1 : itmax
        f(1) = r(1)*cos( theta(1)) + r(2)*cos( x0(1)) + r(3)*cos( x0(2)) + r(4)*cos( theta(4));
    f(2) = r(1)*sin( theta(1)) + r(2)*sin( x0(1)) + r(3)*sin( x0(2)) + r(4)*sin( theta(4));
    resid = norm(f);
if resid > tol
    x = x0 - f/J; 
    x0 = x;
else 
    x = x0 ;
    return
    
end

end

if x(1) > 3.14
    x(1) = x(1)/(2*3.14);
else return
end
    
    if x(2) > 3.14
    x(2) = x(1)/(2*3.14);
else return
    end

end