PRSolution.cpp

Go to the documentation of this file.

#pragma ident "$Id: PRSolution.cpp 3140 2012-06-18 15:03:02Z susancummins $"

//============================================================================
//
//  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 2.1 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
//  
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

#include "MathBase.hpp"
#include "PRSolution.hpp"
#include "GPSEllipsoid.hpp"
#include "Combinations.hpp"
#include "TimeString.hpp"
#include "stl_helpers.hpp"
#include "logstream.hpp"

using namespace std;
using namespace gpstk;

namespace gpstk
{
   const string PRSolution::calfmt = string("%04Y/%02m/%02d %02H:%02M:%02S");
   const string PRSolution::gpsfmt = string("%4F %10.3g");
   const string PRSolution::timfmt = gpsfmt + string(" ") + calfmt;

   ostream& operator<<(ostream& os, const WtdAveStats& was)
      { was.dump(os,was.getMessage()); return os;}
 
   // -------------------------------------------------------------------------
   // Prepare for the autonomous solution by computing direction cosines,
   // corrected pseudoranges and satellite system.
   int PRSolution::PreparePRSolution(const CommonTime& Tr,
                                     vector<SatID>& Sats,
                                     vector<SatID::SatelliteSystem>& Syss,
                                     const vector<double>& Pseudorange,
                                     const XvtStore<SatID> *pEph,
                                     Matrix<double>& SVP) const
      throw()
   {
      int i,j,noeph(0),N,NSVS;
      CommonTime tx;
      Xvt PVT;

      // if necessary, define the SystemIDs vector (but NOT the member data one)
      if(Syss.size() == 0) {
         for(i=0; i<Sats.size(); i++) {
            SatID::SatelliteSystem sys=Sats[i].system;
            if(vectorindex(Syss, sys) == -1)     // not found
               Syss.push_back(sys);              // add it
         }
      }

      // must ignore and mark satellites if system is not found in Syss
      for(N=0,i=0; i<Sats.size(); i++) {
         if(Sats[i].id <= 0) continue;                // already marked
         if(vectorindex(Syss, Sats[i].system) == -1) {
            LOG(DEBUG) << " PRSolution ignores satellite (system) "
               << RinexSatID(Sats[i]) << " at time " << printTime(Tr,timfmt);
            Sats[i].id = -Sats[i].id;                 // don't have system - mark it
            continue;
         }
         ++N;                                         // count good sat
      }

      SVP = Matrix<double>(Sats.size(),4,0.0);        // define the matrix to return
      if(N <= 0) return 0;                            // nothing to do
      NSVS = 0;                                       // count good sats w/ ephem

      // loop over all satellites
      for(i=0; i<Sats.size(); i++) {

         // skip marked satellites
         if(Sats[i].id <= 0) {
            LOG(DEBUG) << " PRSolution ignores marked satellite "
               << RinexSatID(Sats[i]) << " at time " << printTime(Tr,timfmt);
            continue;
         }

         // first estimate of transmit time
         tx = Tr;
         tx -= Pseudorange[i]/C_MPS;
         try {
            PVT = pEph->getXvt(Sats[i], tx);          // get ephemeris range, etc
         }
         catch(InvalidRequest& e) {
            LOG(DEBUG) << "Warning - PRSolution ignores satellite (no ephemeris) "
               << RinexSatID(Sats[i]) << " at time " << printTime(tx,timfmt)
               << " [" << e.getText() << "]";
            Sats[i].id = -::abs(Sats[i].id);
            ++noeph;
            continue;
         }

         // update transmit time and get ephemeris range again
         tx -= PVT.clkbias + PVT.relcorr;
         try {
            PVT = pEph->getXvt(Sats[i], tx);
         }
         catch(InvalidRequest& e) {                   // unnecessary....you'd think!
            LOG(DEBUG) << "Warning - PRSolution ignores satellite (no ephemeris 2) "
               << RinexSatID(Sats[i]) << " at time " << printTime(tx,timfmt)
               << " [" << e.getText() << "]";
            Sats[i].id = -::abs(Sats[i].id);
            ++noeph;
            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);

         LOG(DEBUG) << "SVP: Sat " << RinexSatID(Sats[i])
            << " PR " << fixed << setprecision(3) << Pseudorange[i]
            << " clkbias " << C_MPS*PVT.clkbias
            << " relcorr " << C_MPS*PVT.relcorr;

         // count the good satellites
         NSVS++;
      }

      if(noeph == N) return -4;                       // no ephemeris for any good sat

      return NSVS;
  
   } // end PreparePRPRSolution


   // -------------------------------------------------------------------------
   // Compute a straightforward solution using all the unmarked data.
   // Call PreparePRSolution first.
   int PRSolution::SimplePRSolution(const CommonTime& T,
                                    const vector<SatID>& Sats,
                                    const Matrix<double>& SVP,
                                    const Matrix<double>& invMC,
                                    TropModel *pTropModel,
                                    const int& niterLimit,
                                    const double& convLimit,
                                    const vector<SatID::SatelliteSystem>& Syss,
                                    const Vector<double>& APSol,
                                    Vector<double>& Resids,
                                    Vector<double>& Slopes)
      throw(Exception)
   {
      if(!pTropModel) {
         Exception e("Undefined tropospheric model");
         GPSTK_THROW(e);
      }
      if(Sats.size() != SVP.rows() ||
         (invMC.rows() > 0 && invMC.rows() != Sats.size())) {
         LOG(ERROR) << "Sats has length " << Sats.size();
         LOG(ERROR) << "SVP has dimension " << SVP.rows() << "x" << SVP.cols();
         LOG(ERROR) << "invMC has dimension " << invMC.rows() << "x" << invMC.cols();
         Exception e("Invalid dimensions");
         GPSTK_THROW(e);
      }
      if(Syss.size() == 0) {
         Exception e("Allowed satellite systems input (Syss) is empty!");
         GPSTK_THROW(e);
      }

      int iret(0),i,j,k,n;
      double rho,wt,svxyz[3];
      GPSEllipsoid ellip;

      Valid = false;

      try {
         // -----------------------------------------------------------
         // counts, systems and dimensions
         vector<SatID::SatelliteSystem> mySyss;
         {
            // define the Syss (system IDs) vector, and count good satellites
            vector<SatID::SatelliteSystem> tempSyss;
            for(Nsvs=0,i=0; i<Sats.size(); i++) {
               if(Sats[i].id <= 0)                          // reject marked sats
                  continue;

               SatID::SatelliteSystem sys(Sats[i].system);  // get this system
               if(vectorindex(Syss, sys) == -1)             // reject disallowed sys
                  continue;

               Nsvs++;                                      // count it
               if(vectorindex(tempSyss, sys) == -1)         // add unique system
                  tempSyss.push_back(sys);
            }

            // must sort as in Syss
            for(i=0; i<Syss.size(); i++)
               if(vectorindex(tempSyss, Syss[i]) != -1)
                  mySyss.push_back(Syss[i]);
         }

         // dimension of the solution vector (3 pos + 1 clk/sys)
         const int dim(3 + mySyss.size());

         // require number of good satellites to be >= number unknowns (no RAIM here)
         if(Nsvs < dim) return -3;

         // -----------------------------------------------------------
         // build the measurement covariance matrix
         Matrix<double> iMC;
         if(invMC.rows() > 0) {
            LOG(DEBUG) << "Build inverse MCov";
            iMC = Matrix<double>(Nsvs,Nsvs,0.0);
            for(n=0,i=0; i<Sats.size(); i++) {
               if(Sats[i].id <= 0) continue;
               for(k=0,j=0; j<Sats.size(); j++) {
                  if(Sats[j].id <= 0) continue;
                  iMC(n,k) = invMC(i,j);
                  ++k;
               }
               ++n;
            }
            LOG(DEBUG) << "inv MCov matrix is\n" << fixed << setprecision(4) << iMC;
         }

         // -----------------------------------------------------------
         // define for computation
         Vector<double> CRange(Nsvs),dX(dim);
         Matrix<double> P(Nsvs,dim,0.0),PT,G(dim,Nsvs),PG(Nsvs,Nsvs),Rotation;
         Triple dirCos;
         Xvt SV,RX;

         Solution.resize(dim);
         Covariance.resize(dim,dim);
         Resids.resize(Nsvs);
         Slopes.resize(Nsvs);
         LOG(DEBUG) << " Solution dimension is " << dim << " and Nsvs is " << Nsvs;

         // prepare for iteration loop
         // iterate at least twice so that trop model gets evaluated
         int n_iterate(0), niter_limit(niterLimit < 2 ? 2 : niterLimit);
         double converge(0.0);

         // start with apriori
         if(APSol.size() == 0) Solution = Vector<double>(dim,0.0);
         else {
            // TD what if size is wrong?
            Solution = APSol;

            LOG(DEBUG) << " apriori solution (" << Solution.size() << ") is [ "
               << fixed << setprecision(3) << Solution << " ]";
         }

         // -----------------------------------------------------------
         // iteration loop
         do {
            TropFlag = false;       // true means the trop corr was NOT applied

            // current estimate of position solution
            RX.x = Triple(Solution(0),Solution(1),Solution(2));

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

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

               // correct for earth rotation
               wt = ellip.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);

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

               // direction cosines
               dirCos[0] = (Solution(0)-svxyz[0])/rho;
               dirCos[1] = (Solution(1)-svxyz[1])/rho;
               dirCos[2] = (Solution(2)-svxyz[2])/rho;

               // ------------ data
               // corrected pseudorange (m) minus geometric range
               CRange(n) = SVP(i,3) - rho;

               // correct for troposphere and PCOs (but not on the first iteration)
               if(n_iterate > 0) {
                  SV.x = Triple(svxyz[0],svxyz[1],svxyz[2]);
                  Position R,S;
                  R.setECEF(RX.x[0],RX.x[1],RX.x[2]);
                  S.setECEF(SV.x[0],SV.x[1],SV.x[2]);

                  // trop
                  double tc(R.getHeight());  // tc is a dummy here
                  // must test R for reasonableness to avoid corrupting TropModel
                  if(R.elevation(S) < 0.0 || tc > 100000.0 || tc < -1000.0) {
                     tc = 0.0;
                     TropFlag = true;        // true means failed to apply trop corr
                  }
                  else
                     tc = pTropModel->correction(R,S,T);    // pTropModel not const

                  CRange(n) -= tc;
                  LOG(DEBUG) << "Trop " << i << " " << Sats[i] << " "
                     << fixed << setprecision(3) << tc;

               }  // end if n_iterate > 0

               // get the index, for this clock, in the solution vector
               j = 3 + vectorindex(mySyss, Sats[i].system); // Solution ~ X,Y,Z,clks

               // find the clock for the sat's system
               const double clk(Solution(j));
               LOG(DEBUG) << "Clock is (" << j << ") " << clk;

               // data vector: corrected range residual
               Resids(n) = CRange(n) - clk;

               // ------------ least squares
               // partials matrix
               P(n,0) = dirCos[0];           // x direction cosine
               P(n,1) = dirCos[1];           // y direction cosine
               P(n,2) = dirCos[2];           // z direction cosine
               P(n,j) = 1.0;                 // clock

               // ------------ increment index
               // n is index and number of good satellites - also used for Slope
               n++;

            }  // end loop over satellites

            if(n != Nsvs) {
               Exception e("Counting error after satellite loop");
               GPSTK_THROW(e);
            }

            LOG(DEBUG) << "Partials (" << P.rows() << "x" << P.cols() << ")\n"
               << fixed << setprecision(4) << P;
            LOG(DEBUG) << "Resids (" << Resids.size() << ") "
               << fixed << setprecision(3) << Resids;

            // ------------------------------------------------------
            // compute information matrix (inverse covariance) and generalized inverse
            PT = transpose(P);

            // weight matrix = measurement covariance inverse
            if(invMC.rows() > 0) Covariance = PT * iMC * P;
            else                 Covariance = PT * P;
            LOG(DEBUG) << "Computed inverse cov";

            // invert using SVD
            try {
               Covariance = inverseSVD(Covariance);
            }
            catch(SingularMatrixException& sme) { return -2; }

            // generalized inverse
            if(invMC.rows() > 0) G = Covariance * PT * iMC;
            else                 G = Covariance * PT;

            // PG is used for Slope computation
            PG = P * G;
            LOG(DEBUG) << "Computed PG";

            n_iterate++;                        // increment number iterations

            // ------------------------------------------------------
            // compute solution
            dX = G * Resids;
            LOG(DEBUG) << "Computed dX(" << dX.size() << ")";
            Solution += dX;

            // ------------------------------------------------------
            // test for convergence
            converge = norm(dX);
            if(n_iterate > 1 && converge < convLimit) {              // success: quit
               iret = 0;
               break;
            }
            if(n_iterate >= niterLimit || converge > 1.e10) {        // failure: quit
               iret = -1;
               break;
            }

         } while(1);    // end iteration loop
         LOG(DEBUG) << "Out of iteration loop";

         if(TropFlag) LOG(DEBUG) << "Trop correction not applied at time "
                                 << printTime(T,timfmt);

         // compute slopes
         MaxSlope = 0.0;
         Slopes = 0.0;
         if(iret == 0) for(j=0,i=0; i<Sats.size(); i++) {
            if(Sats[i].id <= 0) continue;

            for(int k=0; k<dim; k++) Slopes(j) += G(k,j)*G(k,j);  // TD why k<4? dim?
            Slopes(j) = SQRT(Slopes(j)*double(n-dim)/(1.0-PG(j,j)));

            if(Slopes(j) > MaxSlope) MaxSlope = Slopes(j);

            j++;
         }

         // compute pre-fit residuals
         if(APSol.size() != 0)
            PreFitResidual = P*(Solution-APSol) - Resids;

         // Compute RMS residual (member)
         RMSResidual = RMS(Resids);

         // Find the maximum slope (member)
         MaxSlope = 0.0;
         for(i=0; i<Slopes.size(); i++)
            if(Slopes(i) > MaxSlope)
               MaxSlope = Slopes[i];

         // save to member data
         currTime = T;
         SatelliteIDs = Sats;
         SystemIDs = mySyss;
         invMeasCov = iMC;
         Partials = P;
         NIterations = n_iterate;
         Convergence = converge;
         Valid = true;

         return iret;
      
      } catch(Exception& e) { GPSTK_RETHROW(e); }

   } // end PRSolution::SimplePRSolution


   // -------------------------------------------------------------------------
   // Compute a solution using RAIM.
   int PRSolution::RAIMCompute(const CommonTime& Tr,
                               vector<SatID>& Sats,
                               vector<SatID::SatelliteSystem>& Syss,
                               const vector<double>& Pseudorange,
                               const Matrix<double>& invMC,
                               const XvtStore<SatID> *pEph,
                               TropModel *pTropModel)
      throw(Exception)
   {
      try {
         int iret,i,j,N;
         vector<int> GoodIndexes;
         // use these to save the 'best' solution within the loop.
         // BestRMS marks the 'Best' set as unused.
         bool BestTropFlag(false);
         int BestNIter(0),BestIret(-5);
         double BestRMS(-1.0),BestSL(0.0),BestConv(0.0);
         Vector<double> BestSol(3,0.0),BestPFR;
         vector<SatID> BestSats,SaveSats;
         Matrix<double> SVP,BestCov,BestInvMCov,BestPartials;
         vector<SatID::SatelliteSystem> BestSyss;

         // initialize
         Valid = false;
         currTime = Tr;
         TropFlag = SlopeFlag = RMSFlag = false;

         // ----------------------------------------------------------------
         // fill the SVP matrix, and use it for every solution
         // NB this routine will: define Syss if it is empty,
         //    reject sat systems not found in Syss, and
         //    reject sats without ephemeris.
         N = PreparePRSolution(Tr, Sats, Syss, Pseudorange, pEph, SVP);

         if(LOGlevel >= ConfigureLOG::Level("DEBUG")) {
            ostringstream oss;
            oss << "RAIMCompute: after PrepareAS(): Satellites:";
            for(i=0; i<Sats.size(); i++) {
               RinexSatID rs(::abs(Sats[i].id), Sats[i].system);
               oss << " " << (Sats[i].id < 0 ? "-" : "") << rs;
            }
            oss << endl;

            oss << " SVP matrix("
               << SVP.rows() << "," << SVP.cols() << ")" << endl;
            oss << fixed << setw(16) << setprecision(5) << SVP;

            LOG(DEBUG) << oss.str();
         }

         // return is >=0(number of good sats) or -4(no ephemeris)
         if(N <= 0) return -4;

         // ----------------------------------------------------------------
         // Build GoodIndexes based on Sats; save Sats as SaveSats.
         // Sats is used to mark good sats (id > 0) and those to ignore (id <= 0).
         // SaveSats saves the original so it can be reused for each RAIM solution.
         // Let GoodIndexes be all the indexes of Sats that are good:
         // Sats[GoodIndexes[.]].id > 0 by definition.
         SaveSats = Sats;
         for(i=0; i<Sats.size(); i++)
            if(Sats[i].id > 0)
               GoodIndexes.push_back(i);

         // dump good satellites for debug
         if(LOGlevel >= ConfigureLOG::Level("DEBUG")) {
            ostringstream oss;
            oss << " Good satellites (" << N << ") are:";
            for(i=0; i<GoodIndexes.size(); i++)
               oss << " " << RinexSatID(Sats[GoodIndexes[i]]);
            LOG(DEBUG) << oss.str();
         }

         // ----------------------------------------------------------------
         // now compute the solution, first with all the data. If this fails,
         // RAIM: 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;
         // Resids stores the post-fit data residuals.
         Vector<double> Resids;

         // stage is the number of satellites to reject.
         int 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
               Sats = SaveSats;
               for(i=0; i<GoodIndexes.size(); i++)
                  if(Combo.isSelected(i))
                     Sats[GoodIndexes[i]].id = -::abs(Sats[GoodIndexes[i]].id);

               if(LOGlevel >= ConfigureLOG::Level("DEBUG")) {
                  ostringstream oss;
                  oss << " RAIM: Try the combo ";
                  for(i=0; i<Sats.size(); i++) {
                     RinexSatID rs(::abs(Sats[i].id), Sats[i].system);
                     oss << " " << (Sats[i].id < 0 ? "-" : " ") << rs;
                  }
                  LOG(DEBUG) << oss.str();
               }

               // define apriroi solution    // TD clocks??
               Vector<double> APSol(4,0.0);
               if(hasMemory) APSol = memory.APSolution;

               // ----------------------------------------------------------------
               // 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
               //       -4  no ephemeris
               iret = SimplePRSolution(Tr, Sats, SVP, invMC, pTropModel,
                       MaxNIterations, ConvergenceLimit, Syss, APSol, Resids, Slopes);

               LOG(DEBUG) << " RAIM: SimplePRS returns " << iret;
               if(iret <= 0 && iret > BestIret) BestIret = iret;

               // ----------------------------------------------------------------
               // if error, either quit or continue with next combo (SPS sets Valid F)
               if(iret < 0) {
                  if(iret == -1) {
                     LOG(DEBUG) << " SPS: Failed to converge - go on";
                     continue;
                  }
                  else if(iret == -2) {
                     LOG(DEBUG) << " SPS: singular - go on";
                     continue;
                  }
                  else if(iret == -3) {
                     LOG(DEBUG) <<" SPS: not enough satellites: quit";
                     break;
                  }
                  else if(iret == -4) {
                     LOG(DEBUG) <<" SPS: no ephemeris: quit";
                     break;
                  }
               }

               // ----------------------------------------------------------------
               // print solution with diagnostic information
               LOG(DEBUG) << outputString(string("RPS"),iret);

               // do again for residuals
               // if memory exists, output residuals
               //if(hasMemory) LOG(DEBUG) << outputString(string("RAP"), -99,
                     //(Solution-memory.APSolution));

               // deal with the results of SimplePRSolution()
               // save 'best' solution for later
               if(BestRMS < 0.0 || RMSResidual < BestRMS) {
                  BestRMS = RMSResidual;
                  BestSol = Solution;
                  BestSats = SatelliteIDs;
                  BestSyss = SystemIDs;
                  BestSL = MaxSlope;
                  BestConv = Convergence;
                  BestNIter = NIterations;
                  BestCov = Covariance;
                  BestInvMCov = invMeasCov;
                  BestPartials = Partials;
                  BestPFR = PreFitResidual;
                  BestTropFlag = TropFlag;
                  BestIret = iret;
               }

               if(stage==0 && RMSResidual < RMSLimit)
                  break;

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

            // end of the stage
            if(BestRMS > 0.0 && BestRMS < RMSLimit) {          // success
               LOG(DEBUG) << " RAIM: Success in the RAIM loop";
               iret = 0;
               break;
            }

            // go to next stage
            stage++;

            // but not if too many are being rejected
            if(NSatsReject > -1 && stage > NSatsReject) {
               LOG(DEBUG) << " RAIM: break before stage " << stage
                  << " due to NSatsReject " << NSatsReject;
               break;
            }

            // already broke out of the combo loop...
            if(iret == -3 || iret == -4) {
               LOG(DEBUG) << " RAIM: break before stage " << stage
                  << "; " << (iret==-3 ? "too few sats" : "no ephemeris");
               break;
            }

            LOG(DEBUG) << " RAIM: go to stage " << stage;

         } while(1);    // end loop over stages

         // ----------------------------------------------------------------
         // copy out the best solution and return
         if(iret >= 0) {
            Sats = SatelliteIDs = BestSats;
            SystemIDs = BestSyss;
            Solution = BestSol;
            Covariance = BestCov;
            invMeasCov = BestInvMCov;
            Partials = BestPartials;
            PreFitResidual = BestPFR;
            Convergence = BestConv;
            NIterations = BestNIter;
            RMSResidual = BestRMS;
            MaxSlope = BestSL;
            TropFlag = BestTropFlag;
            iret = BestIret;

            // must add zeros to state, covariance and partials if these don't match
            if(Syss.size() != BestSyss.size()) {
               N = 3+Syss.size();
               Solution = Vector<double>(N,0.0);
               Covariance = Matrix<double>(N,N,0.0);
               Partials = Matrix<double>(BestPartials.rows(),N,0.0);

               // build a little map of indexes; note systems are sorted alike
               vector<int> indexes;
               for(j=0,i=0; i<Syss.size(); i++)
                  indexes.push_back(Syss[i] == BestSyss[j] ? j++ : -1);

               // copy over position part
               for(i=0; i<3; i++) {
                  Solution(i) = BestSol(i);
                  for(j=0; j<Partials.rows(); j++) Partials(i,j) = BestPartials(i,j);
                  for(j=0; j<3; j++) Covariance(i,j) = BestCov(i,j);
                  for(j=0; j<indexes.size(); j++) {
                     Covariance(i,3+j)=(indexes[j]==-1 ? 0.:BestCov(i,3+indexes[j]));
                     Covariance(3+j,i)=(indexes[j]==-1 ? 0.:BestCov(3+indexes[j],i));
                  }
               }

               // copy the clock part, inserting zeros where needed
               for(j=0; j<indexes.size(); j++) {
                  int n(indexes[j]);
                  bool ok(n != -1);
                  Solution(3+j) = (ok ? BestSol(3+n) : 0.0);
                  for(i=0; i<Partials.rows(); i++)
                     Partials(i,3+j) = (ok ? BestPartials(i,3+n) : 0.0);
                  for(i=0; i<indexes.size(); i++) {
                     Covariance(3+i,3+j) = (ok ? BestCov(3+i,3+n) : 0.0);
                     Covariance(3+j,3+i) = (ok ? BestCov(3+n,3+i) : 0.0);
                  }
               }
            }

            // compute number of satellites actually used
            for(Nsvs=0,i=0; i<SatelliteIDs.size(); i++)
               if(SatelliteIDs[i].id > 0)
                  Nsvs++;
         }

         if(iret==0) {
            if(BestSL > SlopeLimit) { iret = 1; SlopeFlag = true; }
            if(BestSL > SlopeLimit/2.0 && Nsvs == 5) { iret = 1; SlopeFlag = true; }
            if(BestRMS >= RMSLimit) { iret = 1; RMSFlag = true; }
            if(TropFlag) iret = 1;
            Valid = true;

            // compute DOPs
            DOPCompute();

            // update memory
            if(hasMemory)
               memory.add(Solution, Covariance, PreFitResidual, Partials, invMeasCov);
         }

         LOG(DEBUG) << " RAIM exit with ret value " << iret
                     << " and Valid " << (Valid ? "T":"F");

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


   // -------------------------------------------------------------------------
   int PRSolution::DOPCompute(void) throw(Exception)
   {
      try {
         Matrix<double> PTP(transpose(Partials)*Partials);
         Matrix<double> Cov(inverseLUD(PTP));

         PDOP = SQRT(Cov(0,0)+Cov(1,1)+Cov(2,2));

         TDOP = 0.0;
         for(int i=3; i<Cov.rows(); i++) TDOP += Cov(i,i);
         TDOP = SQRT(TDOP);

         GDOP = RSS(PDOP,TDOP);

         return 0;
      }
      catch(Exception& e) { GPSTK_RETHROW(e); }
   }

   // -------------------------------------------------------------------------
   // conveniences for printing the solutions
   string PRSolution::outputValidString(int iret) throw()
   {
      ostringstream oss;
      if(iret != -99) {
         oss << " (" << iret << " " << errorCodeString(iret);
         if(iret==1) {
            oss << " due to";
            if(RMSFlag) oss << " large RMS residual";
            if(SlopeFlag) oss << " large slope";
            if(TropFlag) oss << " missed trop. corr.";
         }
         oss << ") " << (Valid ? "" : "N") << "V";
      }
      return oss.str();
   }  // end PRSolution::outputValidString

   string PRSolution::outputNAVString(string tag, int iret, const Vector<double>& Vec)
      throw()
   {
      ostringstream oss;

      // tag NAV timetag X Y Z clks endtag
      oss << tag << " NAV " << printTime(currTime,gpsfmt)
         << fixed << setprecision(6)
         << " " << setw(16) << (&Vec==&PRSNullVector ? Solution(0) : Vec(0))
         << " " << setw(16) << (&Vec==&PRSNullVector ? Solution(1) : Vec(1))
         << " " << setw(16) << (&Vec==&PRSNullVector ? Solution(2) : Vec(2))
         << fixed << setprecision(3);
      for(int i=0; i<SystemIDs.size(); i++) {
         RinexSatID sat(1,SystemIDs[i]);
         oss << " " << sat.systemString3() << " " << setw(11) << Solution(3+i);
      }
      oss << outputValidString(iret);

      return oss.str();
   }  // end PRSolution::outputNAVString

   string PRSolution::outputPOSString(string tag, int iret, const Vector<double>& Vec)
      throw()
   {
      ostringstream oss;

      // tag POS timetag X Y Z endtag
      oss << tag << " POS " << printTime(currTime,gpsfmt)
         << fixed << setprecision(6)
         << " " << setw(16) << (&Vec==&PRSNullVector ? Solution(0) : Vec(0))
         << " " << setw(16) << (&Vec==&PRSNullVector ? Solution(1) : Vec(1))
         << " " << setw(16) << (&Vec==&PRSNullVector ? Solution(2) : Vec(2))
         << outputValidString(iret);

      return oss.str();
   }  // end PRSolution::outputPOSString

   string PRSolution::outputCLKString(string tag, int iret) throw()
   {
      ostringstream oss;

      // tag CLK timetag SYS clk [SYS clk SYS clk ...] endtag
      oss << tag << " CLK " << printTime(currTime,gpsfmt)
         << fixed << setprecision(3);
      for(int i=0; i<SystemIDs.size(); i++) {
         RinexSatID sat(1,SystemIDs[i]);
         oss << " " << sat.systemString3() << " " << setw(11) << Solution(3+i);
      }
      oss << outputValidString(iret);

      return oss.str();
   }  // end PRSolution::outputCLKString

   // NB must call DOPCompute() if SimplePRSol() only was called.
   string PRSolution::outputRMSString(string tag, int iret) throw()
   {
      ostringstream oss;

      // tag RMS timetag NSVdrop Nsv RMS Slope niter conv
      oss << tag << " RMS " << printTime(currTime,gpsfmt)
         << " " << setw(2) << SatelliteIDs.size()-Nsvs
         << " " << setw(2) << Nsvs
         << fixed << setprecision(3)
         << " " << setw(8) << RMSResidual
         << setprecision(2)
         << " " << setw(7) << TDOP
         << " " << setw(7) << PDOP
         << " " << setw(7) << GDOP
         << setprecision(1)
         << " " << setw(5) << MaxSlope
         << " " << setw(2) << NIterations
         << scientific << setprecision(2)
         << " " << setw(8) << Convergence;
      for(int i=0; i<SatelliteIDs.size(); i++) {
         RinexSatID rs(::abs(SatelliteIDs[i].id), SatelliteIDs[i].system);
         oss << " " << (SatelliteIDs[i].id < 0 ? "-" : " ") << rs;
      }
      oss << outputValidString(iret);

      return oss.str();
   }  // end PRSolution::outputRMSString

   string PRSolution::outputString(string tag, int iret, const Vector<double>& Vec)
      throw()
   {
      ostringstream oss;
      oss << outputNAVString(tag,iret,Vec) << endl;
      oss << outputRMSString(tag,iret);

      return oss.str();
   }  // end PRSolution::outputString

   string PRSolution::errorCodeString(int iret) throw()
   {
      string str("unknown");
      if(iret == 1) str = string("ok but perhaps degraded");
      else if(iret== 0) str = string("ok");
      else if(iret==-1) str = string("failed to converge");
      else if(iret==-2) str = string("singular solution");
      else if(iret==-3) str = string("not enough satellites");
      else if(iret==-4) str = string("not any ephemeris");
      return str;
   }

   string PRSolution::configString(string tag) throw()
   {
      ostringstream oss;
      oss << tag << " " << printTime(currTime,timfmt)
         << (Valid ? "":" not") << " valid,"
         << (Mixed ? "":" not") << " mixed"
         << "\n   iterations " << MaxNIterations
         << "\n   convergence " << scientific << setprecision(2) << ConvergenceLimit
         << "\n   RMS residual limit " << fixed << RMSLimit
         << "\n   RAIM slope limit " << fixed << SlopeLimit << " meters"
         << "\n   Maximum number of satellites to reject is " << NSatsReject
         << "\n   Memory information IS " << (hasMemory ? "":"NOT ") << "stored"
         ;

      // TD output memory information
      //if(APrioriSol.size() >= 4) oss
      //   << "\n   APriori is " << fixed << setprecision(3) << APrioriSol(0)
      //      << " " << APrioriSol(1) << " " << APrioriSol(2) << " " << APrioriSol(3);
      //else oss << "\n   APriori is undefined";

      return oss.str();
   }

   const Vector<double> PRSolution::PRSNullVector;

} // namespace gpstk

---