Esempio n. 1
0
      /* Method to set an a priori position of receiver using
       * Bancroft's method.
       *
       * @param Tr            Time of observation
       * @param Satellite     std::vector of satellites in view
       * @param Pseudorange   std::vector of pseudoranges measured from
       *                      rover station to satellites
       * @param Eph           Satellites Ephemeris
       *
       * @return
       *  0 if OK
       *  -1 if problems arose
       */
   int ModeledPR::Prepare( const DayTime& Tr,
                           std::vector<SatID>& Satellite,
                           std::vector<double>& Pseudorange,
                           const XvtStore<SatID>& Eph )
   {

      Matrix<double> SVP;
      Bancroft Ban;
      Vector<double> vPos;
      PRSolution raimObj;

      try
      {
         raimObj.PrepareAutonomousSolution( Tr,
                                            Satellite,
                                            Pseudorange,
                                            Eph,
                                            SVP );

         if( Ban.Compute(SVP, vPos) < 0 )
         {
            return -1;
         }
      }
      catch(...)
      {
         return -1;
      }

      return Prepare(vPos(0), vPos(1), vPos(2));

   }  // End of method 'ModeledPR::Prepare()'
Esempio n. 2
0
int main(int argc, char *argv[])
{
   // Declaration of objects for storing ephemerides and handling RAIM
   GPSEphemerisStore bcestore;
   PRSolution raimSolver;

   // Object for void-type tropospheric model (in case no meteorological RINEX 
   // is available)
   ZeroTropModel noTropModel;

   // Object for GG-type tropospheric model (Goad and Goodman, 1974)
   GGTropModel ggTropModel;   // Default constructor => default values for model
   // Pointer to one of the two available tropospheric models. It points to 
   // the void model by default
   TropModel *tropModelPtr=&noTropModel;


   // This verifies the ammount of command-line parameters given and prints a help 
   // message, if necessary
   if ((argc < 3) || (argc>4))
   {
      cerr <<  "Usage:" << endl; 
      cerr << "   " << argv[0] << " <RINEX Obs file>  <RINEX Nav file>  [<RINEX Met file>]" << endl;
      exit (-1);
   }

   // Let's compute an useful constant (also found in "icd_200_constants.hpp")
   const double gamma = (L1_FREQ / L2_FREQ)*(L1_FREQ / L2_FREQ);
   
   try
   {  
      // Read nav file and store unique list of ephemeredes
      RinexNavStream rnffs(argv[2]);    // Open ephemerides data file
      RinexNavData rne;
      RinexNavHeader hdr;
      
      // Let's read the header (may be skipped)
      rnffs >> hdr;

      // Storing the ephemeris in "bcstore"
      while (rnffs >> rne) bcestore.addEphemeris(rne);
      // Setting the criteria for looking up ephemeris
      bcestore.SearchNear();
      
      // If provided, open and store met file into a linked list.
      list<RinexMetData> rml;
      if (argc==4)
      {
         RinexMetStream rms(argv[3]);    // Open meteorological data file
         RinexMetHeader rmh;
         // Let's read the header (may be skipped)
         rms >> rmh;
         
         RinexMetData rmd;
         // If meteorological data is provided, let's change pointer to 
         // a GG-model object
         tropModelPtr=&ggTropModel;
         // All data is read into "rml", a meteorological data linked list
         while (rms >> rmd) rml.push_back(rmd);
      }

      // Open and read the observation file one epoch at a time.
      // For each epoch, compute and print a position solution
      RinexObsStream roffs(argv[1]);    // Open observations data file
      // In order to throw exceptions, it is necessary to set the failbit
      roffs.exceptions(ios::failbit);

      RinexObsHeader roh;
      RinexObsData rod;

      // Let's read the header (may be skipped)
      roffs >> roh;

      // Defining iterator "mi" for meteorological data linked list "rml", and 
      // set it to the beginning
      list<RinexMetData>::iterator mi=rml.begin();


      // Let's process all lines of observation data, one by one
      while (roffs >> rod)
      {
         
         // Find a weather point. Only if a meteorological RINEX file was 
         // provided, the meteorological data linked list "rml" is neither empty 
         // or at its end, and the time of meteorological records are below
         // observation data epoch.
         while ( (argc==4) &&
                 (!rml.empty()) &&
                 (mi!=rml.end()) &&
                 ((*mi).time < rod.time) )
         {
            mi++;    // Read next item in list

            // Feed GG tropospheric model object with meteorological parameters
            // Take into account, however, that setWeather is not accumulative,
            // i.e., only the last fed set of data will be used for computation
            ggTropModel.setWeather((*mi).data[RinexMetHeader::TD],
                                   (*mi).data[RinexMetHeader::PR],
                                   (*mi).data[RinexMetHeader::HR]);
         }
         

         // Apply editing criteria 
         if  (rod.epochFlag == 0 || rod.epochFlag == 1)   // Begin usable data
	     {
	        vector<SatID> prnVec;
            vector<double> rangeVec;

            // Let's define the "it" iterator to visit the observations PRN map
            // RinexSatMap is a map from SatID to RinexObsTypeMap: 
            //      std::map<SatID, RinexObsTypeMap>
            RinexObsData::RinexSatMap::const_iterator it;

            // This part gets the PRN numbers and ionosphere-corrected 
            // pseudoranges for the current epoch. They are correspondly fed 
            // into "prnVec" and "rangeVec"
            // "obs" is a public attribute of RinexObsData to get the map 
            // of observations
            for (it = rod.obs.begin(); it!= rod.obs.end(); it++)
            {
               // RinexObsTypeMap is a map from RinexObsType to RinexDatum:
               //   std::map<RinexObsHeader::RinexObsType, RinexDatum>
               RinexObsData::RinexObsTypeMap otmap;

               // Let's define two iterators to visit the observations type map
               RinexObsData::RinexObsTypeMap::const_iterator itP1, itP2;

               /////////////////////////////////////////////////
               //
               //    What did we do in the former code lines?:
               //
               // For each observation data epoch (rod), if valid 
               // (rod.epochFlag = 0 or 1):
               // - use "it" iterator to visit the RinexObsTypeMap of each 
               //   satellite, 
               // - and then use "itP1" and "itP2" iterators to visit the
               //   observation data (RinexDatum) according to their type
               //  (RinexObsType)
               //
               /////////////////////////////////////////////////


               // The "second" field of a RinexPrnMap (it) is a 
               // RinexObsTypeMap (otmap)
               otmap = (*it).second;

               // Let's find a P1 observation inside the RinexObsTypeMap that 
               // is "otmap"
               itP1 = otmap.find(RinexObsHeader::P1);

               // If "itP1" is not the last type of observation, there may be 
               // a P2 observation and the double-frequency ionospheric 
               // corrections may be applied
               if (itP1!=otmap.end())
	           {
                  double ionocorr = 0;

                  // Now, let's find a P2 observation inside the 
                  // RinexObsTypeMap that is "otmap"
                  itP2 = otmap.find(RinexObsHeader::P2);
                  // If we indeed found a P2 observation, let's apply the
                  // ionospheric corrections
                  if (itP2!=otmap.end()) 
                     // The "second" part of a RinexObsTypeMap is a RinexDatum,
                     // whose public attribute "data" indeed holds the actual 
                     // data point
                     ionocorr = 1./(1.-gamma)*((*itP1).second.data-(*itP2).second.data);
                  // Now, we include the current PRN number in the first part 
                  // of "it" (a RinexPrnMap) into the vector holding PRN numbers.
                  // All satellites in view at this epoch that also have P1 and 
                  // P2 observations will be included
                  prnVec.push_back((*it).first);
                  // The same is done for the vector of doubles holding the
                  // corrected ranges
                  rangeVec.push_back((*itP1).second.data-ionocorr);

                  // WARNING: Please note that so far no further correction is 
                  // done on data: Relativistic effects, tropospheric correction,
                  // instrumental delays, etc.
	           }

            }

            // The default constructor for PRSolution objects (like "raimSolver")
            // is to set a RMSLimit of 6.5. We change that here. With this value 
            // of 3e6 the solution will have a lot more dispersion
            raimSolver.RMSLimit = 3e6;

            // In order to compute positions we need the current time, the 
            // vector of visible satellites, the vector of corresponding ranges,
            // the object containing satellite ephemerides and a pointer to the
            // tropospheric model to be applied
	        raimSolver.RAIMCompute(rod.time,prnVec,rangeVec, bcestore,  tropModelPtr);

            // Note: Given that the default constructor sets public attribute
            // "Algebraic" to FALSE, a linearized least squares algorithm will 
            // be used to get the solutions.
            // Also, the default constructor sets ResidualCriterion to true, so 
            // the rejection criterion is based on RMS residual of fit, instead 
            // of RMS distance from an a priori position.

            // If we got a valid solution, let's print it

            if (raimSolver.isValid())
            {
               // Vector "Solution" holds the coordinates, expressed in meters 
               // in an Earth Centered, Earth Fixed (ECEF) reference frame. The
               // order is x, y, z  (as all ECEF objects)
               cout << setprecision(12) << raimSolver.Solution[0] << " " ;
               cout << raimSolver.Solution[1] << " " ;
               cout << raimSolver.Solution[2];
               cout << endl ;
            }
            
 
 	     } // End usable data

      } // End loop through each epoch
   }
Esempio n. 3
0
void P::process()
{
   for (int i=1; i < 33; i++)
   {
      SatID sv(i, SatID::systemGPS);
      svVec.push_back(sv);
   }

   vector<int> dataPoints(32);
   float refDataPoint;
// -----------------------------------------------------------------------
// -----------------------------------------------------------------------
// Following tables hold the data sets that we have for now.
// NOW CARDATA.bin
/*
// subframe three data points from gnssDavisHouseCar2.bin: (z=360198) CRUMMY
   dataPoints[1]=29487862; 
   dataPoints[9]=29415434;
   dataPoints[14]=29393435;
   dataPoints[23]=29360589;
   dataPoints[25]=29365069;
   dataPoints[28]=29328671;
   dataPoints[29]=29471399;
*/
// position -e rin269.08n -z 360204 -w 1498
/*
// sf3 
// position -e rin269.08n -z 360204 -r 8.184 -w 1498
// GOOD results: rms 55 rmsknown 62
   dataPoints[1]=14743933; 
   dataPoints[9]=14707720;
   dataPoints[14]=14696721;
   dataPoints[17]=14745155;
   dataPoints[23]=14680297;
   dataPoints[25]=14682538;
   dataPoints[28]=14662516;
   dataPoints[29]=14735701;
*/
/*
// sf4 GOOD ALSO
   dataPoints[1]=63847008; 
   dataPoints[9]=63810816;
   dataPoints[14]=63799663;
   dataPoints[17]=63848083;
   dataPoints[23]=63783288;
   dataPoints[25]=63785487;
   dataPoints[28]=63765493;
   dataPoints[29]=63838790;
*/
/*
// sf 5   90 meters
   dataPoints[1]=112950083; 
   dataPoints[9]=112913912;
   dataPoints[14]=112902598;
   dataPoints[17]=112951011;
   dataPoints[23]=112886279;
   dataPoints[25]=112888436;
   dataPoints[28]=112868477;
   dataPoints[29]=112941879;
*/
/*
// sf 1  140 m, 220 m (only 7 sats)
   dataPoints[1]=162053158; 
   dataPoints[9]=162017008;
   dataPoints[14]=162005540;
   dataPoints[17]=162053932;
   dataPoints[23]=161989277;
   dataPoints[25]=161991385;
   dataPoints[29]=162044968;
*/
/*
// sf 2   90m and 130m
   dataPoints[1]=211156226; 
   dataPoints[9]=211120111;
   dataPoints[14]=211108482;
   dataPoints[17]=211156860;
   dataPoints[23]=211092268;
   dataPoints[25]=211094333;
   dataPoints[29]=211148057;
*/
// -----------------------------------------------------------------------
// -----------------------------------------------------------------------

   long int total = 0;
   int numberSVs = 0;
   for(int i=0; i<32;i++)
   {

      /* // This code uses first PRN found as reference data point.
      if(dataPoints[i] != 0)
      {
         refDataPoint = dataPoints[i];
         break;
      }*/

      total += dataPoints[i];
      if(dataPoints[i] != 0)
         numberSVs++;
   }
   refDataPoint = total/numberSVs; // average data point to be reference.
   
   vector<double> obsVec(32);
   for(int i=0; i<32; i++)
   {
      if(dataPoints[i] != 0)
      {
         // 0.073 is an arbitrary guessed time of flight
         obsVec[i] = gpstk::C_GPS_M*(0.073 - (refDataPoint - 
                                dataPoints[i])/(1000*sampleRate*1000));
      }  
      else
      {
         SatID temp(0, SatID::systemGPS); 
         svVec[i] = temp; // set SatID equal to 0, the SV won't be considered
      }
   }

   if(verboseLevel)
   {
      for(int i = 0; i < 32; i++)
         cout << svVec[i] << " "  << obsVec[i] << endl;
   }

//-----------------------------------------------------------------------------
// Calculate initial position solution.
   GGTropModel gg;
   gg.setWeather(30., 1000., 50.);    
   PRSolution prSolver;
   prSolver.RMSLimit = 400;
   prSolver.RAIMCompute(time, svVec, obsVec, bce, &gg);
   Vector<double> sol = prSolver.Solution;

   cout << endl << "Position (ECEF): " << fixed << sol[0] << " " << sol[1] 
        << " " << sol[2] << endl << "Clock Error (includes that caused by guess): " 
        << sol[3]*1000/gpstk::C_GPS_M << " ms" << endl;
   cout << "# good SV's: " << prSolver.Nsvs << endl
        << "RMSResidual: " << prSolver.RMSResidual << " meters" << endl << endl;
//----------------------------------------------------------------------------- 
// Calculate Ionosphere correction.
   antennaPos[0] = sol[0];
   antennaPos[1] = sol[1];
   antennaPos[2] = sol[2];
   ECEF ecef(antennaPos);
   for (int i=1; i<=32; i++)
   {
      SatID sv(i, SatID::systemGPS);
      try 
      {
         Xvt svpos = bce.getXvt(sv, time);
         double el = antennaPos.elvAngle(svpos.x);
         double az = antennaPos.azAngle(svpos.x);
         double ic = iono.getCorrection(time, ecef, el, az); // in meters
         ionoVec.push_back(ic);
      }
      catch (Exception& e)
      {}
   }
   if(verboseLevel)
   {
      for(int i = 0; i < 32; i++)
      {
         cout << svVec[i] << " "  << obsVec[i] << " " << ionoVec[i] << endl;
         
      }
   }
   for(int i=0;i<32;i++)
   {
      obsVec[i] -= sol[3]; // convert pseudoranges to ranges
      obsVec[i] += ionoVec[i]; // make iono correction to ranges.
   }

//----------------------------------------------------------------------------- 
// Recalculate position using time corrected by clock error + ionosphere.
   time -= (sol[3] / gpstk::C_GPS_M);
   GGTropModel gg2;
   gg2.setWeather(30.,1000., 20.); /*(Temp(C),Pressure(mbar),Humidity(%))*/    
   PRSolution prSolver2;
   prSolver2.RMSLimit = 400;
   prSolver2.RAIMCompute(time, svVec, obsVec, bce, &gg2);
   Vector<double> sol2 = prSolver2.Solution;
   cout << "Recomputing position with refined time and ionosphere correction:" 
        << fixed << setprecision(6);
   cout << endl << "Position (ECEF): " << fixed << sol2[0] << " " << sol2[1] 
        << " " << sol2[2] << endl << "Clock Error: " 
        << sol2[3]*1e6/gpstk::C_GPS_M << " us" << endl;
   cout << "# good SV's: " << prSolver2.Nsvs << endl
        << "RMSResidual: " << prSolver2.RMSResidual << " meters" << endl;

//-----------------------------------------------------------------------------
// Following block will make PRSolve compute residual from a known hardcoded
// position
   PRSolution prSolver3; 
   vector<double> S;
/*
   S.push_back(-756736.1300); // my house
   S.push_back(-5465547.0217);
   S.push_back(3189100.6012);
*/

   S.push_back(-740314.1444); // ARLSW antenna
   S.push_back(-5457066.8902);
   S.push_back(3207241.5759);

   S.push_back(0.0);
   prSolver3.Solution = S;
   prSolver3.ResidualCriterion = false;
   prSolver3.RMSLimit = 400;
   prSolver3.RAIMCompute(time, svVec, obsVec, bce, &gg2);
   cout << "RMSResidual from known position: " << prSolver3.RMSResidual
        << " meters" << endl << endl;
}
Esempio n. 4
0
int main(int argc, char *argv[])
{
   int verbosity = 1;
   Triple antennaPos;

   CommandOptionWithAnyArg
      ephFileOption('e', "ephemeris", "Rinex Ephemeris data file name.", true);

   CommandOptionNoArg
      helpOption('h', "help", "Print usage. Repeat for more info. "),

      verbosityOption('v', "verbosity", "Increase the verbosity level. The default is 0.");

   CommandOptionWithAnyArg
      antennaPosOption('p', "position", "Initial estimate of the antenna position in ECEF. Only needs to be good to the km level.");

   CommandOptionWithTimeArg
      timeOption('t', "time", "%m/%d/%Y %H:%M:%S",
                 "Time estimate for start of data (MM/DD/YYYY HH:MM:SS).");

   string appDesc("Performs a simple nav solution from correlation delays.");
   CommandOptionParser cop(appDesc);
   cop.parseOptions(argc, argv);

   if (helpOption.getCount() || cop.hasErrors())
   {
      if (cop.hasErrors() && helpOption.getCount()==0)
      {
         cop.dumpErrors(cout);
         cout << "Use -h for help." << endl;
      }
      else
      {
         cop.displayUsage(cout);
      }
      exit(0);
   }

   if (verbosityOption.getCount())
      verbosity = asInt(verbosityOption.getValue()[0]);


   if (antennaPosOption.getCount())
   {
      string aps = antennaPosOption.getValue()[0];
      if (numWords(aps) != 3)
      {
         cout << "Please specify three coordinates in the antenna postion." << endl;
         exit(-1);
      }
      else
         for (int i=0; i<3; i++)
            antennaPos[i] = asDouble(word(aps, i));
   }

   GPSEphemerisStore bce;
   IonoModel iono;
   for (int i=0; i < ephFileOption.getCount(); i++)
   {
      string fn = ephFileOption.getValue()[i];
      RinexNavStream rns(fn.c_str(), ios::in);
      rns.exceptions(ifstream::failbit);

      RinexNavHeader hdr;
      rns >> hdr;
      iono = IonoModel(hdr.ionAlpha, hdr.ionBeta);

      RinexNavData rnd;
      while (rns >> rnd)
         bce.addEphemeris(rnd);

      if (verbosity)
         cout << "Read " << fn << " as RINEX nav. " << endl;
   }

   if (verbosity>1)
      cout << "Have ephemeris data from " << bce.getInitialTime() 
           << " through " << bce.getFinalTime() << endl;

   DayTime time = timeOption.getTime()[0];
   if (verbosity)
      cout << "Initial time estimate: " << time << endl;

   if (time < bce.getInitialTime() || time > bce.getFinalTime())
      cout << "Warning: Initial time does not appear to be within the provided ephemeris data." << endl;


   GPSGeoid gm;
   ECEF ecef(antennaPos);
   map<SatID, double> range;
   vector<SatID> svVec;
   vector<double> expVec, ionoVec;
   for (int i=1; i<=32; i++)
   {
      SatID sv(i, SatID::systemGPS);
      try 
      {
         Xvt svpos = bce.getXvt(sv, time);
         double el = antennaPos.elvAngle(svpos.x);
         double az = antennaPos.azAngle(svpos.x);

         double pr = svpos.preciseRho(ecef, gm, 0);
         double ic = iono.getCorrection(time, ecef, el, az);

         expVec.push_back(pr);
         svVec.push_back(sv);
         ionoVec.push_back(ic);
      }
      catch (Exception& e)
      {}
   }

   // Replace this with the observed delays...
   vector<double> obsVec(expVec);

   try 
   {
      GGTropModel gg;
      gg.setWeather(20., 1000., 50.);    
      PRSolution prSolver;
      prSolver.RMSLimit = 400;
      prSolver.RAIMCompute(time, svVec, obsVec, bce, &gg);
      Vector<double> sol = prSolver.Solution;
      cout << "solution:" << fixed << sol << endl;
   }   
   catch (Exception& e)
   {
      cout << "Caught exception:" << e << endl;
   }
}