% 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)