%                            svsel6.m
%  Scope:   This MATLAB macro determines the selection of a set of 6 satel- 
%           lites based on minimum geometric dilution of precision (GDOP)
%           from all possible sets available. For the selected set of 6
%           satellites the computed dilution-of-precision (DOP) values are
%           outputted.
%  Usage:   [hmat,dops,svind] = svsel6(losuvec)
%  Description of parameters:
%           losuvec -  input, line of sight unit vectors for all visible or
%                      selected satellites assembled into a (3 X nrsv) matrix,
%                      where  nrsv is the number of satellites  (nrsv >= 6)
%           hmat    -  output, 4 by 6 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 (6 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] = svsel6(losuvec)

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

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

k = 0;

m1 = nrsv - 5;
for  i1 = 1:m1
   m2 = i1 + 1;
   m3 = nrsv - 4;
   for  i2 = m2:m3
      m4 = i2 + 1;
      m5 = nrsv - 3;
      for  i3 = m4:m5
         m6 = i3 + 1;
         m7 =  nrsv - 2;
         for  i4 = m6:m7
            m8 = i4 + 1;
            m9 = nrsv - 1; 
            for  i5 = m8:m9
               m10 =  i5 + 1;
               m11 =  nrsv;
               for  i6 = m10:m11
                  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);
                     hmat(j,5) = losuvec(j,i5);
                     hmat(j,6) = losuvec(j,i6);
                  end
                  hmat(4,:) = ones(1,6);
                  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;
                     svind(5) = i5;
                     svind(6) = i6;
                  end
               end
            end
         end
      end
   end
end