rho = 42256;
p1 = pi/4; p2 = 1.01*pi/4; p3 = 1.02*pi/4; p4 = 0.99*pi/4;
o1 = pi; o2 = 1.01*pi; o3 = 1.02*pi; o4 = 0.99*pi;

A1 = rho*cos(p1)*cos(o1);
B1 = rho*cos(p1)*sin(o1);
C1 = rho*sin(p1);

A2 = rho*cos(p2)*cos(o2);
B2 = rho*cos(p2)*sin(o2);
C2 = rho*sin(p2);

A3 = rho*cos(p3)*cos(o3);
B3 = rho*cos(p3)*sin(o3);
C3 = rho*sin(p3);

A4 = rho*cos(p4)*cos(o4);
B4 = rho*cos(p4)*sin(o4);
C4 = rho*sin(p4);

R1 = sqrt(A1^2+B1^2+(C1-6370)^2);
R2 = sqrt(A2^2+B2^2+(C2-6370)^2);
R3 = sqrt(A3^2+B3^2+(C3-6370)^2);
R4 = sqrt(A4^2+B4^2+(C4-6370)^2);

d=0.0001;
c = 299792.458;
t1 = d+R1/c+10^-8;
t2 = d+R2/c-10^-8;
t3 = d+R3/c+10^-8;
t4 = d+R4/c-10^-8;

n=20;
    x0=[0; 0; 0; 0];
    for i=1:n;
        x = x0(1);
        y = x0(2);
        z = x0(3);
        d = x0(4);
        F=[(x-A1)^2+(y-B1)^2+(z-C1)^2-[c*(t1-d)]^2;
    (x-A2)^2+(y-B2)^2+(z-C2)^2-[c*(t2-d)]^2;
    (x-A3)^2+(y-B3)^2+(z-C3)^2-[c*(t3-d)]^2;
    (x-A4)^2+(y-B4)^2+(z-C4)^2-[c*(t4-d)]^2;];
        DF = [2*(x-A1) 2*(y-B1) 2*(z-C1) 2*c^2*(t1-d);
    2*(x-A2) 2*(y-B2) 2*(z-C2) 2*c^2*(t2-d);
    2*(x-A3) 2*(y-B3) 2*(z-C3) 2*c^2*(t3-d);
    2*(x-A4) 2*(y-B4) 2*(z-C4) 2*c^2*(t4-d)];
        v=DF\(-1*F);
        x0=x0+v;
    end
    x0
    err=x0-[0; 0; 6370; .0001];
    err=[err(1); err(2); err(3)]
    emf=max(abs(err))/(c*10^-8)
