clear all;clc
format long
fi = [0 pi/2 pi 3*pi/2]';
r = [58 40 22 28]';

x = [r.*cos(fi)];
y = [r.*sin(fi)];

figure (1)
hold on
plot (x,y,'*')
plot (0,0,'ro')
hold off

f = @(x) x(1)./(1-x(2)*cos(fi-x(3)))-r

Jf=@(x) [1./(1-x(2)*cos(fi-x(3))), (x(1).*cos(fi-x(3)))./((x(2).*cos(fi-x(3))-1).^2), (x(1).*x(2).*-sin(x(3)-fi))./((cos(x(3)-fi)*x(2)-1).^2)]

x = [0 0 0]'

for i=1:20
     x = x+(-pinv(Jf(x))*f(x))
end

figure(2);hold on
plot(r.*cos(fi),r.*sin(fi),'b*')
plot(r.*cos(fi),r.*sin(fi),x(1)./(1-x(2)*cos(fi-x(3)))-r)