c = 299792.458
A = [15600;18760;17610;19170]
B = [7540;2750;14630;610]
C = [20140;18610;13480;18390]

t = [.07074;.07220;.07690;.07242]

%problem 1

r = @(x,y,z,d)  [((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(1/2)-c*(t(1)-d);
                 ((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(1/2)-c*(t(2)-d);
                 ((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(1/2)-c*(t(3)-d);
                 ((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(1/2)-c*(t(4)-d)];



dr = @(x,y,z,d) [(1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(x-A(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(y-B(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(z-C(1))) c;
                 (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(x-A(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(y-B(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(z-C(2))) c;
                 (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(x-A(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(y-B(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(z-C(3))) c;
                 (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(x-A(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(y-B(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(z-C(4))) c];
r0 = [0;0;6370;0];

for j=1:50
    b = r(r0(1),r0(2),r0(3),r0(4));
    A = dr(r0(1),r0(2),r0(3),r0(4));
    r0 = r0 - A\b;
end
r0

%problem 4

p1 = [26570, pi/4, pi];
s1 = [p1(1)*cos(p1(2))*cos(p1(3));p1(1)*cos(p1(2))*sin(p1(3));p1(1)*sin(p1(3))];
R1 = ((s1(1))^2+(s1(2))^2+(s1(3)-6370)^2)^(1/2);
d=0.0001;
t1= d + R1/c

p2 = [26570, pi/3, pi/2];
s2 = [p2(1)*cos(p2(2))*cos(p2(3));p2(1)*cos(p2(2))*sin(p2(3));p2(1)*sin(p2(3))];
R2 = ((s2(1))^2+(s2(2))^2+(s2(3)-6370)^2)^(1/2);
d=0.0001;
t2= d + R2/c

p3 = [26570, pi/6, 3*pi/2];
s3 = [p3(1)*cos(p3(2))*cos(p3(3));p3(1)*cos(p3(2))*sin(p3(3));p3(1)*sin(p3(3))];
R3 = ((s3(1))^2+(s3(2))^2+(s3(3)-6370)^2)^(1/2);
d=0.0001;
t3= d + R3/c

p4 = [26570, pi/2, 2*pi];
s4 = [p4(1)*cos(p4(2))*cos(p4(3));p4(1)*cos(p4(2))*sin(p4(3));p4(1)*sin(p4(3))];
R4 = ((s4(1))^2+(s4(2))^2+(s4(3)-6370)^2)^(1/2);
d=0.0001;
t4= d + R4/c

A = [s1(1);s2(1);s3(1);s4(1)];
B = [s1(2);s2(2);s3(2);s4(2)];
C = [s1(3);s2(3);s3(3);s4(3)];
t = [t1;t2;t3;t4];

r = @(x,y,z,d)  [((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(1/2)-c*(t(1)-d);
                 ((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(1/2)-c*(t(2)-d);
                 ((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(1/2)-c*(t(3)-d);
                 ((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(1/2)-c*(t(4)-d)];



dr = @(x,y,z,d) [(1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(x-A(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(y-B(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(z-C(1))) c;
                 (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(x-A(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(y-B(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(z-C(2))) c;
                 (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(x-A(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(y-B(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(z-C(3))) c;
                 (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(x-A(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(y-B(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(z-C(4))) c];
r0 = [0;0;6370;0];

for j=1:50
    b = r(r0(1),r0(2),r0(3),r0(4));
    A = dr(r0(1),r0(2),r0(3),r0(4));
    r0 = r0 - A\b;
end
rexp0=r0

% 10^-8 difference

A = [s1(1);s2(1);s3(1);s4(1)];
B = [s1(2);s2(2);s3(2);s4(2)];
C = [s1(3);s2(3);s3(3);s4(3)];
t = [t1+10^-8;t2-10^-8;t3+10^-8;t4-10^-8]

r = @(x,y,z,d)  [((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(1/2)-c*(t(1)-d);
                 ((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(1/2)-c*(t(2)-d);
                 ((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(1/2)-c*(t(3)-d);
                 ((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(1/2)-c*(t(4)-d)];



dr = @(x,y,z,d) [(1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(x-A(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(y-B(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(z-C(1))) c;
                 (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(x-A(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(y-B(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(z-C(2))) c;
                 (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(x-A(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(y-B(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(z-C(3))) c;
                 (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(x-A(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(y-B(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(z-C(4))) c];
rtrue = [0;0;6370;0];
r0=rtrue

for j=1:50
    b = r(r0(1),r0(2),r0(3),r0(4));
    A = dr(r0(1),r0(2),r0(3),r0(4));
    r0 = r0 - A\b;
end
rexp1=r0

error = abs(rexp1 - rtrue)
maxerr = max(error)

EMF = maxerr/(c*(10^-8))
EMF1=EMF

A = [s1(1);s2(1);s3(1);s4(1)];
B = [s1(2);s2(2);s3(2);s4(2)];
C = [s1(3);s2(3);s3(3);s4(3)];
t = [t1-10^-8;t2+10^-8;t3-10^-8;t4+10^-8]

r = @(x,y,z,d)  [((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(1/2)-c*(t(1)-d);
                 ((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(1/2)-c*(t(2)-d);
                 ((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(1/2)-c*(t(3)-d);
                 ((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(1/2)-c*(t(4)-d)];



dr = @(x,y,z,d) [(1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(x-A(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(y-B(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(z-C(1))) c;
                 (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(x-A(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(y-B(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(z-C(2))) c;
                 (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(x-A(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(y-B(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(z-C(3))) c;
                 (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(x-A(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(y-B(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(z-C(4))) c];
r0 = [0;0;6370;0];
rtrue=r0;

for j=1:50
    b = r(r0(1),r0(2),r0(3),r0(4));
    A = dr(r0(1),r0(2),r0(3),r0(4));
    r0 = r0 - A\b;
end
rexp2=r0

error = abs(rexp2 - rtrue)
maxerr = max(error)

EMF = maxerr/(c*(10^-8))
EMF2=EMF



% for t+ 10^-8, if you do +,-,+,-, you get 3.19175982742414(EMF1), for -,+,-,+, you get 3.19175273124487(EMF2). 
%problem 5

p1 = [26570, pi/4 + (.05*pi/4), pi+ (.05*pi)];
s1 = [p1(1)*cos(p1(2))*cos(p1(3));p1(1)*cos(p1(2))*sin(p1(3));p1(1)*sin(p1(3))];
R1 = ((s1(1))^2+(s1(2))^2+(s1(3)-6370)^2)^(1/2);
d=0.0001;
t1= d + R1/c

p2 = [26570, pi/4+ (.04*pi/4), pi+ (.04*pi)];
s2 = [p2(1)*cos(p2(2))*cos(p2(3));p2(1)*cos(p2(2))*sin(p2(3));p2(1)*sin(p2(3))];
R2 = ((s2(1))^2+(s2(2))^2+(s2(3)-6370)^2)^(1/2);
d=0.0001;
t2= d + R2/c

p3 = [26570, pi/4 + (.03*pi/4), pi + (.03*pi)];
s3 = [p3(1)*cos(p3(2))*cos(p3(3));p3(1)*cos(p3(2))*sin(p3(3));p3(1)*sin(p3(3))];
R3 = ((s3(1))^2+(s3(2))^2+(s3(3)-6370)^2)^(1/2);
d=0.0001;
t3= d + R3/c

p4 = [26570, pi/4 + (.02*pi/4), pi + (.02*pi)];
s4 = [p4(1)*cos(p4(2))*cos(p4(3));p4(1)*cos(p4(2))*sin(p4(3));p4(1)*sin(p4(3))];
R4 = ((s4(1))^2+(s4(2))^2+(s4(3)-6370)^2)^(1/2);
d=0.0001;
t4= d + R4/c

%+,-,+,-
A = [s1(1);s2(1);s3(1);s4(1)];
B = [s1(2);s2(2);s3(2);s4(2)];
C = [s1(3);s2(3);s3(3);s4(3)];
t = [t1+10^-8;t2-10^-8;t3+10^-8;t4-10^-8]

r = @(x,y,z,d)  [((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(1/2)-c*(t(1)-d);
                 ((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(1/2)-c*(t(2)-d);
                 ((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(1/2)-c*(t(3)-d);
                 ((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(1/2)-c*(t(4)-d)];



dr = @(x,y,z,d) [(1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(x-A(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(y-B(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(z-C(1))) c;
                 (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(x-A(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(y-B(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(z-C(2))) c;
                 (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(x-A(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(y-B(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(z-C(3))) c;
                 (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(x-A(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(y-B(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(z-C(4))) c];
r0 = [0;0;6370;0];
rtrue=r0;

for j=1:50
    b = r(r0(1),r0(2),r0(3),r0(4));
    A = dr(r0(1),r0(2),r0(3),r0(4));
    r0 = r0 - A\b;
end
rexp3=r0

error = abs(rexp3 - rtrue)
maxerr = max(error)
EMF = maxerr/(c*(10^-8))
EMF3=EMF

%-,+,-,+
A = [s1(1);s2(1);s3(1);s4(1)];
B = [s1(2);s2(2);s3(2);s4(2)];
C = [s1(3);s2(3);s3(3);s4(3)];
t = [t1-10^-8;t2+10^-8;t3-10^-8;t4+10^-8]

r = @(x,y,z,d)  [((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(1/2)-c*(t(1)-d);
                 ((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(1/2)-c*(t(2)-d);
                 ((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(1/2)-c*(t(3)-d);
                 ((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(1/2)-c*(t(4)-d)];



dr = @(x,y,z,d) [(1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(x-A(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(y-B(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(z-C(1))) c;
                 (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(x-A(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(y-B(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(z-C(2))) c;
                 (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(x-A(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(y-B(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(z-C(3))) c;
                 (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(x-A(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(y-B(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(z-C(4))) c];
r0 = [0;0;6370;0];
rtue=r0;

for j=1:50
    b = r(r0(1),r0(2),r0(3),r0(4));
    A = dr(r0(1),r0(2),r0(3),r0(4));
    r0 = r0 - A\b;
end
rexp4=r0

error = abs(rexp4 - rtrue)
maxerr = max(error)
EMF = maxerr/(c*(10^-8))
EMF4=EMF

%When the satellites are within 5 percent of each other, for +,-,+,-, EMF was 3.13866977190565e+005 (EMF3), and for -,+,-,+, EMF was 3.22118820187522e+005 (EMF4)


%problem 6 far sattelites

p1 = [26570, pi/4, pi];
s1 = [p1(1)*cos(p1(2))*cos(p1(3));p1(1)*cos(p1(2))*sin(p1(3));p1(1)*sin(p1(3))];
R1 = ((s1(1))^2+(s1(2))^2+(s1(3)-6370)^2)^(1/2);
d=0.0001;
t1= d + R1/c

p2 = [26570, pi/3, pi/2];
s2 = [p2(1)*cos(p2(2))*cos(p2(3));p2(1)*cos(p2(2))*sin(p2(3));p2(1)*sin(p2(3))];
R2 = ((s2(1))^2+(s2(2))^2+(s2(3)-6370)^2)^(1/2);
d=0.0001;
t2= d + R2/c

p3 = [26570, pi/6, 3*pi/2];
s3 = [p3(1)*cos(p3(2))*cos(p3(3));p3(1)*cos(p3(2))*sin(p3(3));p3(1)*sin(p3(3))];
R3 = ((s3(1))^2+(s3(2))^2+(s3(3)-6370)^2)^(1/2);
d=0.0001;
t3= d + R3/c

p4 = [26570, pi/2, 2*pi];
s4 = [p4(1)*cos(p4(2))*cos(p4(3));p4(1)*cos(p4(2))*sin(p4(3));p4(1)*sin(p4(3))];
R4 = ((s4(1))^2+(s4(2))^2+(s4(3)-6370)^2)^(1/2);
d=0.0001;
t4= d + R4/c

p5 = [26570, pi/5, pi];
s5 = [p5(1)*cos(p5(2))*cos(p5(3));p5(1)*cos(p5(2))*sin(p5(3));p5(1)*sin(p5(3))];
R5 = ((s5(1))^2+(s5(2))^2+(s5(3)-6370)^2)^(1/2);
d=0.0001;
t5= d + R5/c

p6 = [26570, pi/6, pi];
s6 = [p6(1)*cos(p6(2))*cos(p6(3));p6(1)*cos(p6(2))*sin(p6(3));p6(1)*sin(p6(3))];
R6 = ((s6(1))^2+(s6(2))^2+(s6(3)-6370)^2)^(1/2);
d=0.0001;
t6= d + R6/c

p7 = [26570, pi/7, pi];
s7 = [p7(1)*cos(p7(2))*cos(p7(3));p7(1)*cos(p7(2))*sin(p7(3));p7(1)*sin(p7(3))];
R7 = ((s7(1))^2+(s7(2))^2+(s7(3)-6370)^2)^(1/2);
d=0.0001;
t7= d + R7/c

p8 = [26570, pi/8, pi];
s8 = [p8(1)*cos(p8(2))*cos(p8(3));p8(1)*cos(p8(2))*sin(p8(3));p8(1)*sin(p8(3))];
R8 = ((s8(1))^2+(s8(2))^2+(s8(3)-6370)^2)^(1/2);
d=0.0001;
t8= d + R8/c

A = [s1(1);s2(1);s3(1);s4(1);s5(1);s6(1);s7(1);s8(1)];
B = [s1(2);s2(2);s3(2);s4(2);s5(2);s6(2);s7(2);s8(2)];
C = [s1(3);s2(3);s3(3);s4(3);s5(3);s6(3);s7(3);s8(3)];
t = [t1;t2;t3;t4;t5;t6;t7;t8];

r = @(x,y,z,d)  [((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(1/2)-c*(t(1)-d);
                 ((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(1/2)-c*(t(2)-d);
                 ((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(1/2)-c*(t(3)-d);
                 ((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(1/2)-c*(t(4)-d);
                 ((x-A(5))^2+(y-B(5))^2+(z-C(5))^2)^(1/2)-c*(t(5)-d);
                 ((x-A(6))^2+(y-B(6))^2+(z-C(6))^2)^(1/2)-c*(t(6)-d);
                 ((x-A(7))^2+(y-B(7))^2+(z-C(7))^2)^(1/2)-c*(t(7)-d);
                 ((x-A(8))^2+(y-B(8))^2+(z-C(8))^2)^(1/2)-c*(t(8)-d)];

dr = @(x,y,z,d) [(1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(x-A(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(y-B(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(z-C(1))) c;
                 (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(x-A(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(y-B(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(z-C(2))) c;
                 (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(x-A(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(y-B(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(z-C(3))) c;
                 (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(x-A(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(y-B(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(z-C(4))) c;
                 (1/2)*((x-A(5))^2+(y-B(5))^2+(z-C(5))^2)^(-1/2)*(2*(x-A(5))) (1/2)*((x-A(5))^2+(y-B(5))^2+(z-C(5))^2)^(-1/2)*(2*(y-B(5))) (1/2)*((x-A(5))^2+(y-B(5))^2+(z-C(5))^2)^(-1/2)*(2*(z-C(5))) c;
                 (1/2)*((x-A(6))^2+(y-B(6))^2+(z-C(6))^2)^(-1/2)*(2*(x-A(6))) (1/2)*((x-A(6))^2+(y-B(6))^2+(z-C(6))^2)^(-1/2)*(2*(y-B(6))) (1/2)*((x-A(6))^2+(y-B(6))^2+(z-C(6))^2)^(-1/2)*(2*(z-C(6))) c;
                 (1/2)*((x-A(7))^2+(y-B(7))^2+(z-C(7))^2)^(-1/2)*(2*(x-A(7))) (1/2)*((x-A(7))^2+(y-B(7))^2+(z-C(7))^2)^(-1/2)*(2*(y-B(7))) (1/2)*((x-A(7))^2+(y-B(7))^2+(z-C(7))^2)^(-1/2)*(2*(z-C(7))) c;
                 (1/2)*((x-A(8))^2+(y-B(8))^2+(z-C(8))^2)^(-1/2)*(2*(x-A(8))) (1/2)*((x-A(8))^2+(y-B(8))^2+(z-C(8))^2)^(-1/2)*(2*(y-B(8))) (1/2)*((x-A(8))^2+(y-B(8))^2+(z-C(8))^2)^(-1/2)*(2*(z-C(8))) c];
       
r0 = [10;10;6370;0];
rtrue=[0;0;6370;0];

for j=1:50
    b = r(r0(1),r0(2),r0(3),r0(4));
    A = dr(r0(1),r0(2),r0(3),r0(4));
    r0 = r0 - A\b;
end
rexp5=r0
error = abs(rexp5 - rtrue)
maxerr = max(error)
EMF = maxerr/(c*(10^-8))
EMF5=EMF

%problem 6 close together
p1 = [26570, pi/4, pi];
s1 = [p1(1)*cos(p1(2))*cos(p1(3));p1(1)*cos(p1(2))*sin(p1(3));p1(1)*sin(p1(3))];
R1 = ((s1(1))^2+(s1(2))^2+(s1(3)-6370)^2)^(1/2);
d=0.0001;
t1= d + R1/c

p2 = [26570, pi/4 + (.05*pi/4), pi + (.05*pi)];
s2 = [p2(1)*cos(p2(2))*cos(p2(3));p2(1)*cos(p2(2))*sin(p2(3));p2(1)*sin(p2(3))];
R2 = ((s2(1))^2+(s2(2))^2+(s2(3)-6370)^2)^(1/2);
d=0.0001;
t2= d + R2/c

p3 = [26570, pi/4 + (.045*pi/4), pi + (.045*pi)];
s3 = [p3(1)*cos(p3(2))*cos(p3(3));p3(1)*cos(p3(2))*sin(p3(3));p3(1)*sin(p3(3))];
R3 = ((s3(1))^2+(s3(2))^2+(s3(3)-6370)^2)^(1/2);
d=0.0001;
t3= d + R3/c

p4 = [26570, pi/4 + (.04*pi/4), pi + (.04*pi)];
s4 = [p4(1)*cos(p4(2))*cos(p4(3));p4(1)*cos(p4(2))*sin(p4(3));p4(1)*sin(p4(3))];
R4 = ((s4(1))^2+(s4(2))^2+(s4(3)-6370)^2)^(1/2);
d=0.0001;
t4= d + R4/c

p5 = [26570, pi/4 + (.035*pi/4), pi + (.035*pi)];
s5 = [p5(1)*cos(p5(2))*cos(p5(3));p5(1)*cos(p5(2))*sin(p5(3));p5(1)*sin(p5(3))];
R5 = ((s5(1))^2+(s5(2))^2+(s5(3)-6370)^2)^(1/2);
d=0.0001;
t5= d + R5/c

p6 = [26570, pi/4 + (.03*pi/4), pi + (.03*pi)];
s6 = [p6(1)*cos(p6(2))*cos(p6(3));p6(1)*cos(p6(2))*sin(p6(3));p6(1)*sin(p6(3))];
R6 = ((s6(1))^2+(s6(2))^2+(s6(3)-6370)^2)^(1/2);
d=0.0001;
t6= d + R6/c

p7 = [26570, pi/4 + (.025*pi/4), pi + (.025*pi)];
s7 = [p7(1)*cos(p7(2))*cos(p7(3));p7(1)*cos(p7(2))*sin(p7(3));p7(1)*sin(p7(3))];
R7 = ((s7(1))^2+(s7(2))^2+(s7(3)-6370)^2)^(1/2);
d=0.0001;
t7= d + R7/c

p8 = [26570, pi/4 + (.02*pi/4), pi + (.02*pi)];
s8 = [p8(1)*cos(p8(2))*cos(p8(3));p8(1)*cos(p8(2))*sin(p8(3));p8(1)*sin(p8(3))];
R8 = ((s8(1))^2+(s8(2))^2+(s8(3)-6370)^2)^(1/2);
d=0.0001;
t8= d + R8/c

A = [s1(1);s2(1);s3(1);s4(1);s5(1);s6(1);s7(1);s8(1)];
B = [s1(2);s2(2);s3(2);s4(2);s5(2);s6(2);s7(2);s8(2)];
C = [s1(3);s2(3);s3(3);s4(3);s5(3);s6(3);s7(3);s8(3)];
t = [t1+10^-8;t2-10^-8;t3+10^-8;t4-10^-8;t5+10^-8;t6-10^-8;t7+10^-8;t8-10^-8];

r = @(x,y,z,d)  [((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(1/2)-c*(t(1)-d);
                 ((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(1/2)-c*(t(2)-d);
                 ((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(1/2)-c*(t(3)-d);
                 ((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(1/2)-c*(t(4)-d);
                 ((x-A(5))^2+(y-B(5))^2+(z-C(5))^2)^(1/2)-c*(t(5)-d);
                 ((x-A(6))^2+(y-B(6))^2+(z-C(6))^2)^(1/2)-c*(t(6)-d);
                 ((x-A(7))^2+(y-B(7))^2+(z-C(7))^2)^(1/2)-c*(t(7)-d);
                 ((x-A(8))^2+(y-B(8))^2+(z-C(8))^2)^(1/2)-c*(t(8)-d)];

dr = @(x,y,z,d) [(1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(x-A(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(y-B(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(z-C(1))) c;
                 (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(x-A(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(y-B(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(z-C(2))) c;
                 (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(x-A(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(y-B(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(z-C(3))) c;
                 (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(x-A(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(y-B(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(z-C(4))) c;
                 (1/2)*((x-A(5))^2+(y-B(5))^2+(z-C(5))^2)^(-1/2)*(2*(x-A(5))) (1/2)*((x-A(5))^2+(y-B(5))^2+(z-C(5))^2)^(-1/2)*(2*(y-B(5))) (1/2)*((x-A(5))^2+(y-B(5))^2+(z-C(5))^2)^(-1/2)*(2*(z-C(5))) c;
                 (1/2)*((x-A(6))^2+(y-B(6))^2+(z-C(6))^2)^(-1/2)*(2*(x-A(6))) (1/2)*((x-A(6))^2+(y-B(6))^2+(z-C(6))^2)^(-1/2)*(2*(y-B(6))) (1/2)*((x-A(6))^2+(y-B(6))^2+(z-C(6))^2)^(-1/2)*(2*(z-C(6))) c;
                 (1/2)*((x-A(7))^2+(y-B(7))^2+(z-C(7))^2)^(-1/2)*(2*(x-A(7))) (1/2)*((x-A(7))^2+(y-B(7))^2+(z-C(7))^2)^(-1/2)*(2*(y-B(7))) (1/2)*((x-A(7))^2+(y-B(7))^2+(z-C(7))^2)^(-1/2)*(2*(z-C(7))) c;
                 (1/2)*((x-A(8))^2+(y-B(8))^2+(z-C(8))^2)^(-1/2)*(2*(x-A(8))) (1/2)*((x-A(8))^2+(y-B(8))^2+(z-C(8))^2)^(-1/2)*(2*(y-B(8))) (1/2)*((x-A(8))^2+(y-B(8))^2+(z-C(8))^2)^(-1/2)*(2*(z-C(8))) c];
       
rtrue = [0;0;6370;0];
r0=[10;10;6370;0];

for j=1:50
    b = r(r0(1),r0(2),r0(3),r0(4));
    A = dr(r0(1),r0(2),r0(3),r0(4));
    r0 = r0 - A\b;
end
rexp6=r0

error = abs(rexp6 - rtrue)
maxerr = max(error)
EMF = maxerr/(c*(10^-8))
EMF6=EMF

A = [s1(1);s2(1);s3(1);s4(1);s5(1);s6(1);s7(1);s8(1)];
B = [s1(2);s2(2);s3(2);s4(2);s5(2);s6(2);s7(2);s8(2)];
C = [s1(3);s2(3);s3(3);s4(3);s5(3);s6(3);s7(3);s8(3)];
t = [t1-10^-8;t2+10^-8;t3-10^-8;t4+10^-8;t5-10^-8;t6+10^-8;t7-10^-8;t8+10^-8];

r = @(x,y,z,d)  [((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(1/2)-c*(t(1)-d);
                 ((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(1/2)-c*(t(2)-d);
                 ((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(1/2)-c*(t(3)-d);
                 ((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(1/2)-c*(t(4)-d);
                 ((x-A(5))^2+(y-B(5))^2+(z-C(5))^2)^(1/2)-c*(t(5)-d);
                 ((x-A(6))^2+(y-B(6))^2+(z-C(6))^2)^(1/2)-c*(t(6)-d);
                 ((x-A(7))^2+(y-B(7))^2+(z-C(7))^2)^(1/2)-c*(t(7)-d);
                 ((x-A(8))^2+(y-B(8))^2+(z-C(8))^2)^(1/2)-c*(t(8)-d)];

dr = @(x,y,z,d) [(1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(x-A(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(y-B(1))) (1/2)*((x-A(1))^2+(y-B(1))^2+(z-C(1))^2)^(-1/2)*(2*(z-C(1))) c;
                 (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(x-A(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(y-B(2))) (1/2)*((x-A(2))^2+(y-B(2))^2+(z-C(2))^2)^(-1/2)*(2*(z-C(2))) c;
                 (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(x-A(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(y-B(3))) (1/2)*((x-A(3))^2+(y-B(3))^2+(z-C(3))^2)^(-1/2)*(2*(z-C(3))) c;
                 (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(x-A(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(y-B(4))) (1/2)*((x-A(4))^2+(y-B(4))^2+(z-C(4))^2)^(-1/2)*(2*(z-C(4))) c;
                 (1/2)*((x-A(5))^2+(y-B(5))^2+(z-C(5))^2)^(-1/2)*(2*(x-A(5))) (1/2)*((x-A(5))^2+(y-B(5))^2+(z-C(5))^2)^(-1/2)*(2*(y-B(5))) (1/2)*((x-A(5))^2+(y-B(5))^2+(z-C(5))^2)^(-1/2)*(2*(z-C(5))) c;
                 (1/2)*((x-A(6))^2+(y-B(6))^2+(z-C(6))^2)^(-1/2)*(2*(x-A(6))) (1/2)*((x-A(6))^2+(y-B(6))^2+(z-C(6))^2)^(-1/2)*(2*(y-B(6))) (1/2)*((x-A(6))^2+(y-B(6))^2+(z-C(6))^2)^(-1/2)*(2*(z-C(6))) c;
                 (1/2)*((x-A(7))^2+(y-B(7))^2+(z-C(7))^2)^(-1/2)*(2*(x-A(7))) (1/2)*((x-A(7))^2+(y-B(7))^2+(z-C(7))^2)^(-1/2)*(2*(y-B(7))) (1/2)*((x-A(7))^2+(y-B(7))^2+(z-C(7))^2)^(-1/2)*(2*(z-C(7))) c;
                 (1/2)*((x-A(8))^2+(y-B(8))^2+(z-C(8))^2)^(-1/2)*(2*(x-A(8))) (1/2)*((x-A(8))^2+(y-B(8))^2+(z-C(8))^2)^(-1/2)*(2*(y-B(8))) (1/2)*((x-A(8))^2+(y-B(8))^2+(z-C(8))^2)^(-1/2)*(2*(z-C(8))) c];
       
rtrue = [0;0;6370;0];
r0=[10;10;6370;0];

for j=1:50
    b = r(r0(1),r0(2),r0(3),r0(4));
    A = dr(r0(1),r0(2),r0(3),r0(4));
    r0 = r0 - A\b;
end
rexp7=r0

error = abs(rexp7 - rtrue)
maxerr = max(error)
EMF = maxerr/(c*(10^-8))
EMF7=EMF






