%                           svsel4.m
%  Scope:   This MATLAB macro determines the selection of a set of 4 satel- 
%           lites based on minimum geometric dilution of precision (GDOP)
%           from all possible sets available. For the selected set of 4
%           satellites the computed dilution-of-precision (DOP) values are
%           outputted.
%  Usage:   [hmat,dops,svind] = svsel4(losuvec)
%  Description of parameters:
%           losuvec -  input, line of sight unit vectors for all visible or
%                      selected satellites assembled into a (3 by nrsv) matrix,
%                      where  nrsv is the number of satellites  (nrsv >= 4)
%           hmat    -  output, 4 by 4 matrix containing the selected LOS 
%                      vectors combination; each column contains one LOS vector
%                      and 1 as the last component
%           dops    -  output, dilution-of-precision (DOP) values in the
%                      following order      
%                      dops(1) = geometric dilution of precision (GDOP)
%                      dops(2) = position dilution of precision (PDOP) 
%                      dops(3) = horizontal dilution of precision (HDOP) 
%                      dops(4) = vertical dilution of precision (VDOP)
%                      dops(5) = time dilution of precision (TDOP)
%           svind   -  output, indices of the selected optimum satellite 
%                      combination (4 satellites); indices are reflecting
%                      the order line of sight unit vectors are listed in
%                      the input array  losuvec
%  Last update:  06/14/00
%  Copyright (C) 1996-00 by LL Consulting. All Rights Reserved.

function  [hmat,dops,svind] = svsel4(losuvec)

[nrow,nrsv] = size(losuvec);
if nrow  ~=  3
   error('Error 1  -  SVSEL4.M;  error in the input data  -  LOS vectors');
end
if nrsv  <  4
   error('Error 2  -  SVSEL4.M;  less than 4 satellites');
end

dops = zeros(5,1);
dops(1) = 1.e+38;
svind = zeros(4,1);

k = 0;

m1 = nrsv - 3;
for  i1 = 1:m1
   m2 = i1 + 1;
   m3 = nrsv - 2;
   for  i2 = m2:m3
      m4 = i2 + 1;
      m5 = nrsv - 1;
      for  i3 = m4:m5
         m6 = i3 + 1;
         m7 =  nrsv;
         for  i4 = m6:m7
            for  j = 1:3
               hmat(j,1) = losuvec(j,i1);
               hmat(j,2) = losuvec(j,i2);
               hmat(j,3) = losuvec(j,i3);
               hmat(j,4) = losuvec(j,i4);
            end
            hmat(4,:) = ones(1,4);
            dop = inv(hmat * hmat');
            gdop = sqrt(dop(1,1) + dop(2,2) + dop(3,3) + dop(4,4)); % gdop
            k = k + 1;
            if   gdop < dops(1)
               dops(1) = gdop;                                      % gdop 
               dops(2) = sqrt(dop(1,1) + dop(2,2) + dop(3,3));      % pdop
               dops(3) = sqrt(dop(1,1) + dop(2,2));                 % hdop
               dops(4) = sqrt(dop(3,3));                            % vdop
               dops(5) = sqrt(dop(4,4));                            % tdop
               svind(1) = i1;
               svind(2) = i2;
               svind(3) = i3;
               svind(4) = i4;
            end
         end
      end
   end
end