close all;
clear all;
clear;
format long g;

c = 299792.458;

A = [15600; 18760; 17610;19170];
B = [7540; 2750; 14630; 610];  C1 = 20140;  t1 = 0.07074;  %%coordinates of first satellite
C = [20140; 18610; 13480; 18390];
t = [0.07074; 0.07220; 0.07690; 0.07242];


x0 = [0; 100; 6400; 0.0001];           %% initial guess for part 4
tol = .05e-8;                  %% error tolerance
n = 0;                          %% iteration count
ae = 10;
f = zeros(4,1);
df = zeros(4,4);
    
while ae>tol
    for i = 1:4
        f(i) = (x0(1)-A(i))^2 + (x0(2)-B(i))^2 + (x0(3)-C(i))^2 - c^2*(t(i)-x0(4))^2;
        df(i,:) = [2*(x0(1)-A(i)),2*(x0(2)-B(i)),2*(x0(3)-C(i)),2*c^2*(t(i)-x0(4))];
    end;
    v = df\-f;
    x1 = x0 + v;
    ae = norm(x1 - x0);
    n  = n + 1;
    x0 = x1;
    if n == 10000
        break
    end;
end;    
n
x1