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[])
{
// 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
}
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;
}
}