% Step 6 iteration
format long g
c = 299792.458;
% our position
position = [0 0 6370 0.0001];
% initial guess vector
f = [0 0 0 0]';
f2 = [-3 5 8000 .0003]';
f3 = [1000 2000 3000 .01]';
% iteration of gaussnewton method
%   gives very small errors for x, y, and z 
%   but a relatively large error for d
%   20 steps seems to be a good number, might be able to do fewer/more
%       for more accuracy but >200 exhibits rounding errors so our error
%       will be higher
for i=1:20
    f = gaussnewton(f(1),f(2),f(3),f(4));
    f2 = gaussnewton(f2(1),f2(2),f2(3),f2(4));
    f3 = gaussnewton(f3(1),f3(2),f3(3),f3(4));
end

% compare estimated vector with correct vector
err = position'-f
err2 = position'-f2
err3 = position'-f3

emf = max(abs(err))/(c*1e-8)
emf2 = max(abs(err2))/(c*1e-8)
emf3 = max(abs(err3))/(c*1e-8)

maxFEDiff = max(max(abs([err err2 err3])))
maxEMF = max([emf emf2 emf3])