%Reality Check # 4. Problem 1.

%Establishing the Multivariate Newton's Method
function f = multiNewton(x,y,z,d)

% Current Locaitons 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;

% Initial guess
x0 = [x; y; z; d];

%Positions of the 4 satellites
F1 = (x - A1)^2 + (y - B1)^2 + (z - C1)^2 - (c * (t1 - d))^2;
F2 = (x - A2)^2 + (y - B2)^2 + (z - C2)^2 - (c * (t2 - d))^2;
F3 = (x - A3)^2 + (y - B3)^2 + (z - C3)^2 - (c * (t3 - d))^2;
F4 = (x - A4)^2 + (y - B4)^2 + (z - C4)^2 - (c * (t4 - d))^2;

%Partial Derivatives of each satellites

DF1x = 2 * (x - A1);
DF1y = 2 * (y - B1);
DF1z = 2 * (z - C1);
DF1d = 2 * c^2 * (t1 - d);

DF2x = 2 * (x - A2);
DF2y = 2 * (y - B2);
DF2z = 2 * (z - C2);
DF2d = 2 * c^2 * (t2 - d);

DF3x = 2 * (x - A3);
DF3y = 2 * (y - B3);
DF3z = 2 * (z - C3);
DF3d = 2 * c^2 * (t3 - d);

DF4x = 2 * (x - A4);
DF4y = 2 * (y - B4);
DF4z = 2 * (z - C4);
DF4d = 2 * c^2 * (t4 - d);

%DF - The Jacobian Matrix
DF = [DF1x DF1y DF1z DF1d; DF2x DF2y DF2z DF2d; DF3x DF3y DF3z DF3d; DF4x DF4y DF4z DF4d];

% The F matrix contains the four sphere equations
F = [F1; F2; F3; F4];

%One step of iteration.
s =  DF \ -F;
f = x0 + s;

end