x(1)=3;
y(1)=5.5;
r(1)=5;
x(2)=6;
y(2)=14;
r(2)=7;
x(3)=9;
y(3)=2;
r(3)=8;
x(4)=11;
y(4)=8;
r(4)=6;
ans=[0;0;0;];
for i =1:4
r0(i)=sqrt((x(i)-ans(1))^2+(y(i)-ans(2))^2)+ans(3);
end
for i = 1:4
g(i,1) = -(x(i)-ans(1))/r(i);
g(i,2) = -(y(i)-ans(2))/r(i);
g(i,3) = 1;
end
for i =1:4
dr(i) = r(i)-r0(i);
end
dans = inv (g'*g)*g'*dr'
ans = ans + dans
error = norm(dans)