A1 = 15600;
B1 = 7540;
C1 = 20140;
A2 = 18760;
B2 = 2750;
C2 = 18610;
A3 = 17610;
B3 = 14630;
C3 = 13480;
A4 = 19170;
B4 = 610;
C4 = 18390;
t1 = 0.07074;
t2 = 0.07220;
t3 = 0.07690;
t4 = 0.07242;
c = 299792.458;  %Satellite positions, times, speed of light (all given constants).

%Subtracted equations from 4.38
%(-2*A1+2*A2)*x + (-2*B1+2*B2)*y + (-2*C1+2*C2)*z + 
%(2*(c^2)*t1 + -2*(c^2)*t2)*d + 
%(A1^2)-(A2^2)+(B1^2)-(B2^2)+(C1^2)-(C2^2)-(c^2)*(t1^2)+(c^2)*(t2^2) = 0

%Do the same for 3rd and 4th equations to get vectors u_x,u_y,u_z,u_d,w
%u_x = [-2*A1+2*A2;-2*A1+2*A3;-2*A1+2*A4]
%u_y = [-2*B1+2*B2;-2*B1+2*B3;-2*B1+2*B4]
%u_z = [-2*C1+2*C2;-2*C1+2*C3;-2*C1+2*C4]
%u_d = [2*(c^2)*t1 + -2*(c^2)*t2; 2*(c^2)*t1 + -2*(c^2)*t3; 
        %2*(c^2)*t1 + -2*(c^2)*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)]

    
G_x = [-2*B1+2*B2 -2*C1+2*C2 -2*A1+2*A2; -2*B1+2*B3 -2*C1+2*C3 -2*A1+2*A3; 
    -2*B1+2*B4 -2*C1+2*C4 -2*A1+2*A4];
%matrix [u_y|u_z|u_x]
det(G_x);
%determinant of that matrix

H_x = [-2*B1+2*B2 -2*C1+2*C2 2*(c^2)*t1+(-2*(c^2)*t2);... 
    -2*B1+2*B3 -2*C1+2*C3 2*(c^2)*t1+(-2*(c^2)*t3);
    -2*B1+2*B4 -2*C1+2*C4 2*(c^2)*t1+(-2*(c^2)*t4)];
%matrix [u_y|u_z|u_d]
det(H_x);
%determinant of that matrix

W_x = [-2*B1+2*B2, -2*C1+2*C2, (A1^2)-(A2^2)+(B1^2)-(B2^2)+(C1^2)-(C2^2)-(c^2)*(t1^2)+(c^2)*(t2^2);
    -2*B1+2*B3, -2*C1+2*C3, (A1^2)-(A3^2)+(B1^2)-(B3^2)+(C1^2)-(C3^2)-(c^2)*(t1^2)+(c^2)*(t3^2);
    -2*B1+2*B4, -2*C1+2*C4, (A1^2)-(A4^2)+(B1^2)-(B4^2)+(C1^2)-(C4^2)-(c^2)*(t1^2)+(c^2)*(t4^2)]; 
%matrix [u_y|u_z|w]
det(W_x);
%determinant of that matrix

G_y = [-2*A1+2*A2 -2*C1+2*C2 -2*B1+2*B2;-2*A1+2*A3 -2*C1+2*C3 -2*B1+2*B3; 
    -2*A1+2*A4 -2*C1+2*C4 -2*B1+2*B4];
%matrix [u_x|u_z|u_y]
det(G_y);
%determinant of that matrix

H_y = [-2*A1+2*A2 -2*C1+2*C2 2*(c^2)*t1+(-2*(c^2)*t2); 
    -2*A1+2*A3 -2*C1+2*C3 2*(c^2)*t1+(-2*(c^2)*t3);
    -2*A1+2*A4 -2*C1+2*C4 2*(c^2)*t1+(-2*(c^2)*t4)];
%matrix [u_x|u_z|u_d]
det(H_y);
%determinant of that matrix

W_y = [-2*A1+2*A2, -2*C1+2*C2, (A1^2)-(A2^2)+(B1^2)-(B2^2)+(C1^2)-(C2^2)-(c^2)*(t1^2)+(c^2)*(t2^2);
    -2*A1+2*A3, -2*C1+2*C3, (A1^2)-(A3^2)+(B1^2)-(B3^2)+(C1^2)-(C3^2)-(c^2)*(t1^2)+(c^2)*(t3^2);
    -2*A1+2*A4, -2*C1+2*C4, (A1^2)-(A4^2)+(B1^2)-(B4^2)+(C1^2)-(C4^2)-(c^2)*(t1^2)+(c^2)*(t4^2)]; 
%matrix [u_x|u_z|w]
det(W_y);
%determinant of that matrix


G_z = [-2*A1+2*A2 -2*B1+2*B2 -2*C1+2*C2;-2*A1+2*A3 -2*B1+2*B3 -2*C1+2*C3; 
    -2*A1+2*A4 -2*B1+2*B4 -2*C1+2*C4];
%matrix [u_x|u_y|u_z]
det(G_z);
%determinant of that matrix

H_z = [-2*A1+2*A2 -2*B1+2*B2 2*(c^2)*t1+(-2*(c^2)*t2); 
    -2*A1+2*A3 -2*B1+2*B3 2*(c^2)*t1+(-2*(c^2)*t3);
    -2*A1+2*A4 -2*B1+2*B4 2*(c^2)*t1+(-2*(c^2)*t4)];
%matix [u_x|u_y|u_d]
det(H_z);
%determinant of that matrix

W_z = [-2*A1+2*A2, -2*B1+2*B2, (A1^2)-(A2^2)+(B1^2)-(B2^2)+(C1^2)-(C2^2)-(c^2)*(t1^2)+(c^2)*(t2^2);
    -2*A1+2*A3, -2*B1+2*B3, (A1^2)-(A3^2)+(B1^2)-(B3^2)+(C1^2)-(C3^2)-(c^2)*(t1^2)+(c^2)*(t3^2);
    -2*A1+2*A4, -2*B1+2*B4, (A1^2)-(A4^2)+(B1^2)-(B4^2)+(C1^2)-(C4^2)-(c^2)*(t1^2)+(c^2)*(t4^2)]; 
%matrix [u_x|u_y|w]
det(W_z);
%determinant of that matrix

%Solved 3 equations:
%x = (-d*det(H_x) - det(W_x)) / det(G_x)
%y = (-d*det(H_y) - det(W_y)) / det(G_y)
%z = (-d*det(H_z) - det(W_z)) / det(G_z)

%Plug these values into any of the equations for r_(1:4) and foil out.
%This gives us a quadratic with d as our variable where there is a
%coefficient for d^2, d, and a constant term.


a = (det(H_x)/det(G_x))^2 + (det(H_y)/det(G_y))^2 + (det(H_z)/det(G_z))^2 - (c^2);
%coefficient for d^2

b = 2*((det(H_x)*det(W_x))/(det(G_x)^2)) + 2*((det(H_y)*det(W_y))/(det(G_y)^2)) + 2*((det(H_z)*det(W_z))/(det(G_z)^2))...
    + 2*A1*(det(H_x)/det(G_x)) + 2*B1*(det(H_y)/det(G_y)) + 2*C1*(det(H_z)/det(G_z)) + (2*(c^2)*t1);
%coefficient for d

m = (det(W_x)/det(G_x))^2 + (det(W_y)/det(G_y))^2 + (det(W_z)/det(G_z))^2 + 2*A1*(det(W_x)/det(G_x))... 
    + 2*B1*(det(W_y)/det(G_y)) + 2*C1*(det(W_z)/det(G_z)) + A1^2 + B1^2 + C1^2 - ((c^2)*(t1^2));
%constant term

R1 = (-b + sqrt((b^2)-(4*a*m)))/(2*a)
%first root from quadratic formula. One solution for d

R2 = (-b - sqrt((b^2)-(4*a*m)))/(2*a)
%second root from quadratic formula. Second solution for d 


x1 = -R1*(det(H_x)/det(G_x)) - (det(W_x)/det(G_x))
%plug in solution for d into the x equation

y1 = -R1*(det(H_y)/det(G_y)) - (det(W_y)/det(G_y))
%plug in solution for d into the y equation

z1 = -R1*(det(H_z)/det(G_z)) - (det(W_z)/det(G_z))
%plug in solution for d into the z equation

x2 = -R2*(det(H_x)/det(G_x)) - (det(W_x)/det(G_x));
%plug in solution for d into the x equation

y2 = -R2*(det(H_y)/det(G_y)) - (det(W_y)/det(G_y));
%plug in solution for d into the y equation

z2 = -R2*(det(H_z)/det(G_z)) - (det(W_z)/det(G_z));
%plug in solution for d into the z equation

%The first root gave the correct x,y,z and d so those will be the values
%displayed