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

% satellite positions in km and travel times in seconds
for i=1:7
    rho=26570;
    phi=[2*pi/16, 3*pi/16, 4*pi/16, 5*pi/16, 6*pi/16, 7*pi/16, 8*pi/16];
    theta=[2*pi/4, 3*pi/4, pi, 5*pi/4, 6*pi/4, 7*pi/4, 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, t0(5)+dt, t0(6)-dt, t0(7)-dt];
t3=[t0(1)+dt, t0(2)-dt, t0(3)+dt, t0(4)+dt, t0(5)+dt, t0(6)-dt, t0(7)-dt];
t4=[t0(1)+dt, t0(2)+dt, t0(3)-dt, t0(4)+dt, t0(5)+dt, t0(6)-dt, t0(7)-dt];
t5=[t0(1)+dt, t0(2)+dt, t0(3)+dt, t0(4)-dt, t0(5)+dt, t0(6)-dt, t0(7)-dt];

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

t12=[t0(1)+dt, t0(2)-dt, t0(3)-dt, t0(4)-dt, t0(5)-dt, t0(6)-dt, t0(7)+dt];
t13=[t0(1)-dt, t0(2)+dt, t0(3)-dt, t0(4)-dt, t0(5)-dt, t0(6)-dt, t0(7)+dt];
t14=[t0(1)-dt, t0(2)-dt, t0(3)+dt, t0(4)-dt, t0(5)-dt, t0(6)-dt, t0(7)+dt];
t15=[t0(1)-dt, t0(2)-dt, t0(3)-dt, t0(4)+dt, t0(5)-dt, t0(6)-dt, t0(7)+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(7*(j-1)+1:7*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;
        f5=(x(i)-A(5))^2+(y(i)-B(5))^2+(z(i)-C(5))^2-(c*(t(5)-d(i)))^2;
        f6=(x(i)-A(6))^2+(y(i)-B(6))^2+(z(i)-C(6))^2-(c*(t(6)-d(i)))^2;
        f7=(x(i)-A(7))^2+(y(i)-B(7))^2+(z(i)-C(7))^2-(c*(t(7)-d(i)))^2;
        F=[f1; f2; f3; f4; f5; f6; f7];
        
        %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));
        df5x=2*(x(i)-A(5));   df5y=2*(y(i)-B(5));   df5z=2*(z(i)-C(5));   df5d=2*c^2*(t(5)-d(i));
        df6x=2*(x(i)-A(6));   df6y=2*(y(i)-B(6));   df6z=2*(z(i)-C(6));   df6d=2*c^2*(t(6)-d(i));
        df7x=2*(x(i)-A(7));   df7y=2*(y(i)-B(7));   df7z=2*(z(i)-C(7));   df7d=2*c^2*(t(7)-d(i));
        DF=[df1x,df1y,df1z,df1d; df2x,df2y,df2z,df2d; df3x,df3y,df3z,df3d; df4x,df4y,df4z,df4d; df5x,df5y,df5z,df5d; df6x,df6y,df6z,df6d; df7x,df7y,df7z,df7d];
        
        %new u(x,y,z,d) vector
        v = DF'*DF\(-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)