/* Computes the quantile function ( cdf^-1() )
*
* @param p Probability value
*
* \ warning Value "p" must be in the range (0, 1)
*/
double GaussianDistribution::invcdf(double p)
throw(InvalidParameter)
{
double inf( 9.0e+99 );
// Check limits
if( ( p < 0.0 ) ||
( p > 1.0 ) )
{
InvalidParameter e( "Invalid input value for 'p'." );
GPSTK_THROW(e);
}
if( p == 0.0 ) return -inf;
if( p == 1.0 ) return inf;
// Compute invcdf
return ( mean
+ 1.4142135623730951 * sigma * gpstk::inverf( 2.0 * p - 1.0 ) );
} // End of method 'GaussianDistribution::invcdf()'
/* Returns a reference to a gnnsRinex object after solving
* the previously defined equation system.
*
* @param gData Data object holding the data.
*/
gnssRinex& SolverPPPFB::Process(gnssRinex& gData)
throw(ProcessingException)
{
try
{
SolverPPP::Process(gData);
// Before returning, store the results for a future iteration
if(firstIteration)
{
// Create a new gnssRinex structure with just the data we need
gnssRinex gBak(gData.extractTypeID(keepTypeSet));
// Store observation data
ObsData.push_back(gBak);
// Update the number of processed measurements
processedMeasurements += gData.numSats();
}
return gData;
}
catch(Exception& u)
{
// Throw an exception if something unexpected happens
ProcessingException e( getClassName() + ":"
+ u.what() );
GPSTK_THROW(e);
}
} // End of method 'SolverPPPFB::Process()'
FileSpec::FileSpecType FileSpec::convertFileSpecType(const string& fst)
throw(FileSpecException)
{
if (fst == string("n")) return station;
else if (fst == string("r")) return receiver;
else if (fst == string("p")) return prn;
else if (fst == string("t")) return selected;
else if (fst == string("I")) return sequence;
else if (fst == string("v")) return version;
else if (fst == string("k")) return clock;
else if (fst == string("x")) return text;
else if (fst == string("Y") ||
fst == string("y")) return year;
else if (fst == string("m")) return month;
else if (fst == string("d")) return dayofmonth;
else if (fst == string("H")) return hour;
else if (fst == string("M")) return minute;
else if (fst == string("S")) return second;
else if (fst == string("f")) return fsecond;
else if (fst == string("G")) return gpsweek;
else if (fst == string("F")) return fullgpsweek;
else if (fst == string("g")) return gpssecond;
else if (fst == string("Q")) return mjd;
else if (fst == string("w")) return dayofweek;
else if (fst == string("j")) return day;
else if (fst == string("s")) return doysecond;
else if (fst == string("Z")) return zcount;
else if (fst == string("z")) return zcountfloor;
else if (fst == string("U")) return unixsec;
else if (fst == string("u")) return unixusec;
else if (fst == string("C") ||
fst == string("c")) return fullzcount;
else
{
FileSpecException fse("Unknown FileSpecType: " + fst);
GPSTK_THROW(fse);
}
}
/* Set the number of degrees of freedom.
*
* @param n Degrees of freedom
*
* \warning "n" must be > 0.0, otherwise n = |n|.
*/
StudentDistribution& StudentDistribution::setNDF(int n)
throw(InvalidParameter)
{
if( n == 0 )
{
InvalidParameter e( "Invalid value for NDF." );
GPSTK_THROW(e);
}
if( n < 0 )
{
ndf = -n;
}
else
{
ndf = n;
}
return (*this);
} // End of method 'StudentDistribution::setNDF()'
void RinexNavData::getBroadcastOrbit6(const string& currentLine)
throw(StringException, FFStreamError)
{
try
{
double SV_health;
accuracy = gpstk::StringUtils::for2doub(currentLine.substr(3,19));
SV_health = gpstk::StringUtils::for2doub(currentLine.substr(22,19));
Tgd = gpstk::StringUtils::for2doub(currentLine.substr(41,19));
IODC = gpstk::StringUtils::for2doub(currentLine.substr(60,19));
health = (short) SV_health;
}
catch (std::exception &e)
{
FFStreamError err("std::exception: " +
string(e.what()));
GPSTK_THROW(err);
}
}
void fromString(const std::string str)
{
std::string STR(gpstk::StringUtils::upperCase(str));
if(STR == std::string("GPUT"))
{ type = GPUT; frTS = TimeSystem::GPS; toTS = TimeSystem::UTC; }
else if(STR == std::string("GAUT"))
{ type = GAUT; frTS = TimeSystem::GAL; toTS = TimeSystem::UTC; }
else if(STR == std::string("SBUT"))
// TD ??
{ type = SBUT; frTS = TimeSystem::GPS; toTS = TimeSystem::UTC; }
else if(STR == std::string("GLUT"))
{ type = GLUT; frTS = TimeSystem::GLO; toTS = TimeSystem::UTC; }
else if(STR == std::string("GPGA"))
{ type = GPGA; frTS = TimeSystem::GPS; toTS = TimeSystem::GAL; }
else if(STR == std::string("GLGP"))
{ type = GLGP; frTS = TimeSystem::GLO; toTS = TimeSystem::GPS; }
else if(STR == std::string("QZGP"))
{ type = QZGP; frTS = TimeSystem::QZS; toTS = TimeSystem::GPS; }
else if(STR == std::string("QZUT"))
{ type = QZUT; frTS = TimeSystem::QZS; toTS = TimeSystem::UTC; }
else if(STR == std::string("BDUT"))
{ type = BDUT; frTS = TimeSystem::BDT; toTS = TimeSystem::UTC; }
else if(STR == std::string("BDGP"))
{ type = BDGP; frTS = TimeSystem::BDT; toTS = TimeSystem::GPS; }
else {
Exception e("Unknown TimeSystemCorrection type: " + str);
GPSTK_THROW(e);
}
}
T MEstimate(const T *xd, int nd, const T& M, const T& MAD, T *w=NULL)
throw(Exception)
{
try {
T tv, m, mold, sum, sumw, *wt, weight, *t;
T tol=0.000001;
int i, n, N=10; // N is the max number of iterations
if(!xd || nd < 2) {
Exception e("Invalid input");
GPSTK_THROW(e);
}
tv = T(RobustTuningT)*MAD;
n = 0;
m = M;
do {
mold = m;
n++;
sum = sumw = T();
for(i=0; i<nd; i++) {
if(w) wt=&w[i];
else wt=&weight;
*wt = T(1);
if(xd[i]<m-tv) *wt = -tv/(xd[i]-m);
else if(xd[i]>m+tv) *wt = tv/(xd[i]-m);
sumw += (*wt);
sum += (*wt)*xd[i];
}
m = sum / sumw;
} while(T(ABSOLUTE((m-mold)/m)) > tol && n < N);
return m;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
} // end MEstimate
// Compute the pressure and temperature at height, and the undulation,
// for the given position and time.
void GlobalTropModel::getGPT(double& P, double& T, double& U)
throw(InvalidTropModel)
{
try { testValidity(); }
catch(InvalidTropModel& e) { GPSTK_RETHROW(e); }
int i;
// undulation and orthometric height
U = 0.0;
for(i=0; i<55; i++) U += (Ageoid[i]*aP[i] + Bgeoid[i]*bP[i]);
double orthoht(height - U);
if(orthoht > 44247.) GPSTK_THROW(InvalidTropModel(
"Invalid Global trop model: Rx Height is too large"));
// press at geoid
double am(0.0),aa(0.0),v0;
for(i=0; i<55; i++) {
am += (APressMean[i]*aP[i] + BPressMean[i]*bP[i]);
aa += (APressAmp[i]*aP[i] + BPressAmp[i]*bP[i]);
}
v0 = am + aa * ::cos(dayfactor);
// pressure at height
// NB this implies any orthoht > 1/2.26e-5 == 44247.78m is invalid!
P = v0 * ::pow(1.0-2.26e-5*orthoht,5.225);
// temper on geoid
am = aa = 0.0;
for(i=0; i<55; i++) {
am += (ATempMean[i]*aP[i] + BTempMean[i]*bP[i]);
aa += (ATempAmp[i]*aP[i] + BTempAmp[i]*bP[i]);
}
v0 = am + aa * ::cos(dayfactor);
// temp at height
T = v0 - 6.5e-3 * orthoht;
}
// Compute continued fractions portion of incomplete beta function I_x(a,b)
/// Routine used internally for Incomplete beta function I_x(a,b)
double cfIBeta(const double& x, const double& a, const double& b) throw(Exception)
{
static const int imax(100);
static const double eps(10*std::numeric_limits<double>().epsilon());
static const double fpmin(10*std::numeric_limits<double>().min());
const double qab(a+b);
const double qap(a+1.0);
const double qam(a-1.0);
double c(1),d(1-qab*x/qap),aa,del;
if(::fabs(d) < fpmin) d=fpmin;
d = 1.0/d;
double h(d);
int i,i2;
for(i=1; i<=imax; i++) {
i2 = 2*i;
aa = i*(b-i)*x/((qam+i2)*(a+i2));
d = 1.0 + aa*d;
if(::fabs(d) < fpmin) d=fpmin;
c = 1.0 + aa/c;
if(::fabs(c) < fpmin) c=fpmin;
d = 1.0/d;
h *= d*c;
aa = -(a+i)*(qab+i)*x/((a+i2)*(qap+i2));
d = 1.0 + aa*d;
if(::fabs(d) < fpmin) d=fpmin;
c = 1.0 + aa/c;
if(::fabs(c) < fpmin) c=fpmin;
d = 1.0/d;
del = d*c;
h *= del;
if(::fabs(del-1.0) < eps) break;
}
if(i > imax) {
Exception e("Overflow in cfIBeta(); a or b too big");
GPSTK_THROW(e);
}
return h;
}
string FileSpec::convertFileSpecType(const FileSpecType fst)
throw(FileSpecException)
{
if (fst == station) return string("n");
else if (fst == receiver) return string("r");
else if (fst == prn) return string("p");
else if (fst == selected) return string("t");
else if (fst == sequence) return string("I");
else if (fst == version) return string("v");
else if (fst == fixed) return string("");
else if (fst == clock) return string("k");
else if (fst == text) return string("x");
else if (fst == year) return string("y");
else if (fst == month) return string("m");
else if (fst == dayofmonth) return string("d");
else if (fst == hour) return string("H");
else if (fst == minute) return string("M");
else if (fst == second) return string("S");
else if (fst == fsecond) return string("f");
else if (fst == gpsweek) return string("G");
else if (fst == fullgpsweek) return string("F");
else if (fst == gpssecond) return string("g");
else if (fst == mjd) return string("Q");
else if (fst == dayofweek) return string("w");
else if (fst == day) return string("j");
else if (fst == doysecond) return string("s");
else if (fst == zcount) return string("Z");
else if (fst == zcountfloor) return string("z");
else if (fst == unixsec) return string("U");
else if (fst == unixusec) return string("u");
else if (fst == fullzcount) return string("C");
else
{
FileSpecException fse("Unknown FileSpecType: " + asString(fst));
GPSTK_THROW(fse);
}
}
/* Dumps data from a satTypeValueMap object.
*
* @param gData Data object holding the data.
*/
satTypeValueMap& Dumper::Process( satTypeValueMap& gData )
throw(ProcessingException)
{
try
{
// Iterate through all items in the GNSS Data Structure
for( satTypeValueMap::const_iterator it = gData.begin();
it!= gData.end();
it++ )
{
// First, print satellite (system and PRN)
*outStr << (*it).first << " ";
// Now, print TypeIDs
printTypeID( (*it).second );
// Print end of line
*outStr << std::endl;
} // End of 'for( satTypeValueMap::const_iterator it = ...'
return gData;
}
catch(Exception& u)
{
// Throw an exception if something unexpected happens
ProcessingException e( getClassName() + ":"
+ u.what() );
GPSTK_THROW(e);
}
} // End of method 'Dumper::Process()'
/* Returns a gnnsSatTypeValue object, adding the new data generated
* when calling this object.
*
* @param gData Data object holding the data.
*/
gnssSatTypeValue& PhaseCodeAlignment::Process(gnssSatTypeValue& gData)
throw(ProcessingException)
{
try
{
Process(gData.header.epoch, gData.body);
return gData;
}
catch(Exception& u)
{
// Throw an exception if something unexpected happens
ProcessingException e( getClassName() + ":"
+ u.what() );
GPSTK_THROW(e);
}
} // End of 'PhaseCodeAlignment::Process()'
/* Returns a gnnsRinex object, adding the new data generated when
* calling this object.
*
* @param gData Data object holding the data.
*/
gnssRinex& LICSDetector::Process(gnssRinex& gData)
throw(ProcessingException)
{
try
{
Process(gData.header.epoch, gData.body, gData.header.epochFlag);
return gData;
}
catch(Exception& u)
{
// Throw an exception if something unexpected happens
ProcessingException e( getClassName() + ":"
+ u.what() );
GPSTK_THROW(e);
}
} // End of method 'LICSDetector::Process()'
/* Returns a gnnsRinex object, adding the new data generated
* when calling this object.
*
* @param gData Data object holding the data.
*/
gnssRinex& ComputeSimpleWeights::Process(gnssRinex& gData)
throw(ProcessingException)
{
try
{
Process(gData.header.epoch, gData.body);
return gData;
}
catch(Exception& u)
{
// Throw an exception if something unexpected happens
ProcessingException e( getClassName() + ":"
+ u.what() );
GPSTK_THROW(e);
}
} // End of method 'ComputeSimpleWeightsWeights::Process()'
/* Returns a gnnsRinex object, adding the new data generated when
* calling this object.
*
* @param gData Data object holding the data.
*/
gnssRinex& EclipsedSatFilter::Process(gnssRinex& gData)
throw(ProcessingException)
{
try
{
Process(gData.header.epoch, gData.body);
return gData;
}
catch(Exception& u)
{
// Throw an exception if something unexpected happens
ProcessingException e( getClassName() + ":"
+ u.what() );
GPSTK_THROW(e);
}
} // End of 'EclipsedSatFilter::Process()'
void RinexNavData::getBroadcastOrbit5(const string& currentLine)
throw(StringException, FFStreamError)
{
try
{
double codeL2, L2P, toe_wn;
idot = gpstk::StringUtils::for2doub(currentLine.substr(3,19));
codeL2 = gpstk::StringUtils::for2doub(currentLine.substr(22,19));
toe_wn = gpstk::StringUtils::for2doub(currentLine.substr(41,19));
L2P = gpstk::StringUtils::for2doub(currentLine.substr(60,19));
codeflgs = (short) codeL2;
L2Pdata = (short) L2P;
weeknum = (short) toe_wn;
}
catch (std::exception &e)
{
FFStreamError err("std::exception: " +
string(e.what()));
GPSTK_THROW(err);
}
}
// Dump the overhead information as a string containing a single line.
// @throw Invalid Request if the required data has not been stored.
string GPSEphemeris::asString(void) const
{
if(!dataLoadedFlag)
GPSTK_THROW(InvalidRequest("Data not loaded"));
try {
ostringstream os;
CivilTime ct;
os << "EPH G" << setfill('0') << setw(2) << satID.id << setfill(' ');
ct = CivilTime(beginValid);
os << printTime(ct," | %4Y %3j %02H:%02M:%02S |");
ct = CivilTime(ctToe);
os << printTime(ct," %3j %02H:%02M:%02S |");
ct = CivilTime(ctToc);
os << printTime(ct," %3j %02H:%02M:%02S |");
ct = CivilTime(endValid);
os << printTime(ct," %3j %02H:%02M:%02S |");
ct = CivilTime(transmitTime);
os << printTime(ct," %3j %02H:%02M:%02S | ");
os << setw(3) << IODE << " | " << setw(3) << IODC << " | " << health;
return os.str();
}
catch(Exception& e) { GPSTK_RETHROW(e);
}
}
/* Compute the satellite clock bias (sec) at the given time
*
* @param epoch Epoch to compute satellite clock bias.
*
* @throw InvalidRequest if required data has not been stored.
*/
double GloEphemeris::svClockBias(const CommonTime& epoch) const
throw( gpstk::InvalidRequest )
{
// First, let's check if there is valid data
if(!valid)
{
InvalidRequest exc("svClockBias(): No valid data stored.");
GPSTK_THROW(exc);
}
// Auxiliar object
Xvt sv;
sv.x = x;
sv.v = v;
// In the GLONASS system, 'clkbias' already includes the relativistic
// correction, therefore we must substract the late from the former.
sv.relcorr = sv.computeRelativityCorrection();
sv.clkbias = clkbias + clkdrift * (epoch - ephTime) - sv.relcorr;
return sv.clkbias;
} // End of method 'GloEphemeris::svClockBias(const CommonTime& epoch)'
void fromString(const std::string s)
throw(Exception)
{
char c;
std::istringstream iss(s);
id = -1;
system = systemGPS; // default
if(s.find_first_not_of(std::string(" \t\n"), 0) == std::string::npos)
return; // all whitespace yields the default
iss >> c; // read one character (non-whitespace)
switch(c)
{
// no leading system character
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
iss.putback(c);
system = SatID::systemGPS;
break;
case 'R':
case 'r':
system = SatID::systemGlonass;
break;
case 'T':
case 't':
system = SatID::systemTransit;
break;
case 'S':
case 's':
system = SatID::systemGeosync;
break;
case 'E':
case 'e':
system = SatID::systemGalileo;
break;
case 'M':
case 'm':
system = SatID::systemMixed;
break;
case ' ':
case 'G':
case 'g':
system = SatID::systemGPS;
break;
default: // non-RINEX system character
Exception e(std::string("Invalid system character \"")
+ c + std::string("\""));
GPSTK_THROW(e);
}
iss >> id;
if(id <= 0) id = -1;
}
// -----------------------------------------------------------------------------------
// Compute the satellite attitude, given the time and the satellite position SV.
// If the SolarSystem is valid, use it; otherwise use SolarPosition.
// See two versions of SatelliteAttitude() for the user interface.
// Return a 3x3 Matrix which contains, as rows, the unit (ECEF) vectors X,Y,Z in the
// body frame of the satellite, namely
// Z = along the boresight (i.e. towards Earth center),
// Y = perpendicular to both Z and the satellite-sun direction, and
// X = completing the orthonormal triad. X will generally point toward the sun.
// Thus this rotation matrix R * (ECEF XYZ vector) = components in body frame, and
// R.transpose() * (sat. body. frame vector) = ECEF XYZ components.
// Also return the shadow factor = fraction of sun's area not visible to satellite.
Matrix<double> doSatAtt(const CommonTime& tt, const Position& SV,
const SolarSystem& SSEph, const EarthOrientation& EO,
double& sf)
throw(Exception)
{
try {
int i;
double d,svrange,DistSun,AngRadSun,AngRadEarth,AngSeparation;
Position X,Y,Z,T,S,Sun;
Matrix<double> R(3,3);
// Z points from satellite to Earth center - along the antenna boresight
Z = SV;
Z.transformTo(Position::Cartesian);
svrange = Z.mag();
d = -1.0/Z.mag();
Z = d * Z; // reverse and normalize Z
// get the Sun's position
if(SSEph.JPLNumber() > -1) {
//SolarSystem& mySSEph=const_cast<SolarSystem&>(SSEph);
Sun = const_cast<SolarSystem&>(SSEph).WGS84Position(SolarSystem::Sun,tt,EO);
}
else {
double AR;
Sun = SolarPosition(tt, AR);
}
DistSun = Sun.radius();
// apparent angular radius of sun = 0.2666/distance in AU (deg)
AngRadSun = 0.2666/(DistSun/149598.0e6);
AngRadSun *= DEG_TO_RAD;
// angular radius of earth at sat
AngRadEarth = ::asin(6378137.0/svrange);
// T points from satellite to sun
T = Sun; // vector earth to sun
T.transformTo(Position::Cartesian);
S = SV;
S.transformTo(Position::Cartesian);
T = T - S; // sat to sun=(E to sun)-(E to sat)
d = 1.0/T.mag();
T = d * T; // normalize T
AngSeparation = ::acos(Z.dot(T)); // apparent angular distance, earth
// to sun, as seen at satellite
// is satellite in eclipse?
try { sf = ShadowFactor(AngRadEarth, AngRadSun, AngSeparation); }
catch(Exception& e) { GPSTK_RETHROW(e); }
// Y is perpendicular to Z and T, such that ...
Y = Z.cross(T);
d = 1.0/Y.mag();
Y = d * Y; // normalize Y
// ... X points generally in the direction of the sun
X = Y.cross(Z); // X will be unit vector
if(X.dot(T) < 0) { // need to reverse X, hence Y also
X = -1.0 * X;
Y = -1.0 * Y;
}
// fill the matrix and return it
for(i=0; i<3; i++) {
R(0,i) = X[i];
R(1,i) = Y[i];
R(2,i) = Z[i];
}
return R;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) {Exception E("std except: "+string(e.what())); GPSTK_THROW(E);}
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}
/* Method to check if a given section/variable pair exists.
*
* @param variable Variable name.
* @param section Section the variable belongs to.
*
*/
bool ConfDataReader::ifExist( string variable,
string section )
throw(ConfigurationException)
{
// Let's make sure that section and variable names are uppercase
section = StringUtils::upperCase(section);
variable = StringUtils::upperCase(variable);
// Check if section and variable exist
confMap::const_iterator it;
it = confData.find(section);
if( it != confData.end() )
{
variableMap::const_iterator it2;
it2 = (*it).second.find(variable);
if( it2 != (*it).second.end() )
{
// Return the corresponding value, if it exists
return true;
}
else
{
if(issueException)
{
// Throw an exception if variable name doesn't exist
ConfigurationException e(
"Variable '" + variable
+ "' in section '" + section
+ "' of configuration file '" + filename
+ "' does not exist.");
GPSTK_THROW(e);
}
else
{
return false;
} // End of 'if(issueException)'
} // End of 'if( it2 != (*it).second.end() )'
}
else
{
if(issueException)
{
// Check if problem is with section "DEFAULT"
if ( section == "DEFAULT" )
{
// Throw an exception if section name doesn't exist
ConfigurationException e(
"Section 'DEFAULT' in configuration file '"
+ filename
+ "' does not exist. Does file '"
+ filename + "' exist?. Do you have "
+ "permission to read it?." );
GPSTK_THROW(e);
}
else
{
// Throw an exception if section name doesn't exist
ConfigurationException e(
"Section '" + section
+ "' in configuration file '" + filename
+ "' does not exist.");
GPSTK_THROW(e);
} // End of 'if ( section == "DEFAULT" )'
}
else
{
return false;
} // End of 'if(issueException)'
} // End of 'if( it != confData.end() )'
} // End of method 'ConfDataReader::ifExist()'
// Method to store conf data in this class' data map
void ConfDataReader::loadData(void)
throw(ConfigurationException)
{
// By default, section name is "DEFAULT"
std::string sectionName("DEFAULT");
// Do this until end-of-file reached or something else happens
while(1)
{
try
{
std::string line;
std::string variable;
std::string value;
formattedGetLine(line, true);
// If line is too long, we throw an exception
if (line.size()>255)
{
ConfigurationException e(
"Line too long in configuration file '" +
filename + "'." );
GPSTK_THROW(e);
}
// Skip the blank line
if(line.size()<1) continue;
// Let's find and strip comment lines
if( (StringUtils::firstWord(line)[0] == '#') ||
(StringUtils::firstWord(line)[0] == ';') )
{
formattedGetLine(line, true);
}
// Let's strip comments at the end of lines
std::string::size_type idx(line.find('#'));
if( !(idx == std::string::npos) )
{
line = line.substr(0, idx);
}
idx = line.find(';');
if( !(idx == std::string::npos) )
{
line = line.substr(0, idx);
}
// Remove trailing and leading blanks
line = StringUtils::strip(line);
// Skip blank lines
if (line.size()==0)
{
continue;
}
// Let's start to get data out of file
// First, handle section names
// Test if this line declares a new section. Check for '['
idx = line.find('[');
if( !(idx == std::string::npos) )
{
// Now, check if there is a closing ']'
std::string::size_type idx2(line.find(']'));
if( !(idx2 == std::string::npos) )
{
// Extract name and remove trailing and leading blanks
line = StringUtils::strip( line.substr(idx+1, idx2-idx-1) );
// Check if section name is appropriate
if( checkName(line) )
{
// Update 'sectionName': make it uppercase
sectionName = StringUtils::upperCase(line);
}
else
{
// Throw an exception if section name isn't appropriate
ConfigurationException e(
"Section name '" +
line + "' in configuration file '" +
filename +
"' does not comply with rules.");
GPSTK_THROW(e);
}
// If this was a section line, continue with next line
continue;
}
else
{
// Throw an exception if section line is not closed
ConfigurationException e(
"Section line '" +
line +
"' in configuration file '" +
filename +
"' was improperly closed" );
GPSTK_THROW(e);
}
}
// Second, handle variables
// Separate variable name from value. Look for separators
idx = line.find('=');
if( idx == std::string::npos )
{
idx = line.find(':');
}
// If we found a separator, keep processing
if( !(idx == std::string::npos) )
{
// Read variable and value
variable = StringUtils::strip( line.substr(0, idx) );
value = StringUtils::strip( line.substr(idx+1) );
// Now separate comments
// Work on 'variable'
std::string varComment;
idx = variable.find(',');
if( !(idx == std::string::npos) )
{
varComment = StringUtils::strip(variable.substr(idx+1));
variable = StringUtils::strip(variable.substr(0, idx));
}
// Check if variable name is appropriate
if( checkName(variable) )
{
// Make 'variable' uppercase
variable = StringUtils::upperCase(variable);
}
else
{
// Throw an exception if variable name isn't appropriate
ConfigurationException e(
"Variable name '" +
variable + "' in configuration file '" +
filename +
"' does not comply with rules.");
GPSTK_THROW(e);
}
// Now work on 'value'
std::string valueComment;
idx = value.find(',');
if( !(idx == std::string::npos) )
{
valueComment = StringUtils::strip(value.substr(idx+1));
value = StringUtils::strip(value.substr(0, idx));
}
// Store configuration data
variableData varData;
varData.varComment = varComment;
varData.value = value;
varData.valueComment = valueComment;
confData[sectionName][variable] = varData;
}
} // End of try block
catch (ConfigurationException& e)
{
GPSTK_THROW(e);
}
catch (EndOfFile& e)
{
// Initialize itCurrentSection
itCurrentSection = confData.begin();
return;
}
catch (...)
{
return;
}
} // End of 'while(1)'
} // End of method 'ConfDataReader::loadData()'
/* Method to get the value of a given variable as a boolean
*
* @param variable Variable name.
* @param section Section the variable belongs to.
*
*/
bool ConfDataReader::getValueAsBoolean( string variable,
string section,
bool defaultVal )
throw(ConfigurationException)
{
// Let's make sure that section and variable names are uppercase
section = StringUtils::upperCase(section);
variable = StringUtils::upperCase(variable);
try
{
// Declare result variable
string result( getValue( variable, section ) );
// Test if result is empty (variable does not exist)
if( result == "" )
{
// Return false if variable is empty. Be aware that an empty
// variable is NOT the same as an unexistent variable
return defaultVal;
}
// 'result' isn't empty. Convert it to uppercase
result = StringUtils::upperCase(result);
// Test if it is "TRUE" or "FALSE"
if( result == "TRUE" )
{
return true;
}
else
{
if( result == "FALSE" )
{
return false;
}
else
{
// Throw an exception if value is neither TRUE nor FALSE
ConfigurationException e(
"Variable name '" +
variable + "' in configuration file '" +
filename +
"' is neither TRUE nor FALSE.");
GPSTK_THROW(e);
} // End of 'if( result == "FALSE" )'
} // End of 'if( result == "TRUE" )'
}
catch (ConfigurationException& e)
{
GPSTK_RETHROW(e);
}
} // End of method 'ConfDataReader::getValueAsBoolean()'
void
Data::reallyGetRecord(FFStream& s)
throw(std::exception, FFStreamError, StringUtils::StringException)
{
Sinex::Stream& strm = dynamic_cast<Sinex::Stream&>(s);
bool terminated = false;
string currentBlock;
string line;
blocks.clear();
strm.formattedGetLine(line, true); /// EOF possible
try
{
header = line;
}
catch (Exception& exc)
{
FFStreamError err(exc);
GPSTK_THROW(err);
}
while (strm.good() )
{
try
{
strm.formattedGetLine(line, true); /// EOF possible
}
catch (EndOfFile& e)
{
break;
}
if (line.size() < 1)
{
FFStreamError err("Invalid empty line.");
GPSTK_THROW(err);
}
switch (line[0])
{
case BLOCK_START:
{
if (currentBlock.size() > 0)
{
FFStreamError err("Unexpected start of block.");
GPSTK_THROW(err);
}
currentBlock = line.substr(1);
BlockFactory::iterator i = blockFactory.find(currentBlock);
if (i == blockFactory.end() )
{
string errMsg("Invalid block title: ");
FFStreamError err(errMsg + currentBlock);
GPSTK_THROW(err);
}
BlockCreateFunc createFunc = i->second;
BlockBase *block = createFunc();
if (block)
{
try
{
block->getBlock(strm);
blocks.push_back(block);
}
catch (Exception& exc)
{
FFStreamError err(exc);
GPSTK_THROW(err);
}
}
else
{
string errMsg("Error creating block: ");
FFStreamError err(errMsg + currentBlock);
GPSTK_THROW(err);
}
break;
}
case BLOCK_END:
{
if (currentBlock.size() == 0)
{
FFStreamError err("Unexpected end of block.");
GPSTK_THROW(err);
}
if (currentBlock.compare(line.substr(1) ) != 0)
{
FFStreamError err("Block start and end do not match.");
GPSTK_THROW(err);
}
currentBlock.clear();
break;
}
case DATA_START:
{
FFStreamError err("Missing start of block.");
GPSTK_THROW(err);
break;
}
case HEAD_TAIL_START:
{
if (line.compare("%ENDSNX") == 0)
{
terminated = true;
}
else
{
string errMsg("Invalid line: ");
FFStreamError err(errMsg + line);
GPSTK_THROW(err);
}
break;
}
case COMMENT_START:
{
/// @TODO - Store
break;
}
default:
{
string errMsg("Invalid line start character: ");
FFStreamError err(errMsg + line[0]);
GPSTK_THROW(err);
break;
}
} // switch
}
if (currentBlock.size() > 0)
{
string errMsg("Block not properly terminated: ");
FFStreamError err(errMsg + currentBlock);
GPSTK_THROW(err);
}
if (!terminated)
{
FFStreamError err("File not properly terminated (missing "
+ FILE_END + " )");
GPSTK_THROW(err);
}
} // Data::reallyGetRecord()
/* Get a combination of observations from a Rinex3ObsData object
*
* @param rinexData The Rinex data set holding the observations
* @param indexObs1 Index representing the observation type #1.
* @param indexObs2 Index representing the observation type #2.
*
* @return
* Number of SVs with this combination of observables available
*
* @note
* The indexes are obtained from the RINEX Observation Header
* using method 'Rinex3ObsHeader::getObsIndex()'.
*/
int ExtractCombinationData::getData( const Rinex3ObsData& rinexData,
int indexObs1,
int indexObs2 )
throw(InvalidRequest)
{
try
{
// Let's make sure each time we start with clean Vectors
availableSV.resize(0);
obsData.resize(0);
// Create a CheckPRData object with the given limits
CheckPRData checker(minPRange, maxPRange);
// Let's define the "it" iterator to visit the observations PRN map
// RinexSatMap is a map from SatID to RinexObsTypeMap:
// std::map<SatID, RinexObsTypeMap>
Rinex3ObsData::DataMap::const_iterator it;
for ( it = rinexData.obs.begin();
it != rinexData.obs.end();
it++ )
{
// The satellites are stored in the first elements of the map...
SatID sat(it->first);
// .. and vectors of available obs are in the second elements
vector<RinexDatum> vecData(it->second);
// Extract observation values
double obsValue1( (vecData[indexObs1]).data );
double obsValue2( (vecData[indexObs2]).data );
// Compute the combination
double combinationValue( getCombination( obsValue1, obsValue2 ) );
// Let's check if the combination is between the limits
if ( !(checkData) || (checker.check(combinationValue)) )
{
// Store all relevant data of this epoch
availableSV = availableSV && sat;
obsData = obsData && combinationValue;
}
} // End of data combination extraction from this epoch
}
catch(...)
{
InvalidRequest e("Unable to compute combination from Rinex3ObsData object");
GPSTK_THROW(e);
}
// Let's record the number of SV with this type of data available
numSV = (int)obsData.size();
// If everything is fine so far, then the results should be valid
valid = true;
return numSV;
}; // End of method 'ExtractCombinationData::getData()'
/* Determines if the satellite is in sunlight or shadow.
* Taken from Montenbruck and Gill p. 80-83
* @param r ECI position vector of spacecraft [m].
* @param r_Sun Sun position vector (geocentric) [m].
* @param r_Moon Moon position vector (geocentric) [m].
* @return 0.0 if in shadow, 1.0 if in sunlight, 0 to 1.0 if in partial shadow
*/
double SolarRadiationPressure::getShadowFunction(Vector<double> r,
Vector<double> r_Sun,
Vector<double> r_Moon,
SolarRadiationPressure::ShadowModel sm)
{
// shadow function
double v = 0.0;
// Mean Radious of Sun, Moon and Earth
const double R_sun = ASConstant::R_Sun;
const double R_earth = ASConstant::R_Earth;
Vector<double> e_Sun = r_Sun/norm(r_Sun); // Sun direction unit vector
double r_dot_sun = dot(r,e_Sun);
if(r_dot_sun>0)
{
// Sunny side of central body is always fully lit and return
v= 1.0;
return v;
}
if(sm == SM_CYLINDRICAL)
{
// Taken fram Jisheng Li P111, and checked with GMAT and Bernese5 SHADOW.f
v = ((r_dot_sun>0 || norm(r-r_dot_sun*e_Sun)>R_earth) ? 1.0 : 0.0);
return v;
}
else if(sm == SM_CONICAL)
{
/*
// Taken from Montenbruck and Gill p. 80-83
double s0, s2;
// Montenbruck and Gill, eq. 3.79
s0 = -dot(r,e_Sun); //-state[0]*unitsun[0] - state[1]*unitsun[1] - state[2]*unitsun[2];
s2 = dot(r,r); //state[0]*state[0] + state[1]*state[1] + state[2]*state[2];
// Montenbruck and Gill, eq. 3.80
double lsc = sqrt(s2 - s0*s0);
// Montenbruck and Gill, eq. 3.81
double sinf1 = (R_sun + R_earth) / norm(r_Sun);
double sinf2 = (R_earth - R_earth) / norm(r_Sun);
// Appropriate l1 and l2 temporarily
double t1 = sinf1 * sinf1;
double t2 = sinf2 * sinf2;
double tanf1 = sqrt(t1 / (1.0 - t1));
double tanf2 = sqrt(t2 / (1.0 - t2));
// Montenbruck and Gill, eq. 3.82
double c1 = s0 + R_earth / sinf1;
double c2 = R_earth / sinf2 - s0; // Different sign from M&G
// Montenbruck and Gill, eq. 3.83
double l1 = c1 * tanf1;
double l2 = c2 * tanf2;
if (lsc > l1) // Outside of the penumbral cone
{
v = 1.0;
return v;
}
else
{
//lit = false;
if (lsc < fabs(l2))
{
// Inside umbral cone
if (c2 >= 0.0)
{
// no annular ring
percentSun = 0.0;
dark = true;
}
else
{
// annular eclipse
pcbrad = asin(R_earth / sqrt(s2));
percentSun = (psunrad*psunrad - pcbrad*pcbrad) /
(psunrad*psunrad);
dark = false;
}
return;
}
// In penumbra
pcbrad = asin(R_earth / sqrt(s2));
percentSun = ShadowFunction(state);
lit = false;
dark = false;
}*/
//////////////////////////////////////////////////////////////////////////
double r_mag = norm(r);
Vector<double> d = r_Sun-r; // vector from sc to sun
double dmag = norm(d);
double a = std::asin(R_sun/dmag); // eq. 3.85
double b = std::asin(R_earth/r_mag); // eq. 3.86
double c = std::acos(-1.0*dot(r,d)/(r_mag*dmag)); // eq. 3.87
if((a+b)<=c) // in Sun light
{
v = 1.0;
}
else if(c < (b-a)) // in Umbra
{
v = 0.0;
}
else // in Penumbra
{
double x = (c*c+a*a-b*b)/(2*c); // eq. 3.93
double y = std::sqrt(a*a-x*x);
double A = a*a*std::acos(x/a)+b*b*std::acos((c-x)/b)-c*y; // eq. 3.92
v = 1.0 - A/(ASConstant::PI*a*a); // eq. 3.94
}
return v;
}
else
{
// unexpected value
Exception e("Unexpect ShadowModel in getShadowFunction()");
GPSTK_THROW(e);
}
return v;
} // End of method 'SolarRadiationPressure::getShadowFunction()'
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;
*/
}
}
/* Returns a satTypeValueMap object, adding the new data
* generated when calling this object.
*
* @param time Epoch corresponding to the data.
* @param gData Data object holding the data.
*/
satTypeValueMap& ComputeSimpleWeights::Process( const CommonTime& time,
satTypeValueMap& gData )
throw(ProcessingException)
{
try
{
// If we are using a 5th order Taylor-based differencing filter, the
// corresponding scale factor to convert from covariance matrix to
// double-differenced covariance matrix is 1.509551839.
double scaleFact( 1.509551839 );
// Declare some important constants
double tropoVar( 0.0004 ); // (0.02 m)^2
double multiVar( 0.000025 ); // (0.005 m)^2
// We need a NBTropModel initialized with dummy values
NBTropModel tropoObj(0.0, 0.0, 1);
SatIDSet satRejectedSet;
// Loop through all the satellites
for( satTypeValueMap::iterator it = gData.begin();
it != gData.end();
++it )
{
double elevP( 0.0 );
try
{
elevP = gData.getValue( (*it).first, TypeID::elevation );
}
catch(...)
{
// If some value is missing, then schedule this satellite
// for removal
satRejectedSet.insert( (*it).first );
continue;
}
// If everything is OK, then compute the weight value and
// put it into the GDS structure
double mt( tropoObj.dry_mapping_function(elevP) );
double weight( 1.0 / ( scaleFact*( mt*mt*tropoVar + multiVar ) ) );
(*it).second[TypeID::weight] = weight;
}
// Remove satellites with missing data
gData.removeSatID(satRejectedSet);
return gData;
}
catch(Exception& u)
{
// Throw an exception if something unexpected happens
ProcessingException e( getClassName() + ":"
+ u.what() );
GPSTK_THROW(e);
}
} // End of method 'ComputeSimpleWeightsWeights::Process()'
int ARLambda::search( const Matrix<double>& L,
const Vector<double>& D,
const Vector<double>& zs,
Matrix<double>& zn,
Vector<double>& s,
const int& m )
{
ARException e("The LAMBDA search method have not been implemented.");
GPSTK_THROW(e);
// TODO: CHECK UNEXPECTED INPUT
// n - number of float parameters
// m - number of fixed solutions
// L - nxn
// D - nx1
// zs - nxn
// zn - nxm
// s - m
const int LOOPMAX = 10000;
const int n = L.rows();
zn.resize(n,m,0.0);
s.resize(m,0.0);
Matrix<double> S(n,n,0.0);
Vector<double> dist(n,0.0),zb(n,0.0),z(n,0.0),step(n,0.0);
int k=n-1; dist[k]=0.0;
zb(k)=zs(k);
z(k)=round(zb(k));
double y=zb(k)-z(k);
step(k)=sign(y);
int c(0),nn(0),imax(0);
double maxdist=1E99;
for(int c=0;c<LOOPMAX;c++)
{
double newdist=dist(k)+y*y/D(k);
if(newdist<maxdist)
{
if(k!=0)
{
dist(--k)=newdist;
for(int i=0;i<=k;i++)
{
S(k,i)=S(k+1,i)+(z(k+1)-zb(k+1))*L(k+1,i);
}
zb(k)=zs(k)+S(k,k);
z(k)=round(zb(k)); y=zb(k)-z(k); step(k)=sign(y);
}
else
{
if(nn<m)
{
if(nn==0||newdist>s(imax)) imax=nn;
for(int i=0;i<n;i++) zn(i,nn)=z(i);
s(nn++)=newdist;
}
else
{
if(newdist<s(imax))
{
for(int i=0;i<n;i++) zn(i,imax)=z(i);
s(imax)=newdist;
for(int i=imax=0;i<m;i++) if (s(imax)<s(i)) imax=i;
}
maxdist=s(imax);
}
z(0)+=step(0); y=zb(0)-z(0); step(0)=-step(0)-sign(step(0));
}
}
else
{
if(k==n-1) break;
else
{
k++;
z(k)+=step(k); y=zb(k)-z(k); step(k)=-step(k)-sign(step(k));
}
}
}
for(int i=0;i<m-1;i++)
{
for(int j=i+1;j<m;j++)
{
if(s(i)<s(j)) continue;
swap(s(i),s(j));
for(k=0;k<n;k++) swap(zn(k,i),zn(k,j));
}
}
if (c>=LOOPMAX)
{
return -1;
}
return 0;
} // End of method 'ARLambda::search()'
/* Returns the position, velocity and clock offset of the indicated
* satellite in ECEF coordinates (meters) at the indicated time,
* in the PZ-90 ellipsoid.
*
* @param[in] sat Satellite's identifier
* @param[in] epoch Time to look up
*
* @return the Xvt of the object at the indicated time
*
* @throw InvalidRequest If the request can not be completed for any
* reason, this is thrown. The text may have additional information
* as to why the request failed.
*/
Xvt GloEphemerisStore::getXvt( const SatID& sat,
const CommonTime& epoch ) const
{
// TD is this too strict?
if(epoch.getTimeSystem() != initialTime.getTimeSystem())
{
InvalidRequest e(string("Requested time system is not GLONASS time"));
GPSTK_THROW(e);
}
// Check that the given epoch is within the available time limits.
// We have to add a margin of 15 minutes (900 seconds).
if ( epoch < (initialTime - 900.0) ||
epoch > (finalTime + 900.0) )
{
InvalidRequest e( "Requested time is out of boundaries for satellite "
+ StringUtils::asString(sat) );
GPSTK_THROW(e);
}
// Look for the satellite in the 'pe' (EphMap) data structure.
GloEphMap::const_iterator svmap = pe.find(sat);
// If satellite was not found, issue an exception
if (svmap == pe.end())
{
InvalidRequest e( "Ephemeris for satellite "
+ StringUtils::asString(sat) + " not found." );
GPSTK_THROW(e);
}
// Let's take the second part of the EphMap
const TimeGloMap& sem = svmap->second;
// Look for 'i': the first element whose key >= epoch.
TimeGloMap::const_iterator i = sem.lower_bound(epoch);;
// Values to be returned will be stored here
Xvt sv;
// If we reached the end, the requested time is beyond the last
// ephemeris record, but it may still be within the allowable time
// span, so we can use the last record.
if ( i == sem.end() )
{
i = --i;
}
// If key > (epoch+900), we must use the previous record if possible.
if ( ( i->first > (epoch+900.0) ) && ( i != sem.begin() ) )
{
i = --i;
}
// Check that the given epoch is within the available time limits for
// this specific satellite, with a margin of 15 minutes (900 seconds).
if ( epoch < (i->first - 900.0) ||
epoch >= (i->first + 900.0) )
{
InvalidRequest e( "Requested time is out of boundaries for satellite "
+ StringUtils::asString(sat) );
GPSTK_THROW(e);
}
// We now have the proper reference data record. Let's use it
GloEphemeris data( i->second );
// Compute the satellite position, velocity and clock offset
sv = data.svXvt( epoch );
// We are done, let's return
return sv;
}; // End of method 'GloEphemerisStore::getXvt()'
