c = 299792.458;
d = 0.0001;
delta_t = 10^-8;

init_vector = [0; 0; 6370; 0];
true_solution = [0; 0; 6370; 0.0001];

list_n = [5, 6, 7, 8, 9, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95, 100, 200];
n_value = zeros(length(list_n),1);
cond_value = zeros(length(list_n),1);
position_error = zeros(length(list_n),1);
emf = zeros(length(list_n),1);

for index = 1:length(list_n)
    n = list_n(index);
    h1 = (pi/2) / n;
    h2 = (2*pi) / n;
    p = 26570;
    phi = [0:n-1]*h1;
    theta = [0:n-1]*h2;
	satA = zeros(1,n);
	satB = zeros(1,n);
	satC = zeros(1,n);
	R = zeros(1,n);
	t = zeros(1,n);
	for i=1:n
		satA(i) = p*cos(phi(1,i))*cos(theta(1,i));
		satB(i) = p*cos(phi(1,i))*sin(theta(1,i));
		satC(i) = p*sin(phi(1,i));
		R(i) = sqrt(satA(i)^2 + satB(i)^2 + (satC(i) - 6370)^2);
		t(i) = d + R(i)/c;
    end

    max_error = 0;
    max_location_err = 0;
    for range = 1:100
        k = 100;
        f = zeros(1,n);
        init_vector = [0; 0; 6370; 0];
        t_error = delta_t*((rand(1, n) > 0.5)*2-1);

        for iteration=1:k
            x = init_vector(1,1);
            y = init_vector(2,1);
            z = init_vector(3,1);
            d = init_vector(4,1);
            for i=1:n
                f(i) = (x - satA(i))^2 + (y - satB(i))^2 + (z - satC(i))^2 - (c*(t(i) + t_error(i) - d))^2;
            end
            f_sol = transpose(f);

            Jacobian = zeros(n,4);
            for i=1:n
                Jacobian(i,1) = 2*(x - satA(i));
                Jacobian(i,2) = 2*(x - satB(i));
                Jacobian(i,3) = 2*(x - satC(i));
                Jacobian(i,4) = 2*c^2*(t(i) + t_error(i) - d);
            end

            A = transpose(Jacobian)*Jacobian;
            b = -(transpose(Jacobian)*f_sol);
            v = A \ b;

            init_vector = init_vector + v;
        end
        comp_u = init_vector;

        max_error = max(max_error, max(abs(comp_u - true_solution)));
        max_location_err = max(max_location_err, norm(comp_u - true_solution));
    end

	% Fill in the
	position_error(index,1) = max_location_err;
	n_value(index,1) = n;
    emf(index,1) = max_error/(c*delta_t);
end


n_value
position_error
emf

scatter(n_value, emf);
title('Accuracy Gains as Number of Satellites Increased');
xlabel('Number of Satellites');
ylabel('EMF');

n_value =

     5
     6
     7
     8
     9
    10
    15
    20
    25
    30
    35
    40
    45
    50
    55
    60
    65
    70
    75
    80
    85
    90
    95
   100
   200


position_error =

   0.014600439590207
   0.012440696010066
   0.013179776628537
   0.010600754670808
   0.012261999510706
   0.011238914969229
   0.010542336028930
   0.009133567957769
   0.007029112431280
   0.007070865016775
   0.006436903111350
   0.007066535569200
   0.005848313186843
   0.004989778980199
   0.005566992097848
   0.005105153379862
   0.004679076897071
   0.004131008164579
   0.004172970373857
   0.005002741327855
   0.004629408001522
   0.004610325681260
   0.004513915303222
   0.004205867072926
   0.002769574561135


emf =

   3.950610240222178
   3.401901682135793
   3.579146114689253
   2.961296105840221
   3.213320715724663
   3.074581794279269
   2.803564158302370
   2.597025519598200
   2.126472956239019
   1.752750640528643
   1.605711346344776
   1.889631176370452
   1.463592227378127
   1.345054590941800
   1.572004302864150
   1.380191431253274
   1.451077515729889
   1.208321183372480
   1.233897637505365
   1.479065562406484
   1.286931573751224
   1.300142846933222
   1.306991915996136
   1.212841539757214
   0.768356956221451

Published with MATLAB® R2017a