%%% Step 4 %%%
format long
c=299792.458;   %speed of light

% satellite positions in km and travel times in seconds
for i=1:4
    rho=26570;
    phi=[pi/8, 2*pi/8, 3*pi/8, 4*pi/8];
    theta=[pi/2, pi, 3*pi/2, 2*pi];
    
    A(i)=rho*cos(phi(i))*cos(theta(i));
    B(i)=rho*cos(phi(i))*sin(theta(i));
    C(i)=rho*sin(phi(i));
    
    R(i)=sqrt(A(i)^2+B(i)^2+(C(i)-6370)^2);
    t0(i)=0.0001+R(i)/c;
end

% changes in the transmission time (+/- 10^-8)
dt=10^(-8);
t1=t0;

t2=[t0(1)-dt, t0(2)+dt, t0(3)+dt, t0(4)+dt];
t3=[t0(1)+dt, t0(2)-dt, t0(3)+dt, t0(4)+dt];
t4=[t0(1)+dt, t0(2)+dt, t0(3)-dt, t0(4)+dt];
t5=[t0(1)+dt, t0(2)+dt, t0(3)+dt, t0(4)-dt];

t6=[t0(1)-dt, t0(2)-dt, t0(3)+dt, t0(4)+dt];
t7=[t0(1)+dt, t0(2)+dt, t0(3)-dt, t0(4)-dt];
t8=[t0(1)-dt, t0(2)+dt, t0(3)+dt, t0(4)-dt];
t9=[t0(1)+dt, t0(2)-dt, t0(3)-dt, t0(4)+dt];
t10=[t0(1)-dt, t0(2)+dt, t0(3)-dt, t0(4)+dt];
t11=[t0(1)+dt, t0(2)-dt, t0(3)+dt, t0(4)-dt];

t12=[t0(1)+dt, t0(2)-dt, t0(3)-dt, t0(4)-dt];
t13=[t0(1)-dt, t0(2)+dt, t0(3)-dt, t0(4)-dt];
t14=[t0(1)-dt, t0(2)-dt, t0(3)+dt, t0(4)-dt];
t15=[t0(1)-dt, t0(2)-dt, t0(3)-dt, t0(4)+dt];

T=[t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t12, t13, t14, t15];

% initial vector
u=[0; 0; 6370; 0.0001];
x(1)=u(1);  y(1)=u(2);  z(1)=u(3);  d(1)=u(4);

% iteration
for j=1:15
    t=T(4*(j-1)+1:4*j);
    n=20;
    for i=1:n
        % functions of (x,y,z,d), from (4.38)
        f1=(x(i)-A(1))^2+(y(i)-B(1))^2+(z(i)-C(1))^2-(c*(t(1)-d(i)))^2;
        f2=(x(i)-A(2))^2+(y(i)-B(2))^2+(z(i)-C(2))^2-(c*(t(2)-d(i)))^2;
        f3=(x(i)-A(3))^2+(y(i)-B(3))^2+(z(i)-C(3))^2-(c*(t(3)-d(i)))^2;
        f4=(x(i)-A(4))^2+(y(i)-B(4))^2+(z(i)-C(4))^2-(c*(t(4)-d(i)))^2;
        F=[f1; f2; f3; f4];
        
        % Jacobian Matrix
        df1x=2*(x(i)-A(1));   df1y=2*(y(i)-B(1));   df1z=2*(z(i)-C(1));   df1d=2*c^2*(t(1)-d(i));
        df2x=2*(x(i)-A(2));   df2y=2*(y(i)-B(2));   df2z=2*(z(i)-C(2));   df2d=2*c^2*(t(2)-d(i));
        df3x=2*(x(i)-A(3));   df3y=2*(y(i)-B(3));   df3z=2*(z(i)-C(3));   df3d=2*c^2*(t(3)-d(i));
        df4x=2*(x(i)-A(4));   df4y=2*(y(i)-B(4));   df4z=2*(z(i)-C(4));   df4d=2*c^2*(t(4)-d(i));
        DF=[df1x,df1y,df1z,df1d; df2x,df2y,df2z,df2d; df3x,df3y,df3z,df3d; df4x,df4y,df4z,df4d];
        
        % new u(x,y,z,d) vector
        v=DF\(-F);
        u=u+v;
        x(i+1)=u(1);   y(i+1)=u(2);   z(i+1)=u(3);   d(i+1)=u(4);
    end
    u1(4*(j-1)+1:4*j)=u;
    du=u'-u1(1:4);
    
    % error magnification factor
    normdt = c*norm(dt, inf);   %the backward error (or input error)
    normdu = norm(du, inf);   %the forward error (or output error)
    emf(j) = normdu/normdt;
end
new_u=u'
emf
maxemf = max(emf)