% Solve the system (4.37) by using Multivariate Newtons Method. 
% Find the receiver position (x, y, z) near earth and time correction d. 

%As a check,
% (x, y, z) = (-41.77271,-16.78919,6370.0596) 
% d = -3.201566 ?10^-3 seconds.

c= 299792.458; % speed of light km/sec

%%% simultaneous satellite positions in km
A1= 15600; B1= 7540; C1= 20140;
A2 = 18760; B2= 2750; C2=18610;
A3 = 17610; B3 = 14630; C3= 13480;
A4= 19170; B4=610; C4=18390;

%%% time intervals in seconds
t1= 0.07074;
t2= 0.07220;
t3= 0.07690;
t4= 0.07242;
% initial vector
u= [0;0;6370;0];
x(1)=u(1);  y(1)=u(2);  z(1)=u(3);  d(1)=u(4);
% iteration
for i=1: 10
   
    x(i)= u(1);
    y(i)= u(2) ;
    z(i)= u(3);
    d(i)= u(4);
    
    % functions of (x,y,z,d), from (4.73)
    f1= ( x(i)-A1).^2 + (y(i)-B1).^2 + (z(i)-C1).^2 - (c*(t1 - d(i))).^2;
    f2= ( x(i)-A2).^2 + (y(i)-B2).^2 + (z(i)-C2).^2 - (c*(t2 - d(i))).^2;
    f3= ( x(i)-A3).^2 + (y(i)-B3).^2 + (z(i)-C3).^2 - (c*(t3 - d(i))).^2;
    f4= ( x(i)-A4).^2 + (y(i)-B4).^2 + (z(i)-C4).^2 - (c*(t4 - d(i))).^2; 
    F = [f1;f2;f3;f4];
   
   % Jacobian Matrix
   df1x= (2*(x(i)-A1));
   df1y= (2*(y(i)-B1));
   df1z= (2*(z(i)-C1));
   df1t= (2*c^2*(t1-d(i)));
 
   df2x=2*(x(i)-A2);
   df2y=2*(y(i)-B2);
   df2z=2*(z(i)-C2);
   df2t=2*c^2*(t2-d(i));

   df3x=2*(x(i)-A3);
   df3y=2*(y(i)-B3);
   df3z=2*(z(i)-C3);
   df3t=2*c^2*(t3-d(i));

   df4x=2*(x(i)-A4);
   df4y=2*(y(i)-B4);
   df4z=2*(z(i)-C4);
   df4t=2*c^2*(t4-d(i));

   DF = [df1x, df1y, df1z, df1t; df2x, df2y df2z df2t;df3x df3y df3z df3t;df4x df4y df4z df4t];
   
   % new u(x,y,z,d) vector
    V = DF\(-F);
    u = u+V;

end

u