Sample Code for Project2 Part 2:
%lightspeed
c = 299792.458;
% Satalite Coordinates
A1 = 15600;
B1 = 7540;
C1 = 20140;
A2 = 18760;
B2 = 2750;
C2 = 18610;
A3 = 17610;
B3 = 14630;
C3 = 13480;
A4 = 19170;
B4 = 610;
C4 = 18390;
%Satelite Time
t1 = .07074;
t2 = .07220;
t3 = .07690;
t4 = .07242;
%Set up vectors
ux = [ 2*(A2-A1) 2*(A3-A1) 2*(A4-A1) ]';
uy = [ 2*(B2-B1) 2*(B3-B1) 2*(B4-B1) ]';
uz = [ 2*(C2-C1) 2*(C3-C1) 2*(C4-C1) ]';
ud = [ 2*c^2*(t1-t2) 2*c^2*(t1-t3) 2*c^2*(t1-t4) ]';
w = [A1^2-A2^2+B1^2-B2^2+C1^2-C2^2+c^2*(t2^2-t1^2)...
A1^2-A3^2+B1^2-B3^2+C1^2-C3^2+c^2*(t3^2-t1^2)...
A1^2-A4^2+B1^2-B4^2+C1^2-C4^2+c^2*(t4^2-t1^2)]';
%Combine some terms for easier algebra
Mx = -1*det( [uy uz ud] )/det( [uy uz ux] );
Nx = -1*det( [uy uz w] )/det( [uy uz ux] );
My = -1*det( [ux uz ud] )/det( [ux uz uy] );
Ny = -1*det( [ux uz w] )/det( [ux uz uy] );
Mz = -1*det( [ux uy ud] )/det( [ux uy uz] );
Nz = -1*det( [ux uy w] )/det( [ux uy uz] );
a = Mx^2+My^2+Mz^2-c^2;
b = 1+2*Nx*Mx-2*A1*Mx+2*Ny*My-2*B1*My+2*Nz*Mz-2*C1*Mz+2*t1*c^2;
cO = Nx^2-2*A1*Nx + A1^2 + Ny^2-2*B1*Ny + B1^2 + Nz^2-2*C1*Nz + C1^2 - c^2*t1^2;
%Finding roots and calculate x, y, z
%2 roots: User can choose correct root
disp('First root')
d1 = (-b + sqrt(b^2-4*a*cO))/(2*a)
x1 = d1*Mx + Nx
y1 = d1*My + Ny
z1 = d1*Mz + Nz
disp('Second root');
d2 = (-b - sqrt(b^2-4*a*cO))/(2*a)
x2 = d2*Mx + Nx
y2 = d2*My + Ny
z2 = d2*Mz + Nz
end