lib/PosSol/PRSolutionLegacy.cpp

//==============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 3.0 of the License, or
//  any later version.
//
//  The GPSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GPSTk; if not, write to the Free Software Foundation,
//  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
//
//  This software was developed by Applied Research Laboratories at the
//  University of Texas at Austin.
//  Copyright 2004, The Board of Regents of The University of Texas System
//
//==============================================================================

//==============================================================================
//
//  This software was developed by Applied Research Laboratories at the
//  University of Texas at Austin, under contract to an agency or agencies
//  within the U.S. Department of Defense. The U.S. Government retains all
//  rights to use, duplicate, distribute, disclose, or release this software.
//
//  Pursuant to DoD Directive 523024
//
//  DISTRIBUTION STATEMENT A: This software has been approved for public
//                            release, distribution is unlimited.
//
//==============================================================================


/**
 * @file PRSolutionLegacy.cpp
 * Autonomous pseudorange navigation solution, including RAIM algorithm
 */

#include "MathBase.hpp"
#include "PRSolutionLegacy.hpp"
#include "EphemerisRange.hpp"
#include "GPSEllipsoid.hpp"
#include "GPSWeekSecond.hpp"

//#define DEBUG_PRINT_WARNINGS

// ----------------------------------------------------------------------------
// Combinations.hpp
// Find all the Combinations of n things taken k at a time.

#include "Exception.hpp"

/// Class Combinations will compute C(n,k), all the Combinations of n things
/// taken k at a time (where k <= n).
/// Let n 'things' be indexed by i (i=0...n-1), e.g. stored in an array of length n.
/// This class computes C(n,k) as sets of k indexes into the 'things' array.
/// These indexes are accessible via member functions Selection() or isSelected().
/// Next() computes the next combination until there are no more (when it returns -1).

class Combinations
{

public:

      /// Default constructor
   Combinations(void)
      throw()
   {
      nc = n = k = 0;
      Index = NULL;
   }

      /// Constructor for C(n,k) = Combinations of n things taken k at a time (k <= n)
      /// @throw Exception on invalid input (k>n).
   Combinations(int N, int K)
   {
      try
      {
         init(N,K);
      }
      catch(gpstk::Exception& e)
      {
         GPSTK_RETHROW(e);
      }
   }

      /// copy constructor
   Combinations(const Combinations& right)
      throw()
   {
      *this = right;
   }

      /// destructor
   ~Combinations(void)
   {
      if (Index)
         delete[] Index;
      Index = NULL;
   }

      /// Assignment operator.
   Combinations& operator=(const Combinations& right)
      throw()
   {
      this->~Combinations();
      init(right.n,right.k);
      nc = right.nc;
      for (int j=0; j<k; j++)
      {
         Index[j] = right.Index[j];
      }
      return *this;
   }

      /// Compute the next combination, returning the number of Combinations computed
      /// so far; if there are no more Combinations, return -1.
   int Next(void) throw();

      /// Return index i (0 <= i < n) of jth selection (0 <= j < k);
      /// if j is out of range, return -1.
   int Selection(int j)
      throw()
   {
      if (j < 0 || j >= k)
         return -1;
      return Index[j];
   }

      /// Return true if the given index j (0 <= j < n) is
      /// currently selected (i.e. if j = Selection(i) for some i)
   bool isSelected(int j)
      throw()
   {
      for (int i=0; i<k; i++)
      {
         if (Index[i] == j)
            return true;
      }
      return false;
   }

private:

      /// The initialization routine used by constructors.
      /// @throw Exception on invalid input (k>n or either n or k < 0).
   void init(int N, int K);

      /// Recursive function to increment Index[j].
   int Increment(int j) throw();

   int nc;         ///< number of Combinations computed so far
   int k,n;        ///< Combinations of n things taken k at a time, given at c'tor
   int* Index;     ///< Index[j] = index of jth selection (j=0...k-1; I[j]=0...n-1)

};

// ----------------------------------------------------------------------------

int Combinations::Next(void)
   throw()
{
   if (k < 1)
      return -1;
   if (Increment(k-1) != -1)
      return ++nc;
   return -1;
}

int Combinations::Increment(int j)
   throw()
{
   if (Index[j] < n-k+j) {        // can this index be incremented?
      Index[j]++;
      for (int m=j+1; m<k; m++)
      {
         Index[m]=Index[m-1]+1;
      }
      return 0;
   }
      // is this the last index?
   if (j-1 < 0) return -1;
      // increment the next lower index
   return Increment(j-1);
}

void Combinations::init(int N, int K)
{
   if (K > N || N < 0 || K < 0)
   {
      gpstk::Exception e("Combinations(n,k) must have k <= n, with n,k >= 0");
      GPSTK_THROW(e);
   }

   if (K > 0)
   {
      Index = new int[K];
      if (!Index)
      {
         gpstk::Exception e("Could not allocate");
         GPSTK_THROW(e);
      }
   }
   else
      Index = NULL;

   nc = 0;
   k = K;
   n = N;
   for (int j=0; j<k; j++)
   {
      Index[j]=j;
   }
}

// ----------------------------------------------------------------------------

using namespace std;
using namespace gpstk;

namespace gpstk
{
   int PRSolutionLegacy::RAIMCompute(const CommonTime& Tr,
                                     vector<SatID>& Satellite,
                                     const vector<double>& Pseudorange,
                                     const XvtStore<SatID>& Eph,
                                     TropModel *pTropModel)
   {
      try
      {
         int iret,N,Nreject,MinSV,stage;
         size_t i, j;
         vector<bool> UseSat, UseSave;
         vector<int> GoodIndexes;
         // Use these to save the 'best' solution within the loop.
         int BestNIter=0;
         double BestRMS=0.0, BestSL=0.0, BestConv=0.0;
         Vector<double> BestSol(3,0.0);
         vector<bool> BestUse;
         BestRMS = -1.0;      // this marks the 'Best' set as unused.
         Matrix<double> BestCovariance;

         // ----------------------------------------------------------------
         // initialize

         Valid = false;

         bool hasGlonass = false;
         bool hasOther = false;
         Vector<int> satSystems(Satellite.size());
         //Check if we have a mixed system
         for ( i = 0; i < Satellite.size(); ++i)
         {
            satSystems[i] = ((Satellite[i].system == SatelliteSystem::Glonass) ? (1) : (0) );

            if (satSystems[i] == 1) hasGlonass = true;
            else hasOther = true;
         }

         // Save the input solution
         // (for use in rejection when ResidualCriterion is false).
         if (!hasGlonass && Solution.size() != 4)
         {
            Solution.resize(4);
         }
         else if (hasGlonass && hasOther && Solution.size() != 5)
         {
            Solution.resize(5);
         }
         Solution = 0.0;
         APrioriSolution = Solution;

         // ----------------------------------------------------------------
         // fill the SVP matrix, and use it for every solution
         // NB this routine can set Satellite[.]=negative when no ephemeris

         i = PrepareAutonomousSolution(Tr, Satellite, Pseudorange, Eph, SVP,
             Debug?pDebugStream:0);
         if(Debug && pDebugStream) {
            *pDebugStream << "In RAIMCompute after PAS(): Satellites:";
            for(j=0; j<Satellite.size(); j++) {
               RinexSatID rs(::abs(Satellite[j].id), Satellite[j].system);
               *pDebugStream << " " << (Satellite[j].id < 0 ? "-":"") << rs;
            }
            *pDebugStream << endl;
            *pDebugStream << " SVP matrix("
               << SVP.rows() << "," << SVP.cols() << ")" << endl;
            *pDebugStream << fixed << setw(16) << setprecision(3) << SVP << endl;
         }
         if (i) return i;  // return is 0 (ok) or -4 (no ephemeris)

         // count how many good satellites we have
         // Initialize UseSat based on Satellite, and build GoodIndexes.
         // UseSat is used to mark good sats (true) and those to ignore (false).
         // UseSave saves the original so it can be reused for each solution.
         // Let GoodIndexes be all the indexes of Satellites that are good:
         // UseSat[GoodIndexes[.]] == true by definition

         for (N=0,i=0; i<Satellite.size(); i++)
         {
            if (Satellite[i].id > 0)
            {
               N++;
               UseSat.push_back(true);
               GoodIndexes.push_back(i);
            }
            else
            {
               UseSat.push_back(false);
            }
         }
         UseSave = UseSat;

         // don't even try if there are not 4 good satellites

         if (!hasGlonass && N < 4)
            return -3;
         else if (hasGlonass && hasOther && N < 5)
            return -3;

         // minimum number of sats needed for algorithm
         MinSV = 5;   // this would be RAIM
         // ( not really RAIM || not RAIM at all - just one solution)
         if (!ResidualCriterion || NSatsReject==0)
            MinSV=4;

         // how many satellites can RAIM reject, if we have to?
         // default is -1, meaning as many as possible
         Nreject = NSatsReject;
         if (Nreject == -1 || Nreject > N-MinSV)
            Nreject=N-MinSV;

         // ----------------------------------------------------------------
         // now compute the solution, first with all the data. If this fails,
         // reject 1 satellite at a time and try again, then 2, etc.

         // Slopes for each satellite are computed (cf. the RAIM algorithm)
         Vector<double> Slopes(Pseudorange.size());

         // Residuals stores the post-fit data residuals.
         Vector<double> Residuals(Satellite.size(),0.0);

         // stage is the number of satellites to reject.
         stage = 0;

         do{
            // compute all the Combinations of N satellites taken stage at a time
            Combinations Combo(N,stage);

            // compute a solution for each combination of marked satellites
            do{
               // Mark the satellites for this combination
               UseSat = UseSave;
               for (i = 0; i < GoodIndexes.size(); i++)
               {
                  if (Combo.isSelected(i))
                     UseSat[i]=false;
               }
               // ----------------------------------------------------------------
               // Compute a solution given the data; ignore ranges for marked
               // satellites. Fill Vector 'Slopes' with slopes for each unmarked
               // satellite.
               // Return 0  ok
               //       -1  failed to converge
               //       -2  singular problem
               //       -3  not enough good data
               NIterations = MaxNIterations;             // pass limits in
               Convergence = ConvergenceLimit;
               iret = AutonomousPRSolution(Tr, UseSat, SVP, pTropModel, Algebraic,
                  NIterations, Convergence, Solution, Covariance, Residuals, Slopes,
                  pDebugStream, ((hasGlonass && hasOther)?(&satSystems):(NULL)) );

               // ----------------------------------------------------------------
               // Compute RMS residual...
               if (!ResidualCriterion)
               {
                  // when 'distance from a priori' is the criterion.
                  Vector<double> D=Solution-APrioriSolution;
                  RMSResidual = RMS(D);
               }
               else
               {
                  // and in the usual case
                  RMSResidual = RMS(Residuals);
               }
               // ... and find the maximum slope
               MaxSlope = 0.0;
               if (iret == 0)
               {
                  for (i=0; i<UseSat.size(); i++)
                  {
                     if (UseSat[i] && Slopes(i)>MaxSlope)
                        MaxSlope=Slopes[i];
                  }
               }
               // ----------------------------------------------------------------
               // print solution with diagnostic information
               if (Debug && pDebugStream)
               {
                  GPSWeekSecond weeksec(Tr);
                  *pDebugStream << "RPS " << setw(2) << stage
                     << " " << setw(4) << weeksec.week
                     << " " << fixed << setw(10) << setprecision(3) << weeksec.sow
                     << " " << setw(2) << N-stage << setprecision(6)
                     << " " << setw(16) << Solution(0)
                     << " " << setw(16) << Solution(1)
                     << " " << setw(16) << Solution(2)
                     << " " << setw(14) << Solution(3);
                  if (hasGlonass && hasOther)
                     *pDebugStream << " " << setw(16) << Solution(4);
                  *pDebugStream << " " << setw(12) << RMSResidual
                     << " " << fixed << setw(5) << setprecision(1) << MaxSlope
                     << " " << NIterations
                     << " " << scientific << setw(8) << setprecision(2)<< Convergence;
                     // print the SatID for good sats
                  for (i=0; i<UseSat.size(); i++)
                  {
                     if (UseSat[i])
                        *pDebugStream << " " << setw(3)<< Satellite[i].id;
                     else
                        *pDebugStream << " " << setw(3) << -::abs(Satellite[i].id);
                  }
                     // also print the return value
                  *pDebugStream << " (" << iret << ")" << endl;
               }// end debug print

               // ----------------------------------------------------------------
               // deal with the results of AutonomousPRSolution()
               if (iret)
               {     // failure for this combination
                  RMSResidual = 0.0;
                  Solution = 0.0;
               }
               else
               {  // success
                     // save 'best' solution for later
                  if (BestRMS < 0.0 || RMSResidual < BestRMS)
                  {
                     BestRMS = RMSResidual;
                     BestSol = Solution;
                     BestUse = UseSat;
                     BestSL = MaxSlope;
                     BestConv = Convergence;
                     BestNIter = NIterations;
                  }
                     // quit immediately?
                  if ((stage==0 || ReturnAtOnce) && RMSResidual < RMSLimit)
                     break;
               }

               // get the next Combinations and repeat
            } while(Combo.Next() != -1);

            // end of the stage
            if (BestRMS > 0.0 && BestRMS < RMSLimit)
            {     // success
               iret=0;
               break;
            }

            // go to next stage
            stage++;
            if (stage > Nreject)
               break;

         } while(iret == 0);        // end loop over stages

         // ----------------------------------------------------------------
         // copy out the best solution and return
         Convergence = BestConv;
         NIterations = BestNIter;
         RMSResidual = BestRMS;
         Solution = BestSol;
         MaxSlope = BestSL;
         Covariance =BestCovariance;
         for (Nsvs=0,i=0; i<BestUse.size(); i++)
         {
            if (!BestUse[i])
               Satellite[i].id = -::abs(Satellite[i].id);
            else
               Nsvs++;
         }

         if (iret==0 && BestSL > SlopeLimit)
            iret=1;
         if (iret==0 && BestSL > SlopeLimit/2.0 && Nsvs == 5)
            iret=1;
         if (iret>=0 && BestRMS >= RMSLimit)
            iret=2;

         if (iret==0)
            Valid=true;

         return iret;
      }
      catch(Exception& e)
      {
         GPSTK_RETHROW(e);
      }
   }  // end PRSolutionLegacy::RAIMCompute()

///-------------------------------------------------------------------------///
///----------------------PrepareAutonomousSolution--------------------------///
///-------------------------------------------------------------------------///

   int PRSolutionLegacy::PrepareAutonomousSolution(const CommonTime& Tr,
                                                   vector<SatID>& Satellite,
                                                   const vector<double>& Pseudorange,
                                                   const XvtStore<SatID>& Eph,
                                                   Matrix<double>& SVP,
                                                   ostream *pDebugStream)
      throw()
   {
      int i,j,nsvs,N=Satellite.size();
      CommonTime tx;                // transmit time
      Xvt PVT;

      if (N <= 0)
         return 0;
      SVP = Matrix<double>(N,4);
      SVP = 0.0;

      for (nsvs=0,i=0; i<N; i++)
      {
            // skip marked satellites
         if (Satellite[i].id <= 0) continue;

            // test the system
            if(Satellite[i].system == SatelliteSystem::GPS)
               ;
            else {
               Satellite[i].id = -::abs(Satellite[i].id);
               if(pDebugStream) *pDebugStream
                  << "Warning: Ignoring satellite (system) " << Satellite[i];
               continue;
            }


            // first estimate of transmit time
         tx = Tr;
         tx -= Pseudorange[i]/C_MPS;
            // get ephemeris range, etc
         try
         {
            PVT = Eph.getXvt(Satellite[i], tx);
         }
         catch(InvalidRequest& e)
         {
            ///Negate SatID because getXvt failed.
            #ifdef DEBUG_PRINT_WARNINGS
               cout << "Eph.getXvt(Satellite[" << i << "], tx); threw InvalidRequest:" << endl;
               cout << e << endl << endl;
            #endif
            Satellite[i].id = -::abs(Satellite[i].id);
               if(pDebugStream) *pDebugStream
                  << "Warning: PRSolutionLegacy ignores satellite (ephemeris) "
                  << Satellite[i] << endl;
            continue;
         }

            // update transmit time and get ephemeris range again
         tx -= PVT.clkbias + PVT.relcorr;     // clk+rel
         try
         {
            PVT = Eph.getXvt(Satellite[i], tx);
         }
         catch(InvalidRequest& e)
         {
            ///Negate SatID because getXvt failed.
            Satellite[i].id = -::abs(Satellite[i].id);
            continue;
         }

            // SVP = {SV position at transmit time}, raw range + clk + rel
         for (j=0; j<3; j++)
         {
            SVP(i,j) = PVT.x[j];
         }
         SVP(i,3) = Pseudorange[i] + C_MPS * (PVT.clkbias + PVT.relcorr);
         nsvs++;
      }

      if (nsvs == 0)
         return -4;
      return 0;
   } // end PrepareAutonomousPRSolution

///-------------------------------------------------------------------------///
///-------------------------------------------------------------------------///

   int PRSolutionLegacy::AlgebraicSolution(Matrix<double>& A,
                                           Vector<double>& Q,
                                           Vector<double>& X,
                                           Vector<double>& R)
   {
       try
       {
         int N=A.rows();
         Matrix<double> AT=transpose(A);
         Matrix<double> B=AT,C(4,4);

         C = AT * A;
         // invert
         try
         {
            //double big,small;
            //condNum(C,big,small);
            //if (small < 1.e-15 || big/small > 1.e15) return -2;
            C = inverseSVD(C);
         }
         catch(SingularMatrixException& sme)
         {
            return -2;
         }

         B = C * AT;

         Vector<double> One(N,1.0),V(4),U(4);
         double E,F,G,d,lam;

         U = B * One;
         V = B * Q;
         E = Minkowski(U,U);
         F = Minkowski(U,V) - 1.0;
         G = Minkowski(V,V);
         d = F*F-E*G;
            // avoid imaginary solutions: what does this really mean?
         if (d < 0.0)
            d=0.0;
         d = SQRT(d);

            // first solution ...
         lam = (-F+d)/E;
         X = lam*U + V;
         X(3) = -X(3);
            // ... and its residual
         R(0) = A(0,3)-X(3) - RSS(X(0)-A(0,0), X(1)-A(0,1), X(2)-A(0,2));

            // second solution ...
         lam = (-F-d)/E;
         X = lam*U + V;
         X(3) = -X(3);
            // ... and its residual
         R(1) = A(0,3)-X(3) - RSS(X(0)-A(0,0), X(1)-A(0,1), X(2)-A(0,2));

            // pick the right solution
         if (ABS(R(1)) > ABS(R(0)))
         {
            lam = (-F+d)/E;
            X = lam*U + V;
            X(3) = -X(3);
         }

            // compute the residuals
         for (int i=0; i<N; i++)
         {
            R(i) = A(i,3)-X(3) - RSS(X(0)-A(i,0), X(1)-A(i,1), X(2)-A(i,2));
         }

         return 0;

      }
      catch(Exception& e)
      {
         GPSTK_RETHROW(e);
      }
   }  // end PRSolutionLegacy::AlgebraicSolution

///-------------------------------------------------------------------------///
///--------------------------AutonomousPRSolution---------------------------///
///-------------------------------------------------------------------------///

   int PRSolutionLegacy::AutonomousPRSolution(const CommonTime& T,
                                             const vector<bool>& Use,
                                             const Matrix<double> SVP,
                                             TropModel *pTropModel,
                                             const bool Algebraic,
                                             int& n_iterate,
                                             double& converge,
                                             Vector<double>& Sol,
                                             Matrix<double>& Cov,
                                             Vector<double>& Resid,
                                             Vector<double>& Slope,
                                             ostream *pDebugStream,
                                             Vector<int>* satSystems)   ///Change
   {
      if (!pTropModel)
      {
         Exception e("Undefined tropospheric model");
         GPSTK_THROW(e);
      }

      int size;
      if (satSystems != NULL)
         size = 5;
      else
         size = 4;

      try
      {
         int iret,j,n,N;
         size_t i;
         double rho,wt,svxyz[3];
         GPSEllipsoid ell;               // WGS84?

         //if (pDebugStream)
         //   *pDebugStream << "Enter APRS " << n_iterate << " "
         //                << scientific << setprecision(3) << converge << endl;

            // find the number of good satellites
         for (N=0,i=0; i<Use.size(); i++)
         {
            if (Use[i])
               N++;
         }
         if ((satSystems != NULL) && N < 5)
            return -3;
         else if (N < 4)
            return -3;

            // define for computation
         Vector<double> CRange(N),dX(size);
         Matrix<double> P(N,size),PT,G(size,N),PG(N,N);
         Xvt SV,RX;

         Sol.resize(size);
         Cov.resize(size,size);
         Resid.resize(N);
         Slope.resize(Use.size());

            // define for algebraic solution
         Vector<double> U(size),Q(N);
         Matrix<double> A(P);
            // and for linearized least squares
         int niter_limit = (n_iterate < 2 ? 2 : n_iterate);
         double conv_limit = converge;

            // prepare for iteration loop
         Sol = 0.0;                                // initial guess: center of earth
         n_iterate = 0;
         converge = 0.0;

            // iteration loop
            // do at least twice (even for algebraic solution) so that
            // trop model gets evaluated
          //bool applied_trop;
          do {
            //applied_trop = true;


               // current estimate of position solution
            for(i=0; i<3; i++) RX.x[i]=Sol(i);

               // loop over satellites, computing partials matrix
            for (n=0,i=0; i<Use.size(); i++)
            {
                  // ignore marked satellites
               if (!Use[i])
                  continue;

                  // time of flight (sec)
               if (n_iterate == 0)
                  rho = 0.070;             // initial guess: 70ms
               else
                  rho = RSS(SVP(i,0)-Sol(0), SVP(i,1)-Sol(1), SVP(i,2)-Sol(2))
                            / ell.c();

                  // correct for earth rotation
               wt = ell.angVelocity()*rho;             // radians
               svxyz[0] =  ::cos(wt)*SVP(i,0) + ::sin(wt)*SVP(i,1);
               svxyz[1] = -::sin(wt)*SVP(i,0) + ::cos(wt)*SVP(i,1);
               svxyz[2] = SVP(i,2);

                  // corrected pseudorange (m)
               CRange(n) = SVP(i,3);

                  // correct for troposphere (but not on the first iteration)
               if (n_iterate > 0)
               {
                  SV.x[0] = svxyz[0];
                  SV.x[1] = svxyz[1];
                  SV.x[2] = svxyz[2];
                  // must test RX for reasonableness to avoid corrupting TropModel
                  Position R(RX),S(SV);
                  double tc=R.getHeight(), elev = R.elevation(SV);
                  if (elev < 0.0 || tc > 10000.0 || tc < -1000) {
                     tc=0.0;
                     //applied_trop = false;
                  }
                  else tc = pTropModel->correction(R,S,T);
                  //if(pDebugStream) *pDebugStream << "Trop " << i << " "
                  //   << fixed << setprecision(3) << tc << endl;
                  CRange(n) -= tc;

               }

                  // geometric range
               rho = RSS(svxyz[0]-Sol(0),svxyz[1]-Sol(1),svxyz[2]-Sol(2));

                  // partials matrix
               P(n,0) = (Sol(0)-svxyz[0])/rho;           // x direction cosine
               P(n,1) = (Sol(1)-svxyz[1])/rho;           // y direction cosine
               P(n,2) = (Sol(2)-svxyz[2])/rho;           // z direction cosine
               P(n,3) = 1.0;
               if (satSystems != NULL)
                  P(n,4) = (*satSystems)[i]; // 1 if GLONASS, 0 if GPS

                  // data vector: corrected range residual
               Resid(n) = CRange(n) - rho - Sol(3);
               if (satSystems != NULL && (*satSystems)[i] == 1)
               {
                  Resid(n) -= Sol(4);
               }

                  // TD: allow weight matrix = measurement covariance
               // P *= MCov;
               // Resid *= MCov;

                  // compute intermediate quantities for algebraic solution
               if (satSystems == NULL && Algebraic)
               {
                  U(0) = A(n,0) = svxyz[0];
                  U(1) = A(n,1) = svxyz[1];
                  U(2) = A(n,2) = svxyz[2];
                  U(3) = A(n,3) = CRange(n);
                  U(4) = A(n,4) = P(n,4);
                  Q(n) = 0.5 * Minkowski(U,U);
               }

               n++;        // n is number of good satellites - used for Slope
            }  // end loop over satellites

               // compute information matrix = inverse covariance matrix
            PT = transpose(P);
            Cov = PT * P;

               // invert using SVD
            //double big,small;
            //condNum(PT*P,big,small);
            //if (small < 1.e-15 || big/small > 1.e15)
            //   return -2;
            try
            {
               Cov = inverseSVD(Cov);
            }
            //try { Cov = inverseLUD(Cov); }
            catch(SingularMatrixException& sme)
            {
               return -2;
            }

               // generalized inverse
            G = Cov * PT;
            PG = P * G;                         // used for Slope

            n_iterate++;                        // increment number iterations

               // ----------------- algebraic solution -----------------------
            if (satSystems == NULL && Algebraic)
            {
               iret = PRSolutionLegacy::AlgebraicSolution(A,Q,Sol,Resid);
               if (iret)
                  return iret;                     // (singular)
               if (n_iterate > 1)
               {                       // need 2, for trop
                  iret = 0;
                  break;
               }
            }
               // ----------------- linearized least squares solution --------
            else
            {
               dX = G * Resid;
               Sol += dX;
                  // test for convergence
               converge = norm(dX);
                  // success: quit
               if (n_iterate > 1 && converge < conv_limit)
               {
                  iret = 0;
                  break;
               }
                  // failure: quit
               if (n_iterate >= niter_limit || converge > 1.e10)
               {
                  iret = -1;
                  break;
               }
            }

         }

        while(1);    // end iteration loop

         //if(!applied_trop && pDebugStream)
            //*pDebugStream << "Warning - trop correction not applied at time "
               //<< T.printf("%4F %10.3g\n");


            // compute slopes
         Slope = 0.0;
         if (iret == 0)
         {
            for (j=0,i=0; i<Use.size(); i++)
            {
               if (!Use[i])
                  continue;
               for (int k=0; k<4; k++)
               {
                  Slope(i) += G(k,j)*G(k,j);
               }
               Slope(i) = SQRT(Slope(i)*double(n-4)/(1.0-PG(j,j)));
               j++;
            }
         }

         return iret;
      }
      catch(Exception& e)
      {
         GPSTK_RETHROW(e);
      }
   } // end PRSolutionLegacy::AutonomousPRSolution

///-------------------------------------------------------------------------///
///-------------------------------------------------------------------------///

} // namespace gpstk