% Step 3
% Define satelite locations and time corrections A,B,C,t
A1 = 15600;
B1 = 7540;
C1 = 10380;
T1 = .0593200;
A2 = 11760;
B2 = 2750;
C2 = 16190;
T2 = .0519200;
A3 = 11610;
B3 = 14630;
C3 = 7680;
T3 = .0624200;
A4 = 15170;
B4 = 610;
C4 = 13320;
T4 = .0557100;
c = 299792.458;
%%
x0=[0;0;6370;0];
x=x0(1);
y=x0(2);
z=x0(3);
d=x0(4);
index = 0;
for i=1:35
    Df=[(2*x-2*A1) (2*y-2*B1) (2*z-2*C1) (-2*d*(c^2)+2*T1*(c^2));...
        (2*x-2*A2) (2*y-2*B2) (2*z-2*C2) (-2*d*(c^2)+2*T2*(c^2));...
        (2*x-2*A3) (2*y-2*B3) (2*z-2*C3) (-2*d*(c^2)+2*T3*(c^2));...
        (2*x-2*A4) (2*y-2*B4) (2*z-2*C4) (-2*d*(c^2)+2*T4*(c^2))];
    
    f=[((x-A1)^2)+((y-B1)^2)+((z-C1)^2)-((c^2)*((T1-d)^2));...
       ((x-A2)^2)+((y-B2)^2)+((z-C2)^2)-((c^2)*((T2-d)^2));...
       ((x-A3)^2)+((y-B3)^2)+((z-C3)^2)-((c^2)*((T3-d)^2));...
       ((x-A4)^2)+((y-B4)^2)+((z-C4)^2)-((c^2)*((T4-d)^2))];
    
    g=-Df\f;
    x0=[x;y;z;d];
    x1=x0+g;
    x=x1(1);
    y=x1(2);
    z=x1(3);
    d=x1(4);
    index = index+1;
end
x = x1(1)
y = x1(2)
z = x1(3)
d = x1(4)