prsolution.hpp

一个gps小工具包

#pragma ident "$Id: PRSolution.hpp 778 2007-09-25 16:14:58Z ocibu $"



/**
 * @file PRSolution.hpp
 * Autonomous pseudorange navigation solution, including RAIM algorithm
 */
 
#ifndef PRS_POSITION_SOLUTION_HPP
#define PRS_POSITION_SOLUTION_HPP

//============================================================================
//
//  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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//  
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

#include <vector>
#include <ostream>
#include "icd_200_constants.hpp"
#include "DayTime.hpp"
#include "SatID.hpp"
#include "Matrix.hpp"
#include "RinexObsHeader.hpp"
#include "XvtStore.hpp"
#include "TropModel.hpp"

namespace gpstk
{
   /** @defgroup GPSsolutions GPS solution algorithms and Tropospheric models */
   //@{
 
   /** This class defines an interface to routines which compute a position
    * and time solution from pseudorange data, with a data editing algorithm
    * based on Receiver Autonomous Integrity Monitoring (RAIM) concepts.
    * RAIM ref. "A Baseline GPS RAIM Scheme and a Note on the Equivalence of
    * Three RAIM Methods," by R. Grover Brown, Journal of the Institute of
    * Navigation, Vol. 39, No. 3, Fall 1992, pg 301.
    */
   class PRSolution
   {
   public:
         /// Constructor
      PRSolution() throw() : RMSLimit(6.5), SlopeLimit(100.), 
         Algebraic(false), ResidualCriterion(true), ReturnAtOnce(false),
         NSatsReject(-1), Debug(false), pDebugStream(&std::cout),
         MaxNIterations(10),  ConvergenceLimit(3.e-7), Valid(false) {};

      /** Compute a position/time solution, given satellite PRNs and pseudoranges
       *  using a RAIM algorithm.
       * @param Tr          Measured time of reception of the data.
       * @param Satellite   std::vector<SatID> of satellites; on successful
       *                    return, satellites that were excluded by the algorithm
       *                    are marked by a negative 'prn' member.
       * @param Pseudorange std::vector<double> of raw pseudoranges (parallel to
       *                    Satellite), in meters; on successful return,
       *                    contains residuals of fit (m) (for unmarked satellites).
       * @param Eph         gpstk::EphemerisStore to be used in the algorithm.
       *
       * @return Return values:
       *  2  solution is found, but it is not good (RMS residual exceed limits)
       *  1  solution is found, but it is suspect (slope is large)
       *  0  ok
       * -1  algorithm failed to converge
       * -2  singular problem, no solution is possible
       * -3  not enough good data (> 4) to form a (RAIM) solution
       *     (the 4 satellite solution might be returned - check isValid())
       * -4  ephemeris is not found for one or more satellites
       */
      int RAIMCompute(const DayTime& Tr,
                      std::vector<SatID>& Satellite,
                      std::vector<double>& Pseudorange,
                      const XvtStore<SatID>& Eph,
                      TropModel *pTropModel)
         throw(Exception);

         /// Return the status of solution
      bool isValid()
         const throw() { return Valid; }

      // input:

      /// RMS limit - either residual of fit or distance (see ResidualCriterion).
      double RMSLimit;

      /// Slope limit.
      double SlopeLimit;

      /// Use an algebraic (if true) or linearized least squares (if false) algorithm.
      bool Algebraic;

      /** Use a rejection criterion based on RMS residual of fit (true)
       * or RMS distance from an a priori position. If false, member Vector Solution
       * must be defined as this a priori position when RAIMCompute() is called.
       */
      bool ResidualCriterion;

      /** Return as soon as a solution meeting the limit requirements is found
       * (this makes it a non-RAIM algorithm).
       */
      bool ReturnAtOnce;

      /** Maximum number of satellites that may be rejected in the RAIM algorithm;
       * if this = -1, as many as possible will be rejected (RAIM requires at least 5
       * satellites). A (single) non-RAIM solution can be obtained by setting this
       * to 0 before calling RAIMCompute().
       */
      int NSatsReject;

      /// If true, RAIMCompute() will output solution information to *pDebugStream.
      bool Debug;

      /// Pointer to an ostream, default &std::cout; if Debug is true, RAIMCompute()
      /// will print all preliminary solutions to this stream.
      std::ostream *pDebugStream;

      // TD optional: measurement covariance matrix

      /// Maximum number of iterations allowed in the linearized least squares
      /// algorithm.
      int MaxNIterations;

      /// Convergence limit (m): continue iteration loop while RSS change in
      /// solution exceeds this.
      double ConvergenceLimit;

      // output:

      /// flag: output content is valid.
      bool Valid;

      /** Vector<double> containing the computed position solution (ECEF, meter);
       * valid only when isValid() is true.
       */
      Vector<double> Solution;

      /** 4x4 Matrix<double> containing the computed solution covariance (meter);
       * valid only when isValid() is true.
       */
      Matrix<double> Covariance;

      /** Root mean square residual of fit (except when RMSDistanceFlag is set,
       * then RMS distance from apriori 4-position); in meters.
       */
      double RMSResidual;

      /** Slope computed in the RAIM algorithm (largest of all satellite values)
       * for the returned solution, dimensionless ??.
       */
      double MaxSlope;

      /// the actual number of iterations used (linearized least squares algorithm)
      int NIterations;

      /// the RSS change in solution at the end of iterations.
      double Convergence;

      /// the number of good satellites used in the final computation
      int Nsvs;

      /** Compute the satellite position / corrected range matrix (SVP) which is used
       * by AutonomousPRSolution(). SVP is output, dimensioned (N,4) where N is the
       * number of satellites and the length of both Satellite and Pseudorange.
       * Data is ignored whenever Sats[i].id is < 0. NB caller should verify that the
       * number of good entries (Satellite[.] > 0) is > 4 before proceeding.
       * @param Tr          Measured time of reception of the data.
       * @param Sats        std::vector<SatID> of satellites; satellites that are
       *                    to be excluded by the algorithm are marked by a
       *                    negative 'prn' member.
       * @param Pseudorange std::vector<double> of raw pseudoranges (parallel to
       *                    Satellite), in meters
       * @param Eph         gpstk::XvtStore<SatID> to be used in the algorithm.
       * @param SVP         gpstk::Matrix<double> of dimension (N,4), N is the number
       *                    of unmarked satellites in Sats[], on output this
       *                    contains the satellite positions at transmit time, and
       *                    the corrected pseudorange.
       * @param pDebug      pointer to an ostream for debug output, NULL (the default)
       *                    for no debug output.
       * @return Return values:
       *  0  ok
       * -4  ephemeris not found for all the satellites
       */
      static int PrepareAutonomousSolution(const DayTime& Tr,
                                           std::vector<SatID>& Sats,
                                           std::vector<double>& Pseudorange,
                                           const XvtStore<SatID>& Eph,
                                           Matrix<double>& SVP,
                                           std::ostream *pDebug=NULL)
         throw();

      /** Compute a single autonomous pseudorange solution.
       * Input only:
       * @param Tr          Measured time of reception of the data.
       * @param Use         std::vector<bool> of length N, the number of satellites;
       *                    false means do not include that sat. in the computation.
       * @param SVP         Matrix<double> of dimension (N,4). This Matrix must have
       *                    been computed by calling PrepareAutonomousPRSolution().
       * @param Algebraic   bool flag indicating algorithm to use : algebraic (true)
       *                    or linearized least squares (false).
       * @param pTropModel  pointer to a gpstk::TropModel for use within the algorithm
       *
       *   Weight matrix TD...
       *
       * Input and output (for least squares only; ignored if Algebraic==true):
       * @param n_iterate   integer limit on the number of iterations. On output,
       *                    it is the number of iterations actually used.
       * @param converge    double convergence criterion, = RSS change in solution,
       *                    in meters. On output, it is the the final value.
       *
       * Output:  (these will be resized within the function)
       * @param Sol         gpstk::Vector<double> solution (ECEF + time components;
       *                    all in meters)
       * @param Cov         gpstk::Matrix<double> 4X4 covariance matrix (meter*meter)
       * @param Resid       gpstk::Vector<double> post-fit range residuals for each
       *                    satellite (m), the length of this Vector is the number of
       *                    satellites actually used (see Use).
       * @param Slope       gpstk::Vector<double> slope value used in RAIM for each
       *                    good satellite, length N
       * @param pDebug      pointer to an ostream for debug output, NULL (the default)
       *                    for no debug output.
       * @return Return values:
       *  0  ok
       * -1  failed to converge
       * -2  singular problem
       * -3  not enough good data to form a solution (at least 4 satellites required)
       * -4  ephemeris not found for one or more satellites
       */
      static int AutonomousPRSolution(const DayTime& Tr,
                                      const std::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,
                                      std::ostream *pDebug=NULL)
            throw(Exception);

   private:

      /** Matrix, dimensioned Nx4, where N data are input, containing satellite
       * positions at transmit time (0,1,2) and raw pseudorange+clk+relativity (3).
       */
      Matrix<double> SVP;

      /// Save the input solution (for use in rejection when ResidualCriterion is
      /// false).
      Vector<double> APrioriSolution;

      /** Function used internally to handle the details of the Algebraic solution */
      static int AlgebraicSolution(Matrix<double>& A,
                                               Vector<double>& Q,
                                               Vector<double>& X,
                                               Vector<double>& R);

   }; // end class PRSolution

   //@}

} // namespace gpstk

#endif