GPS_LS.m

% Method and Problem from "Ta2011Thesis.pdf"

syms X Y Z dT;

x = [-1.14435581932368,0.88498653721608,-1.28799462471086,-0.62238562333828,1.04260459627803,0.15114376130666,1.98311575365209]*1e7;
y = [2.18537228998174,1.52115049991917,0.84279115293681,2.55024173739922,2.18281560737286,2.36981504953570,0.65606228041700]*1e7;
z = [0.92840515634504,1.98379922835602,2.17295977908060,-0.38396284978272,-1.10756652807472,1.16498729017268,1.72062794938024]*1e7;
r = [21364414.7719640,21133235.1936572,23653202.7278275,22023593.4330683,23223303.5998171,20094101.9563987,23840149.6080318];
c = 3e8;

for i = 1:7
    R(i) = r(i) - sqrt((X-x(i))^2 + (Y-y(i))^2 + (Z-z(i))^2) - c*dT;
end

R = R';
J = [];

for i = 1:7
    J = [J; diff(R(i),X) diff(R(i),Y) diff(R(i),Z) diff(R(i),dT)];
end

x = [0;0;0;0];
eps = 1e-3;
ite = 20;
i = 1;
dxs = eps + 1;

while (abs(dxs'*dxs) > eps) && i < ite 
    Js = subs(J,[X,Y,Z,dT],x');
    Rs = subs(R,[X,Y,Z,dT],x');
    dx = (Js'*Js)\Js'*Rs;
    x = x - double(dx);
    i = i + 1;
    dxs = double(dx);
    fprintf("Iteration: %d\nError: %.2e\nOutput: X=%.3e Y=%.3e Z=%.3e dT=%.3e\n\n",i,dxs'*dxs,x(1),x(2),x(3),x(4));
end

Xr = 918074;
Yr = 5703774;
Zr = 2693919;

fprintf("AE: dX=%.3e dY=%.3e dZ=%.3e\n",x(1)-Xr,x(2)-Yr,x(3)-Zr);
fprintf("APE: dX=%.3e dY=%.3e dZ=%.3e\n\n",100*(x(1)-Xr)/Xr,100*(x(2)-Yr)/Yr,100*(x(3)-Zr)/Zr);