% Assign Variables
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;

% Speed of Light
c = 299792.458;

% Initialize Matrix's
Ux = zeros(3,1);
Uy = zeros(3,1);
Uz = zeros(3,1);

w = zeros(3,1);

D = zeros(3,1);

% Store Values for referencing
A = [A1;A2;A3;A4];

B = [B1;B2;B3;B4];

C = [C1;C2;C3;C4];

t = [t1;t2;t3;t4];


% Create x,y,z,d, w Vectors

for a=1:1:3
    Ux(a) = 2*A(a+1) - 2*A1;
end



for a=1:1:3
    Uy(a) = 2*B(a+1) - 2*B1;
end



for a=1:1:3
    Uz(a) = 2*C(a+1) - 2*C1;
end



for a=1:1:3
    w(a) = (A(1)^2 - ...
        A(a+1)^2 + B(1)^2 - ...
        B(a+1)^2 + C(1)^2 - ...
        C(a+1)^2 + (c^2*t(a+1)^2) - ...
        (c^2*t(1)^2));
end



for a=1:1:3
    D(a) = 2*(c^2*(t(1)-t(a+1)));
end


% Solve for x in terms of D
XColumn = [Uy Uz Ux];
DColumn = [Uy Uz D];
wColumn = [Uy Uz w];

% Determinant of X, d, and w
xDeterminant=det(XColumn);
Ddet = det(DColumn);
wDet = det(wColumn);

% Declare symbolic variable
syms d

% Equation for x
x = (-d*Ddet  - wDet) / xDeterminant;

% Shufflie the columns to solve for y
YColumn = [Ux Uz Uy];
D2Column = [Ux Uz D];
W2Column = [Ux Uz w];

yDeterminant = det(YColumn);
D2det = det(D2Column);
W2det = det(W2Column);

% Equation for y
y  = (-d*D2det - W2det) / yDeterminant;

% Shufflie the columns to solve for z
ZColumn = [Ux Uy Uz];
D3Column = [Ux Uy D];
W3Column = [Ux Uy w];


zDeterminant = det(ZColumn);
D3det = det(D3Column);
W3det = det(W3Column);

% Equation for z
z = (-d*D3det - W3det) / zDeterminant;

% Plug x, y, and z into the equation and solve
fun = ((x)-A1)^2 + ((y)-B1)^2 + ((z)-C1)^2 - (c*(t1-d))^2;

% Roots of the equation
roots=double(solve(fun));
r1=roots(1);r2=roots(2);

% Plug both roots into x  y and z
x1 = double(subs(x,d,r1))
y1 = double(subs(y,d,r1))
z1 = double(subs(z,d,r1))

x2 = double(subs(x,d,r2))
y2 = double(subs(y,d,r2))
z2 = double(subs(z,d,r2))

r2

% See the back error of the calculations
check  = (x2-A1)^2 + (y2-B1)^2 + (z2-C1)^2 - (c*(t1-r2))^2

t = -3:.01:3;
fun2 = subs(fun,d,t);
plot(t,fun2);
moreFun = char(vpa(fun));
title('Plot of Row 1 in Figure 1')
xlabel('Time (s)');
ylabel('Values of d')
grid on

x1 =

  -39.7478


y1 =

 -134.2741


z1 =

  -9.4136e+03


x2 =

  -41.7727


y2 =

  -16.7892


z2 =

   6.3701e+03


r2 =

   -0.0032


check =

     0

Published with MATLAB® R2013a