format long
rho = 42256;
d=0.0001;
sat = 10;
c = 299792.458;
p = zeros(sat,1);
o = zeros(sat,1);
A = zeros(sat,1);
B = zeros(sat,1);
C = zeros(sat,1);
R = zeros(sat,1);
t = zeros(sat,1);
pm= ones(sat,1);
emfMAT = [];
for a1=-1:2:1;
    pm(1)=a1;
    for a2=-1:2:1;
        pm(2)=a2;
        for a3=-1:2:1;
            pm(3)=a3;
            for a4=-1:2:1;
                pm(4)=a4;
                for a5=-1:2:1;
                    pm(5)=a5;
                    for a6=-1:2:1;
                        pm(6)=a6;
                        for a7=-1:2:1;
                            pm(7)=a7;
                            for a8=-1:2:1;
                                pm(8)=a8;
                                for a9=-1:2:1;
                                    pm(9)=a9;
                                    for a10=-1:2:1;
                                        pm(10)=a10;
                                            for i= 1:sat;
                                                p(i) = (i-1)*pi/(2*sat);
                                                o(i) = (i-1)*pi/(sat/2);
                                                A(i) = rho*cos(p(i))*cos(o(i));
                                                B(i) = rho*cos(p(i))*sin(o(i));
                                                C(i) = rho*sin(p(i));
                                                R(i) = sqrt(A(i)^2+B(i)^2+(C(i)-6370)^2);
                                                t(i) = d +R(i)/c+(pm(i))*10^-8;
                                            end
                                        F = zeros(sat,1);
                                        DF = zeros(sat,4);
                                        n=40;
                                            x0=[0; 0; 0; 0];
                                            for j=1:n;
                                                x = x0(1);
                                                y = x0(2);
                                                z = x0(3);
                                                d = x0(4);
                                                for i = 1:sat;
                                                    F(i) = (x-A(i))^2+(y-B(i))^2+(z-C(i))^2-[c*(t(i)-d)]^2;
                                                    DF(i,1) = 2*(x-A(i));
                                                    DF(i,2) = 2*(y-B(i));
                                                    DF(i,3) = 2*(z-C(i));
                                                    DF(i,4) = 2*c^2*(t(i)-d);
                                                end
                                                v=(DF'*DF)\(-1*DF'*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);
                                            emfMAT = [emfMAT emf];
                                    end
                                end
                            end
                        end
                    end
                end
            end
        end
    end
end
maxEMF = max(abs(emfMAT))     