% Reality Check # 4: Problem 2

% Current Locations of each Satellite.

A1 = 15600;
B1 = 7540;
C1 = 20140;
t1 = 0.07074;

A2 = 18760;
B2 = 2750;
C2 = 18610;
t2 = 0.07220;

A3 = 17610;
B3 = 14630;
C3 = 13480;
t3 = 0.07690;

A4 = 19170;
B4 = 610;
C4 = 18390;
t4 = 0.07242;

%'c' = Speed of Light.

c = 299792.458;


% Subtracting the last three equationfrom the first equation yields three 
% linear equations in the four unknowns x*Ux + y*Uy + z*Uz + d*Ud + W = 0

Ux = -2 *[A1 - A2; A1 - A3; A1 - A4];
Uy = -2 *[B1 - B2; B1 - B3; B1 - B4];
Uz = -2 *[C1 - C2; C1 - C3; C1 - C4];
Ud = 2 *(c^2)*[t1 - t2; t1 - t3; t1 - t4];
W = [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)];

% A formula for x in terms of d can be obtained from 
% 0 = det[Uy | Uz | x*Ux + y*Uy + z*Uz + d*Ud + W], noting that the 
% determinant is linear in its columns and that a matrix with a repeated 
% column has determinant zero, then 
% x = (d*det[Uy|Uz|Ud]+det[Uy|Uz|W])/det[Uy|Uz|Ux]
% The same method is applied to obtain y and z:
% y = (d*det[Ux|Uz|Ud]+det[Ux|Uz|W])/det[Uy|Uz|Ux]
% z = (d*det[Uy|Ux|Ud]+det[Uy|Ux|W])/det[Uy|Uz|Ux]
% in terms of d. 

Dx1 = det([Uy Uz Ux]);
Dx2 = det([Uy Uz Ud]);
Dx3 = det([Uy Uz W]);

Dy1 = Dx1;
Dy2 = det([Uz Ux Ud]);
Dy3 = det([Uz Ux W]);

Dz1 = Dx1;
Dz2 = det([Uy Ux Ud]);
Dz3 = det([Uy Ux W]);

x1 = -Dx2/Dx1;
x2 = -Dx3/Dx1;

y1 = -Dy2/Dy1;
y2 = -Dy3/Dy1;

z1 = Dz2/Dz1;
z2 = Dz3/Dz1;

% Quadratic formula

A = (x1^2 + y1^2 + z1^2 - c^2);
B = 2*((x1*x2 - x1*A1) + (y1*y2 - y1*B2) + (z1*z2 - z1*C1)+ t1*c^2);
C = (x2^2 - 2*A1*x2 + A1^2) + (y2^2 -2*B1*y2 + B1^2) + (z2^2 - 2*C1*z2 + C1^2) - (c^2*t1^2);
d1 = (-B + sqrt(B^2 - 4*A*C))/(2*A);
d2 = (-B - sqrt(B^2 - 4*A*C))/(2*A);

% First root
x = x1*d1 + x2;
y = y1*d1 + y2;
z = z1*d1 + z2;

% Second root
xx = x1*d2 + x2;
yy = y1*d2 + y2;
zz = z1*d2 + z2;