% Step 6, add 4 more satelites
p=26570;
c=299792.458;
A1=p*cos(pi/6)*cos(pi/4);
A2=p*cos(pi/4)*cos(pi/3);
A3=p*cos(4*pi/3)*cos(pi/3);
A4=p*cos(3*pi/2)*cos(pi/6);
A5=p*cos(pi/6)*cos(pi/6);
A6=p*cos(pi/3)*cos(pi/4);
A7=p*cos(2*pi/3)*cos(pi/3);
A8=p*cos(4*pi/3)*cos(pi/2);
B1=p*cos(pi/6)*sin(pi/4);
B2=p*cos(2*pi/3)*sin(pi/3);
B3=p*cos(pi/3)*sin(pi/6);
B4=p*cos(4*pi/3)*sin(pi/3);
B5=p*cos(pi/6)*sin(pi/6);
B6=p*cos(pi/3)*sin(pi/3);
B7=p*cos(pi/3)*sin(pi/2);
B8=p*cos(2*pi/3)*sin(pi/4);
C1=p*sin(pi/6);
C2=p*sin(pi/6);
C3=p*sin(pi/3);
C4=p*sin(pi/3);
C5=p*sin(pi/4);
C6=p*sin(pi/4);
C7=p*sin(pi/2);
C8=p*sin(pi/2);
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));
R5=sqrt((A5^2)+(B5^2)+((C5-6370)^2));
R6=sqrt((A6^2)+(B6^2)+((C6-6370)^2));
R7=sqrt((A7^2)+(B7^2)+((C7-6370)^2));
R8=sqrt((A8^2)+(B8^2)+((C8-6370)^2));
x0=[0;0;6370;.0001];
x=x0(1);
y=x0(2);
z=x0(3);
d=x0(4);
T1=d+(R1/c);
T2=d+(R2/c);
T3=d+(R3/c);
T4=d+(R4/c);
T5=d+(R5/c);
T6=d+(R6/c);
T7=d+(R7/c);
T8=d+(R8/c);

plot3(A1,B1,C1,'bo')
hold on
plot3(A2,B2,C2,'go')
plot3(A3,B3,C3,'ko')
plot3(A4,B4,C4,'ro')
plot3(A5,B5,C5,'ko')
plot3(A6,B6,C6,'ro')
plot3(A7,B7,C7,'ko')
plot3(A8,B8,C8,'ro')
plot3(0,0,0,'ro','linewidth',15)
maxA = max([A1,A2,A3,A4]);
maxB = max([B1,B2,B3,B4]);
maxC = max([C1,C2,C3,C4]);
axis([-maxA maxA -maxB maxB -.5*maxC 1.5*maxC ])
plot3(0,0,0,'ro','linewidth',15)
%% Run Gauss Newton Method
%clc
x0=[0;0;0;.0001];
x=x0(1);
y=x0(2);
z=x0(3);
d=x0(4);
for i=1:50
    Df=[(2*x-2*A1) (2*y-2*B1) (2*z-2*C1) (-2*d*(c^2)+2*T1*(c^2));...
        (2*x-2*A2) (2*y-2*B2) (2*z-2*C2) (-2*d*(c^2)+2*T2*(c^2));...
        (2*x-2*A3) (2*y-2*B3) (2*z-2*C3) (-2*d*(c^2)+2*T3*(c^2));...
        (2*x-2*A4) (2*y-2*B4) (2*z-2*C4) (-2*d*(c^2)+2*T4*(c^2));...
        (2*x-2*A5) (2*y-2*B5) (2*z-2*C5) (-2*d*(c^2)+2*T5*(c^2));...
        (2*x-2*A6) (2*y-2*B6) (2*z-2*C6) (-2*d*(c^2)+2*T6*(c^2));...
        (2*x-2*A7) (2*y-2*B7) (2*z-2*C7) (-2*d*(c^2)+2*T7*(c^2));...        
        (2*x-2*A8) (2*y-2*B8) (2*z-2*C8) (-2*d*(c^2)+2*T8*(c^2))];   
    f=[((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)^2));...
       ((x-A5)^2)+((y-B5)^2)+((z-C5)^2)-((c^2)*((T5-d)^2));...
       ((x-A6)^2)+((y-B6)^2)+((z-C6)^2)-((c^2)*((T6-d)^2));...
       ((x-A7)^2)+((y-B7)^2)+((z-C7)^2)-((c^2)*((T7-d)^2));...       
       ((x-A8)^2)+((y-B8)^2)+((z-C8)^2)-((c^2)*((T8-d)^2))];
    %g=-Df\f;
    g = -(Df'*Df)\(Df'*f);
    x0=[x;y;z;d];
    x1=x0+g;
    x=x1(1);
    y=x1(2);
    z=x1(3);
    d=x1(4);
    index = i;
    % Condition number
    condDf(i) = cond(Df'*Df);
    condf(i) = cond(Df'*f);
    condg(i) = cond(g);
end
x
y
z
d
condDf = condDf'
GPS_error = sqrt(x^2+y^2+(6370-z)^2)