function g = gpsQuadratic(d)

format long;

% (x-A1)^2 +(y-B1)^2 +(z-C1)^2 =[c(t1-d)]^2
% (x-A2)^2 +(y-B2)^2 +(z-C2)^2 =[c(t2-d)]^2
% (x-A3)^2 +(y-B3)^2 +(z-C3)^2 =[c(t3-d)]^2
% (x-A4)^2 +(y-B4)^2 +(z-C4)^2 =[c(t4-d)]^2
% vector = x={1;2;3;4;5} = verticle vector

%Speed of light in km/s
c=299792.458;

%Given satellite positions
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;


% f1 = (x-A1)^2 +(y-B1)^2 +(z-C1)^2 -(c*t1-c*d)^2
% f2 = (x-A2)^2 +(y-B2)^2 +(z-C2)^2 -(c*t2-c*d)^2
% f3 = (x-A3)^2 +(y-B3)^2 +(z-C3)^2 -(c*t3-c*d)^2
% f4 = (x-A4)^2 +(y-B4)^2 +(z-C4)^2 -(c*t4-c*d)^2
% mf = x*(2*A2-2*A1)+(A1^2 - A2^2) + y*(2*B2-2*B1)+(B1^2 - B2^2) + z*(2*C2-2*C1)+(C1^2 - C2^2) + (c^2*t2^2 - c^2*t1^2) + d(2*c^2*t1-2*c^2*t2);
% ms = x*(2*A3-2*A1)+(A1^2 - A3^2) + y*(2*B3-2*B1)+(B1^2 - B3^2) + z*(2*C3-2*C1)+(C1^2 - C3^2) + (c^2*t3^2 - c^2*t1^2) + d(2*c^2*t1-2*c^2*t3);
% mt = x*(2*A4-2*A1)+(A1^2 - A4^2) + y*(2*B4-2*B1)+(B1^2 - B4^2) + z*(2*C4-2*C1)+(C1^2 - C4^2) + (c^2*t4^2 - c^2*t1^2) + d(2*c^2*t1-2*c^2*t4);

%name each row of the matrix f1, f2 and f3
%making each value as f1x, f1y, f1z and f1d (which is r13)
%so r = f1x ; f1y ; f1z ; r13 ; r15
%r15 being the parts of the equation that do not have unknowns
%f1 = x*(2*A2 -2*A1) + y*(2*B2 - 2*B1) + z*(2*C2 - 2*C1) + d*(2*c^2(t1-t2)) + A1^2 - A2^2 + B1^2 - B2^2 + C1^2 - C2^2 + c^2*(-t1^2) + c^2*t2^2
f1x = 2*A2-2*A1;
f1y = 2*B2-2*B1;
f1z = 2*C2-2*C1;
f1d = 2*c^2*(t1-t2); %r14
f1r15 = -(A1^2 - A2^2 + B1^2 - B2^2 + C1^2 - C2^2 + c^2*(t2^2-t1^2));

%f2 = x*(2*A3 - 2*A1) + y*(2*B3 - 2*B1) + z*(2*C3 - 2*C1) + d*(2*c^2(t1-t3)) + A1^2 - A3^2 + B1^2 - B3^2 + C1^2 - C3^2 + c^2*(-t1^2) + c^2*t3^2
f2x = 2*A3-2*A1;
f2y = 2*B3-2*B1;
f2z = 2*C3-2*C1;
f2d = 2*c^2*(t1-t3); %r24
f2r25 = -(A1^2 - A3^2 + B1^2 - B3^2 + C1^2 - C3^2 + c^2*(t3^2-t1^2));

%f3 = x*(2*A4 - 2*A1) + y*(2*B4 - 2*B1) + z*(2*C4 - 2*C1) + d*(2*c^2(t1-t4)) + A1^2 - A4^2 + B1^2 - B4^2 + C1^2 - C4^2 + c^2*(-t1^2) + c^2*t4^2
f3x = 2*A4-2*A1;
f3y = 2*B4-2*B1;
f3z = 2*C4-2*C1;
f3d = 2*c^2*(t1-t4); %34
f3r35 = -(A1^2 - A4^2 + B1^2 - B4^2 + C1^2 - C4^2 + c^2*(t4^2-t1^2));

%matrix r
initialMatrix = [f1x, f1y, f1z, f1d, f1r15 ; f2x, f2y, f2z, f2d, f2r25 ; f3x, f3y, f3z, f3d, f3r35];
r = rref(initialMatrix);
r14 = r(1,4);
r15 = r(1,5);
r24 = r(2,4);
r25 = r(2,5);
r34 = r(3,4);
r35 = r(3,5);
x = @(d) -r14*d + r15;
y = @(d) -r24*d + r25;
z = @(d) -r34*d + r35;


%substitute into the first equation f = (x-A1)^2 +(y-B1)^2 +(z-C1)^2 - (c(t1-d))^2
f = @(d) (x(d) - A1)^2 + (y(d) - B1)^2 + (z(d) - C1)^2 - (c*(t1 - d))^2;

%Quadratic formula variables
qa = (r14^2 +r24^2 + r34^2 - c^2);
qb = (2*A1*r14 - 2*r14*r15 + 2*B1*r24 - 2*r24*r25 + 2*C1*r34 - 2*r34*r35 + 2*c^2*t1);
qc = (r15^2 - 2*A1*r15 + A1^2 + r25^2 - 2*B1*r25 + B1^2 + r35^2 - 2*C1*r35 + C1^2 - c^2*t1^2);

% Solve part 1
d1 = (-qb + sqrt(qb^2 - 4*qa*qc))/(2*qa)
x1 = x(d1)
y1 = y(d1)
z1 = z(d1)
% Solve part 2
d2 = (-qb - sqrt(qb^2 - 4*qa*qc))/(2*qa)
x2 = x(d2)
y2 = y(d2)
z2 = z(d2)

end

d1 =
  -0.003201565829594
x1 =
 -41.772709570835708
y1 =
 -16.789194106525994
z1 =
     6.370059559223344e+03
d2 =
   0.185173047095946
x2 =
 -39.747837348164673
y2 =
    -1.342741443606669e+02
z2 =
    -9.413624553735766e+03

Published with MATLAB® R2017a