User:Mike1024/gpsAlgorithm

MATLAB source code to replicate my calculations:

lorentzInnerProduct function is defined as:

function result = lorentzInnerProduct(a,b)
  result = a(1)*b(1) + a(2)*b(2) + a(3)*b(3) - a(4)*b(4);
  return

Then we can perform these calculations:

% Satellite position and pseudorange:
%       X            Y            Z           PR
A = [15524471.18 -16649826.22 13512272.39 -22262088.18 ;
    -2304058.534 -23287906.47 11917038.11 -19908187.05 ;
     -14799931.4 -21425358.24 6069947.224 -21479180.58 ;
     16680243.36 -3069625.561 20378551.05 -24554242.17]
 
% Expected solution:
% X = -733185.996310
% Y = -5443791.999681
% Z = 3231192.995208
% Clock bias = 299.991910

i0 = ones(4,1)
r=zeros(4,1);
for row=1:4
    r(row,1) = 0.5*lorentzInnerProduct(A(row,:),A(row,:));
end
B=inv(A' * A) * A' % Assuming W = identity matrix = has no effect

u=B*i0
v=B*r
E= lorentzInnerProduct(u,u);
F= lorentzInnerProduct(u,v)-1;
G= lorentzInnerProduct(v,v);

% Quadratic equation:
a=E
b=2*F
c=G
lamA = (-b+sqrt(b^2-4*a*c))/(2*a)
lamB = (-b-sqrt(b^2-4*a*c))/(2*a)

% Two possible solutions:
yA = lamA*u+v
yB = lamB*u+v

% Discard the solution that doesn't work:
testA = A-ones(4,1)*yA'
distsA = sum(testA(:,1:3)'.^2)'.^0.5
distErrorsA = A(:,4)+distsA+ones(4,1)*yA(4,1)
if ( max(distErrorsA) < 1e-6 )
  fprintf('The solution is: \n')
  fprintf('%.6f\n',yB)
end

testB = A-ones(4,1)*yB'
distsB = sum(testB(:,1:3)'.^2)'.^0.5
distErrorsB = A(:,4)+distsB+ones(4,1)*yB(4,1)
if ( max(distErrorsB) < 1e-6 )
  fprintf('The solution is: \n')
  fprintf('%.6f\n',yB)
end

This produces the output:

The solution is: 
-733185.996310
-5443791.999681
3231192.995208
299.991910