PRSolution.hpp

Go to the documentation of this file.

//==============================================================================
//
//  This file is part of GNSSTk, the ARL:UT GNSS Toolkit.
//
//  The GNSSTk 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 GNSSTk 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 GNSSTk; 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-2022, 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.
//
//==============================================================================
 
 
#ifndef PRS_POSITION_SOLUTION_HPP
#define PRS_POSITION_SOLUTION_HPP
 
#include <vector>
#include <ostream>
#include "gnsstk_export.h"
#include "stl_helpers.hpp"
#include "GNSSconstants.hpp"
#include "CommonTime.hpp"
#include "RinexSatID.hpp"
#include "Stats.hpp"
#include "Matrix.hpp"
#include "Namelist.hpp"
#include "NavLibrary.hpp"
#include "TropModel.hpp"
 
namespace gnsstk
{
 
   class WtdAveStats {
   private:
      unsigned int N;
        //unused
      //double APV;
      std::string msg;
      std::string lab[3];
      Stats<double> S[3];
      Matrix<double> sumInfo;
      Vector<double> sumInfoState,Sbias;
 
   public:
 
      // ctor
      WtdAveStats(void)
      {
         reset();
         lab[0]="ECEF_X";  lab[1]="ECEF_Y"; lab[2]="ECEF_Z";
      }
 
      void setMessage(std::string m) { msg = m; }
      std::string getMessage(void) const { return msg; }
 
      void setLabels(std::string lab1, std::string lab2, std::string lab3)
         { lab[0]=lab1; lab[1]=lab2; lab[2]=lab3; }
 
      Vector<double> getSol(void) const
      {
         return (getCov()*sumInfoState + Sbias);
      }
 
      Matrix<double> getCov(void) const { return inverseSVD(sumInfo); }
 
      Matrix<double> getInfo(void) const { return sumInfo; }
 
      int getN(void) const { return N; }
 
      void reset(void)
      {
         N = 0;
         sumInfo = Matrix<double>();
         sumInfoState = Vector<double>();
         Sbias = Vector<double>(3);
         S[0].Reset();
         S[1].Reset();
         S[2].Reset();
      }
 
      // add to statistics, and to weighted average solution and covariance
      void add(const Vector<double>& Sol, const Matrix<double>& Cov)
      {
         try {
            // add to the statistics
            for(unsigned int i=0; i<3; i++) {
               if(N==0) Sbias(i) = Sol(i);
               S[i].Add(Sol(i)-Sbias(i));
            }
 
            // NB do NOT include clock(s); this can ruin the position average
            Vector<double> Sol3(Sol);
            Sol3.resize(3);               // assumes position states come first
            Sol3 = Sol3 - Sbias;
            Matrix<double> Cov3(Cov,0,0,3,3);
 
            // information matrix (position only)
            Matrix<double> Info(inverseSVD(Cov3));
            if(N == 0) {                  // first call: dimension and set to zero
               sumInfo = Matrix<double>(3,3,0.0);
               sumInfoState = Vector<double>(3,0.0);
            }
 
            // add to the total information
            sumInfo += Info;
            sumInfoState += Info * Sol3;
            ++N;
         }
         catch(Exception& e) { GNSSTK_RETHROW(e); }
      }
 
      // dump statistics and weighted average
      void dump(std::ostream& os, std::string msg="") const
      {
         try {
            os << "Simple statistics on " << msg << std::endl
               << std::fixed << std::setprecision(3);
            if(N > 0) {
               os << "  " << lab[0] << " N: " << S[0].N()
                  << std::fixed << std::setprecision(4)
                  << " Ave: " << S[0].Average()+Sbias[0]
                  << " Std: " << S[0].StdDev()
                  << " Min: " << S[0].Minimum()+Sbias[0]
                  << " Max: " << S[0].Maximum()+Sbias[0]
                  << std::endl;
               os << "  " << lab[1] << " N: " << S[1].N()
                  << std::fixed << std::setprecision(4)
                  << " Ave: " << S[1].Average()+Sbias[1]
                  << " Std: " << S[1].StdDev()
                  << " Min: " << S[1].Minimum()+Sbias[1]
                  << " Max: " << S[1].Maximum()+Sbias[1]
                  << std::endl;
               os << "  " << lab[2] << " N: " << S[2].N()
                  << std::fixed << std::setprecision(4)
                  << " Ave: " << S[2].Average()+Sbias[2]
                  << " Std: " << S[2].StdDev()
                  << " Min: " << S[2].Minimum()+Sbias[2]
                  << " Max: " << S[2].Maximum()+Sbias[2]
                  << std::endl;
 
               os << "Weighted average " << msg << std::endl;
               Matrix<double> Cov(inverseSVD(sumInfo));
               Vector<double> Sol(Cov * sumInfoState + Sbias);
               os << std::setw(14) << std::setprecision(4) << Sol << "    " << N;
            }
            else os << " No data!";
         }
         catch(Exception& e) { GNSSTK_RETHROW(e); }
      }
 
      friend std::ostream& operator<<(std::ostream& s, const WtdAveStats& as);
 
   }; // end class WtdAveStats
 
 
   class PRSolution
   {
   public:
       PRSolution() : RMSLimit(6.5),
                      SlopeLimit(1000.),
                      NSatsReject(-1),
                      MaxNIterations(10),
                      ConvergenceLimit(3.e-7),
                      hasMemory(true),
                      fixedAPriori(false),
                      nsol(0), ndata(0), APV(0.0),
                      Valid(false)
         {
            was.reset();
            APSolution = Vector<double>(4,0.0);
         }
 
      bool isValid() const { return Valid; }
 
      // input parameters: -------------------------------------------------
 
      double RMSLimit;
 
      double SlopeLimit;
 
      int NSatsReject;
 
      int MaxNIterations;
 
      double ConvergenceLimit;
 
      std::vector<SatelliteSystem> allowedGNSS;
 
      bool hasMemory;
 
      // input and output: -------------------------------------------------
 
      std::vector<SatID> SatelliteIDs;
 
      // output: -------------------------------------------------
 
      Vector<double> Solution;
 
      Matrix<double> Covariance;
 
      Matrix<double> invMeasCov;
 
      Matrix<double> Partials;
 
      std::vector<SatelliteSystem> dataGNSS;
 
      WtdAveStats was;
      double APV;
      int ndata,nsol,ndof;
      bool fixedAPriori;
      Triple fixedAPrioriPos;
      Vector<double> APSolution;
 
 
      Vector<double> PreFitResidual;
 
      double RMSResidual;
 
      double MaxSlope;
 
      double TDOP,PDOP,GDOP;
 
      int NIterations;
 
      double Convergence;
 
      int Nsvs;
 
      bool TropFlag;
 
      bool RMSFlag, SlopeFlag;
 
      // member functions -------------------------------------------
 
      int PreparePRSolution(const CommonTime& Tr,
                            std::vector<SatID>& Sats,
                            const std::vector<double>& Pseudorange,
                            NavLibrary& eph,
                            Matrix<double>& SVP,
                            NavSearchOrder order = NavSearchOrder::User) const;
 
      int SimplePRSolution(const CommonTime& Tr,
                           const std::vector<SatID>& Sats,
                           const Matrix<double>& SVP,
                           const Matrix<double>& invMC,
                           TropModel *pTropModel,
                           const int& niterLimit,
                           const double& convLimit,
                           Vector<double>& Resids,
                           Vector<double>& Slopes);
 
      int RAIMComputeUnweighted(const CommonTime& Tr,
                                std::vector<SatID>& Satellites,
                                const std::vector<double>& Pseudorange,
                                NavLibrary& eph,
                                TropModel *pTropModel,
                                NavSearchOrder order = NavSearchOrder::User);
 
      int RAIMCompute(const CommonTime& Tr,
                      std::vector<SatID>& Satellites,
                      const std::vector<double>& Pseudorange,
                      const Matrix<double>& invMC,
                      NavLibrary& eph,
                      TropModel *pTropModel,
                      NavSearchOrder order = NavSearchOrder::User);
 
      int DOPCompute(void);
 
      // output -----------------------------------------------------
      std::string outputPOSString(std::string tag, int iret=-99,
                                    const Vector<double>& Vec=PRSNullVector);
 
      std::string outputCLKString(std::string tag, int iret=-99);
 
      std::string outputNAVString(std::string tag, int iret=-99,
                                    const Vector<double>& Vec=PRSNullVector);
 
      std::string outputRMSString(std::string tag, int iret=-99);
      std::string outputValidString(int iret=-99);
 
      std::string outputString(std::string tag, int iret=-99,
                               const Vector<double>& Vec=PRSNullVector);
      std::string outputStringHeader(std::string tag)
         { return outputString(tag,-999); }
 
      std::string errorCodeString(int iret);
 
      std::string configString(std::string tag);
 
      // ------------------------------------------------------------
      void fixAPSolution(const double& X, const double& Y, const double& Z)
      {
         //fixedAPriori = true;   //TD user input
 
         fixedAPrioriPos[0] = X;
         fixedAPrioriPos[1] = Y;
         fixedAPrioriPos[2] = Z;
 
         if(hasMemory) {
            APSolution = Vector<double>(3+allowedGNSS.size(),0.0);
            for(unsigned int i=0; i<3; i++)
               APSolution(i) = fixedAPrioriPos[i];
         }
      }
 
      inline double getAPV(void) {
         if(ndof > 0) return APV/ndof;
         return 0.0;
      }
 
      // dump solution, statistics and weighted average
      void dumpSolution(std::ostream& os, std::string msg="PRS")
      {
         try {
            was.setMessage(msg);
            os << was << std::endl;
 
            Matrix<double> Cov(was.getCov());
            double sig(ndof > 0 ? ::sqrt(APV/ndof) : 0.0);
            if(ndof > 0)               // scale covariance
            {
               for(size_t i=0; i<Cov.rows(); i++)
               {
                  for(size_t j=i; j<Cov.cols(); j++)
                     Cov(i,j) = Cov(j,i) = Cov(i,j)*sig;
               }
            }
            // print cov as labeled matrix
            Namelist NL;
            NL += "ECEF_X"; NL += "ECEF_Y"; NL += "ECEF_Z";
            LabeledMatrix LM(NL,Cov);
            LM.scientific().setprecision(3).setw(14).symmetric(true);
 
            os << "Covariance: " << msg << std::endl << LM << std::endl;
            os << "APV: " << msg << std::fixed << std::setprecision(3)
               << " sigma = " << sig << " meters with "
               << ndof << " degrees of freedom.\n";
         }
         catch(Exception& e) { GNSSTK_RETHROW(e); }
      }
 
      void updateAPSolution(const Vector<double>& Sol)
      {
         int k;
         unsigned int i;
 
         // first call
         if(APSolution.size() == 0) {
            APSolution = Vector<double>(3+allowedGNSS.size(),0.0);
            for(i=0; i<3; i++)
               APSolution(i) = fixedAPrioriPos[i];
         }
 
         // must expand Sol to have all allowed clocks
         Vector<double> S(3+allowedGNSS.size(),0.0);
         for(i=0; i<3; i++)
            S(i) = (fixedAPriori ? fixedAPrioriPos[i] : Sol(i));
         for(i=0; i<allowedGNSS.size(); i++) {
            k = vectorindex(dataGNSS,allowedGNSS[i]);
            S(3+i) = (k == -1 ? APSolution[3+i] : Sol[3+k]);
         }
 
         APSolution = S;
 
         //APSolution = Sol;
         //if(fixedAPriori)
         //   for(int i=0; i<3; i++) APSolution(i) = fixedAPrioriPos[i];
      }
 
      void addToMemory(const Vector<double>& Sol, const Matrix<double>& Cov,
               const Vector<double>& PreFitResid, const Matrix<double>& Partials,
               const Matrix<double>& invMeasCov)
      {
         was.add(Sol, Cov);
 
         // first solution: apriori solution has no clock, so PFR bad
         if(was.getN() == 1) return;
 
         try {
            // consider only the XYZ states, ignore clocks
            Matrix<double> Part(Partials,0,0,Partials.rows(),3);
            Matrix<double> invMC(invMeasCov);
            if(invMC.rows() == 0) {
               invMC=Matrix<double>(Part.rows(),Part.rows());
               ident(invMC);
            }
            Matrix<double> sumInfo(was.getInfo());
            Matrix<double> Ginv(Part*sumInfo*transpose(Part) + invMC);
            Matrix<double> G(inverseSVD(Ginv));
            Vector<double> Gpfr(G*PreFitResid);
            APV += dot(PreFitResid,Gpfr);
            ndata += PreFitResid.size();
            ndof = ndata-sumInfo.rows();
         }
         catch(Exception& e) {
            e.addText("APV failed.");
            GNSSTK_RETHROW(e);
         }
      }
 
   private:
 
      bool Valid;
 
      CommonTime currTime;
 
      static const std::string calfmt,gpsfmt,timfmt;
 
      GNSSTK_EXPORT static const Vector<double> PRSNullVector;
 
   }; // end class PRSolution
 
 
} // namespace gnsstk
 
#endif