/* 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()'
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 }
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; }
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; } }