%                                uposd4.m
%  Scope:   This MATLAB macro computes GPS user's position by using a direct
%           (non-iterative) GPS solution method proposed by Bancroft [1], [2],
%           when four satellite positions and the corresponding pseudoranges
%           are known.
%  Usage:   [nsol,upos,ucbias] = uposd4(svpos,rho)
%  Description of parameters:
%           svpos  -  input, four ECEF satellite positions, each row contains
%                     the three component for a satellite, all components are
%                     in meters
%           rho    -  input, columnwise four pseudoranges, the pseudoranges 
%                     are in meters
%           nsol   -  output, number of solutions (0, 1 or 2)
%           upos   -  output, ECEF user's position, the components are 
%                     in meters; default is 0 when nsol = 0
%           ucbias -  output, user's clock bias (the difference between user's
%                     time and GPS time) measured in units of distance (meters);
%                     default is 0 when nsol = 0
%  References: 
%           [1] Bancroft, S., An algebraic solution of the GPS equations.
%               IEEE Transactions on Aerospace and Electronic Systems, 
%               vol. AES-21, No. 7, 1985, pp. 56-59.
%           [2] Chaffee, J. W., Abel, J. S., Bifurcation of pseudorange 
%               equations. Institute of Navigation, Proceedings of the 
%               1993 National Technical Meeting, San Francisco, CA, Jan.
%               20-22, 1993, pp. 203-211.
%  Last update: 07/29/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

function  [nsol,upos,ucbias] = uposd4(svpos,rho)

[nrow,ncol] = size(svpos);
if  (nrow ~= 4) | (ncol ~= 3)
   error('Error 1  -  UPOSD4; check the dimension of svpos');
end
[nrow,ncol] = size(rho);
if  (nrow ~= 4) | (ncol ~= 1)
   error('Error 2  -  UPOSD4; check the dimension of rho');
end

for k = 1:4
   tempa = svpos(k,1:3);
   alpha(k) = 0.5 * ( tempa * tempa' - rho(k) * rho(k) );
end

b = [ones(4,1) alpha'];
a = [svpos rho];
x = a\b;

nsol = 0;

c0 = x(1:3,1)' * x(1:3,1) - x(4,1) * x(4,1);
c1 = x(1:3,1)' * x(1:3,2) - x(4,1) * x(4,2) - 1.;
c2 = x(1:3,2)' * x(1:3,2) - x(4,2) * x(4,2);
temp = c1 * c1 - c0 * c2;
if  temp < 0.
   return
end
temp = sqrt(temp);

%  Determine first potential solution

lambda1 = (- c1 + temp) / c0;
upos1 = lambda1 * x(1:3,1) + x(1:3,2);
ucbias1 = - lambda1 * x(4,1) - x(4,2);
temp1 = upos1 - svpos(1,1:3)';
distance1 = sqrt(temp1' * temp1) - rho(1);   %  compute compatibility 
                                             %  for first satellite only         
if  abs(distance1) < 100.                    %  threshold of 100 meters
   nsol = nsol + 1;
   upos = upos1;
   ucbias = ucbias1;
end

%  Determine second potential solution

lambda2 = (- c1 - temp) / c0;
upos2 = lambda2 * x(1:3,1) + x(1:3,2);
ucbias2 = - lambda2 * x(4,1) - x(4,2);
temp2 = upos2 - svpos(1,1:3)';
distance2 = sqrt(temp2' * temp2) - rho(1);   %  compute compatibility 
                                             %  for first satellite only         
if  abs(distance2) < 100.                    %  threshold of 100 meters
   nsol = nsol + 1;
   if  nsol == 1
      upos = upos2;
      ucbias = ucbias2;
   elseif  nsol == 2
      upos = [upos1 upos2];
      ucbias = [ucbias1 ucbias2];
   end
end

if nsol == 0
   upos = zeros(3,1);
   ucbias = 0.;
end