fixed the problem! Cannot just iterate, must Solve cos and sin one at a time, otherwise cannot treat the other one as a constant! Now it converges, but very slowly. Takes like 10 iterations. However is still very stable. Something is wrong with the jacobian