%%% Step 2 %%%
format long
c=299792.458;   %speed of light

% satellite positions in km
S1=[15600,7540,20140];
S2=[18760,2750,18610];
S3=[17610,14630,13480];
S4=[19170,610,18390];

A1=S1(1); B1=S1(2); C1=S1(3);
A2=S2(1); B2=S2(2); C2=S2(3);
A3=S3(1); B3=S3(2); C3=S3(3);
A4=S4(1); B4=S4(2); C4=S4(3);

% time intervals in seconds
t=[0.07074,0.07220,0.07690,0.07242];
t1=t(1);  t2=t(2);  t3=t(3);  t4=t(4);

% Three linear equations in the four unknowns u = [x; y; z; d].
% In matrix form, Au = b where A is a 3 x 4 matrix.
A=[2*(A2-A1), 2*(B2-B1), 2*(C2-C1), 2*(c^2)*(t1-t2);
   2*(A3-A1), 2*(B3-B1), 2*(C3-C1), 2*(c^2)*(t1-t3);
   2*(A4-A1), 2*(B4-B1), 2*(C4-C1), 2*(c^2)*(t1-t4)];

b=[-A1^2+A2^2-B1^2+B2^2-C1^2+C2^2+c^2*t1^2-c^2*t2^2;
   -A1^2+A3^2-B1^2+B3^2-C1^2+C3^2+c^2*t1^2-c^2*t3^2;
   -A1^2+A4^2-B1^2+B4^2-C1^2+C4^2+c^2*t1^2-c^2*t4^2];

% Apply the "rref" command to the augmented matrix [A|b].
Ab=[A b];
R=rref(Ab);

r14=R(1,4); r15=R(1,5); 
r24=R(2,4); r25=R(2,5);
r34=R(3,4); r35=R(3,5);

% a quadratic equation in one variable d.
A = r14^2 +r24^2 +r34^2 -c^2;
B = 2*A1*r14 -2*r14*r15 +2*B1*r24 -2*r24*r25 +2*C1*r34 -2*r34*r35 +2*c^2*t1; 
C = A1^2 +r15^2 -2*A1*r15 +B1^2 +r25^2 -2*B1*r25 +C1^2 +r35^2 -2*C1*r35 -c^2*t1^2;

% quadratic formula
d1 = (-B + sqrt(B^2 - 4*A*C)) / (2*A)
d2 = (-B - sqrt(B^2 - 4*A*C)) / (2*A)

% check
u1=[-r14*d1+r15, -r24*d1+r25, -r34*d1+r35, d1]
u2=[-r14*d2+r15, -r24*d2+r25, -r34*d2+r35, d2]