%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% gps.m
% GPS solver
% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function U = gps(U1, U2, U3, U4, steps)
  
  % Inputs:
  % U1, U2, U3 = [X; Y; Z]
  % Calls a one-step method such as trapstep.m
  % Example usage: pend([0 10],[pi/2 0],.05,1)
  
  % Initialize Vars
  R = 6370;           % Radius of the earth (in km) 
  U = [0; 0; R; 0];   % Origin
  c = 299792.458;     % Speed of light (in km/sec)
  
  % Find the radius
  R(1) = c*U1(4);
  R(2) = c*U2(4);
  R(3) = c*U3(4);
  R(4) = c*U4(4);
  
  for i = 1:steps
    S1 = norm(U - U1);
    S2 = norm(U - U2);
    S3 = norm(U - U3);
    S4 = norm(U - U4);
    
    Df = [(U(1) - U1(1))/S1 (U(2) - U1(2))/S1 (U(3) - U1(3))/S1 c ;
          (U(1) - U2(1))/S2 (U(2) - U2(2))/S2 (U(3) - U2(3))/S2 c ;
          (U(1) - U3(1))/S3 (U(2) - U3(2))/S3 (U(3) - U3(3))/S3 c ;
          (U(1) - U4(1))/S4 (U(2) - U4(2))/S4 (U(3) - U4(3))/S4 c ];
      
    f = [S1 - R(1); S2 - R(2); S3 - R(3); S4 - R(4)];
    U = U - (Df \ f);
  end
  
end % end function gps