C++ Triple::azAngle方法代码示例

//.........这里部分代码省略.........
      {
         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.);    
   PRSolution2 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_MPS << " 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];
   Position 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_MPS);
   GGTropModel gg2;
   gg2.setWeather(30.,1000., 20.); /*(Temp(C),Pressure(mbar),Humidity(%))*/    
   PRSolution2 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_MPS << " 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
   PRSolution2 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;
}

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);
//.........这里部分代码省略.........