/* 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 CommonTime& Tr,
std::vector<SatID>& Satellite,
std::vector<double>& Pseudorange,
const XvtStore<SatID>& Eph )
{
Matrix<double> SVP;
Bancroft Ban;
Vector<double> vPos;
PRSolution2 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()'
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_MPS*(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.);
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;
}
void ObsArray::load(const std::vector<std::string>& obsList,
const std::vector<std::string>& navList)
{
// First check for existance of input files
for (size_t i=0; i< obsList.size(); i++)
{
if (!FileUtils::fileAccessCheck(obsList[i]))
{
ObsArrayException oae("Cannot read obs file " + obsList[i]);
GPSTK_THROW(oae);
}
}
for (size_t i=0; i< navList.size(); i++)
{
if (!FileUtils::fileAccessCheck(navList[i]))
{
ObsArrayException oae("Cannot read nav file " + navList[i]);
GPSTK_THROW(oae);
}
else
// Load the ephemeris information from the named NAV file.
ephStore.loadFile(navList[i]);
}
long totalEpochsObs = 0;
Triple antPos;
double dR;
for (size_t i=0; i< obsList.size(); i++)
{
//RinexObsHeader roh;
long numEpochsObs = 0;
double dataRate;
Triple antennaPos;
scanObsFile(obsList[i], numEpochsObs, dataRate, antennaPos);
if (i==0)
{
antPos=antennaPos;
dR=dataRate;
if (antennaPos.mag()<1) // A reported antenna position near the
// center of the Earth. REcompute.
{
PRSolution2 prSolver;
prSolver.RMSLimit = 400;
GGTropModel ggTropModel;
ggTropModel.setWeather(20., 1000., 50.); // A default model for sea level.
// NOTE: The following section is partially adapted to Rinex3, but
// more work is needed here
RinexObsStream tempObsStream(obsList[i]);
Rinex3ObsData tempObsData;
Rinex3ObsHeader tempObsHeader;
tempObsStream >> tempObsHeader;
tempObsStream >> tempObsData;
ExtractPC ifObs;
ifObs.getData(tempObsData, tempObsHeader);
std::vector<SatID> vsats(ifObs.availableSV.size());
for (size_t ii=0; ii<ifObs.availableSV.size(); ++ii)
{
vsats[ii]=ifObs.availableSV[ii];
}
std::vector<double> vranges(ifObs.obsData.size());
for (size_t ii=0; ii<ifObs.obsData.size(); ++ii)
{
vranges[ii]=ifObs.obsData[ii];
}
prSolver.RAIMCompute(tempObsData.time,
vsats, vranges,
ephStore, &ggTropModel);
antPos[0] = prSolver.Solution[0];
antPos[1] = prSolver.Solution[1];
antPos[2] = prSolver.Solution[2];
/*
std::cout << "Position resolved at "
<< antPos[0] << ", " << antPos[1] << ", "
<< antPos[2] << std::endl;
*/
}
}
//-----------------------------------------------------------------------------
void RxSim::process()
{
pthread_t *thread_id = new pthread_t[numTrackers];
pthread_attr_t attr;
int rc;
void *status;
vector<Par> p(numTrackers);
vector<NavFramer> nf(numTrackers);
long int dataPoint =0;
vector<int> count(numTrackers);
for(int i=0;i<numTrackers;i++)
{
nf[i].debugLevel = debugLevel;
nf[i].dump(cout);
count[i]=0;
}
complex<float> s;
while (*input >> s)
{
Buffer b;
b.arr.push_back(s);
dataPoint++;
int index = 0;
int bufferSize = 40*16367;
while(index < bufferSize) // Fill input buffer
{
*input >> s;
b.arr.push_back(s);
index++;
dataPoint++;
}
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
edgeFound = false;
for(int i = 0; i < numTrackers; i++)
{
p[i].dp = dataPoint; // Set parameters for each tracker.
p[i].bufferSize = bufferSize;
p[i].s = &b;
p[i].count = &count[i];
p[i].tr = tr[i];
p[i].nf = &nf[i];
p[i].v = (verboseLevel);
p[i].prn = tr[i]->prn;
p[i].solvePos = solvePos;
// Split
rc = pthread_create( &thread_id[i], &attr, Cfunction, &p[i] ) ;
if (rc)
{
printf("ERROR; return code from pthread_create() is %d\n", rc);
exit(-1);
}
}
// Join
for(int i = 0; i < numTrackers; i++)
{
rc = pthread_join( thread_id[i], &status) ;
if (rc)
{
printf("ERROR; return code from pthread_join() is %d\n", rc);
exit(-1);
}
}
if(edgeFound == true)
{
//---------------------------------------------------------------------------
// Position Solution
if(solvePos == true)
{
GPSEphemerisStore bce;
IonoModel iono;
CommonTime time;
double zCount = (double)ZCount - 6.0;
double sampleRate = 1/timeStep;
GPSEllipsoid gm;
vector<SatID> svVec;
vector<double> ionoVec;
Triple antennaPos;
time = GPSWeekZcount(gpsWeek, zCount);
RinexNavStream rns(ephFile.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 (time < bce.getInitialTime() || time > bce.getFinalTime())
cout << "Warning: Initial time does not appear to be "
<< "within the provided ephemeris data." << endl;
for (int i=1; i < 33; i++)
{
SatID sv(i, SatID::systemGPS);
svVec.push_back(sv);
}
float refDataPoint;
long int total = 0;
int numberSVs = 0;
for(int i=0; i<32;i++)
{
total += dataPoints[i];
if(dataPoints[i] != 0)
numberSVs++;
}
refDataPoint = total/numberSVs;
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_MPS*(0.073 - (refDataPoint -
dataPoints[i])/(sampleRate)); //*2 because of hilbert
}
else
{
SatID temp(0, SatID::systemGPS);
svVec[i] = temp; // set SatID equal to 0,
//the SV won't be considered
}
}
// 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;
time -= (sol[3] / gpstk::C_MPS);
cout << "Time: " << time << endl;
//cout << "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;
// If we wanted to just output ranges, we can correct the obsVector
// using the clock error and have the range to each sat.
for(int i = 0; i < 32; i++)
dataPoints[i] = 0;
// 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.
}*/
// Then plug back into RAIMCompute...
}
}
//---------------------------------------------------------------------------
if (cc->localTime > timeLimit)
break;
}
