%                                uposdg.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 at least four satellite positions and the corresponding
%           pseudoranges are known.
%  Usage:   [nsol,upos,ucbias] = uposdg(svpos,rho)
%  Description of parameters:
%           svpos  -  input, at least four ECEF satellite positions, each row 
%                     contains the three component for a satellite, 
%                     all components are in meters
%           rho    -  input, columnwise, at least 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
%           ucbias -  output, user's clock bias (the difference between user's
%                     time and GPS time) measured in units of distance (meters)
%  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] = uposdg(svpos,rho)

[nrow1,ncol1] = size(svpos);
if  (nrow1 < 4) | (ncol1 ~= 3)
   error('Error1  -  UPOSDG; check the dimension of svpos');
end
[nrow2,ncol2] = size(rho);
if  (nrow2 < 4) | (ncol2 ~= 1) | (nrow1 ~= nrow2)
   error('Error2  -  UPOSDG; check the dimension of rho');
end

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

b = [ones(nrow1,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);

%  Determine compatibility of the first potential solution

d1 = 0.;
for k= 1:nrow1
   temp1 = upos1 - svpos(k,1:3)';
   dist1 = sqrt(temp1' * temp1) - rho(k);
   d1 = d1 + abs(dist1);
end

if  (d1/nrow1) < 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);

%  Determine compatibility of the second potential solution

d2 = 0.;
for k= 1:nrow1
   temp2 = upos2 - svpos(k,1:3)';
   dist2 = sqrt(temp2' * temp2) - rho(k);     
   d2 = d2 + abs(dist2);
end
    
if  (d2/nrow1) < 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