// Compute and return the full tropospheric delay. The receiver height,
// latitude and time must has been set before using the appropriate
// methods.
// @param elevation Elevation of satellite as seen at receiver, in degrees
double GlobalTropModel::correction(double elevation) const
throw(InvalidTropModel)
{
try { testValidity(); }
catch(InvalidTropModel& e) { GPSTK_RETHROW(e); }
// Global mapping functions good down to 3 degrees of elevation
if(elevation < 3.0) { return 0.0; }
double map_dry(GlobalTropModel::dry_mapping_function(elevation));
double map_wet(GlobalTropModel::wet_mapping_function(elevation));
// Compute total tropospheric delay
double tropDelay((GlobalTropModel::dry_zenith_delay() * map_dry) +
(GlobalTropModel::wet_zenith_delay() * map_wet));
return tropDelay;
} // end GlobalTropModel::correction(elevation)
// Re-define the tropospheric model with explicit weather data.
// Typically called just before correction().
// @param wx the weather to use for this correction
void NBTropModel::setWeather(const WxObservation& wx)
throw(InvalidParameter)
{
interpolateWeather = false;
try
{
TropModel::setWeather(wx);
// humid actually stores vapor partial pressure
double th=300./temp;
humid = 2.409e9*humid*th*th*th*th*std::exp(-22.64*th);
validWeather = true;
valid = validWeather && validRxHeight && validRxLatitude && validDOY;
}
catch(InvalidParameter& e)
{
valid = validWeather = false;
GPSTK_RETHROW(e);
}
}
/// Incomplete beta function I_x(a,b), 0<=x<=1, a,b>0
/// I sub x (a,b) = (1/beta(a,b)) integral (0 to x) { t^(a-1)*(1-t)^(b-1)dt }
/// @param x input value, 0 <= x <= 1
/// @param a input value, a > 0
/// @param b input value, b > 0
/// @return Incomplete beta function I_x(a,b)
double incompleteBeta(const double& x, const double& a, const double& b)
throw(Exception)
{
if(x < 0 || x > 1) GPSTK_THROW(Exception("Invalid x argument"));
if(a <= 0 || b <= 0) GPSTK_THROW(Exception("Non-positive argument"));
if(x == 0) return 0.0;
if(x == 1) return 1.0;
try {
double factor = ::exp(lnGamma(a+b) - lnGamma(a) - lnGamma(b)
+ a * ::log(x) + b * ::log(1.0-x));
if(x < (a+1.0)/(a+b+2.0))
return factor*cfIBeta(x,a,b)/a;
else
return 1.0-factor*cfIBeta(1.0-x,b,a)/b;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}
// -------------------------------------------------------------
/// the smoother update
virtual void KalmanSmootherUpdate(void) throw(gpstk::Exception)
{
try {
NTU--;
//LOG(DEBUG) << " SU at " << NTU << " with state " << srif.getNames();
gpstk::Matrix<double> Rw = RwStore[NTU];
gpstk::Matrix<double> Rwx = RwxStore[NTU];
gpstk::Matrix<double> PhiInv = PhiInvStore[NTU];
gpstk::Matrix<double> G = GStore[NTU];
gpstk::Vector<double> Zw = ZwStore[NTU];
// SU knows nothing about time; this is just for output purposes
time = TimeStore[NTU];
// should Control vector correction be here???
if(doSRISU) {
gpstk::Matrix<double> Phi;
Phi = inverse(PhiInv);
srif.smootherUpdate(Phi,Rw,G,Zw,Rwx);
inverted = false;
}
else {
srif.DMsmootherUpdate(Cov,State,PhiInv,Rw,G,Zw,Rwx);
}
// correct for Control vector
if(ControlStore.size() > 0) {
gpstk::Vector<double> Control = ControlStore[NTU];
if(doSRISU) {
srif.shift(PhiInv*Control);
}
else {
State -= PhiInv * Control;
}
}
}
catch(gpstk::Exception& e) {
e.addText("KSU");
GPSTK_RETHROW(e);
}
}
/// Factorial of an integer, returned as a double.
/// @param n argument, n must be >= 0
/// @return n! or factorial(n), as a double
/// @throw if the input argument is < 0
double factorial(const int& n) throw(Exception)
{
try {
if(n < 0) GPSTK_THROW(Exception("Negative argument"));
if(n > 32) return ::exp(lnGamma(double(n+1)));
static double store[33] = { 1.0, 1.0, 2.0, 6.0, 24.0, 120.0 };
static int nstore=5;
while(nstore < n) {
int i = nstore++;
store[nstore] = store[i] * nstore;
}
return store[n];
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}
// Dump the orbit, etc information to the given output stream.
// @throw Invalid Request if the required data has not been stored.
void GPSEphemeris::dumpBody(std::ostream& os) const
{
try {
OrbitEph::dumpBody(os);
os << " GPS-SPECIFIC PARAMETERS\n"
<< scientific << setprecision(8)
<< "Tgd (L1/L2) : " << setw(16) << Tgd << " meters" << endl
<< "HOW time : " << setw(6) << HOWtime << " (sec of GPS week "
<< setw(4) << static_cast<GPSWeekSecond>(ctToe).getWeek() << ")"
<< " fitDuration: " << setw(2) << fitDuration << " hours" << endl
<< "TransmitTime: " << OrbitEph::timeDisplay(transmitTime) << endl
<< "Accuracy : flag(URA): " << accuracyFlag << " => "
<< fixed << setprecision(2) << getAccuracy() << " meters" << endl
<< "IODC: " << IODC << " IODE: " << IODE << " health: " << health
<< " (0=good) codeflags: " << codeflags << " L2Pdata: " << L2Pdata
<< endl;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}
/// Natural log of the gamma function for positive argument.
/// Gamma(x) = integral(0 to inf) { t^(x-1) exp(-t) dt }
/// @param x argument, x must be > 0
/// @return double ln(gamma(x)), the natural log of the gamma function of x.
/// @throw if the input argument is <= 0
double lnGamma(const double& x) throw(Exception)
{
try {
static const double con[8] = {
76.18009172947146, -86.50532032941677, 24.01409824083091,
-1.231739572450155, 1.208650973866179e-3, -5.395239384953e-6,
1.000000000190015, 2.5066282746310005 };
if(x <= 0) GPSTK_THROW(Exception("Non-positive argument"));
double y(x);
double t(x+5.5);
t -= (x+0.5) * ::log(t);
double s(con[6]);
for(int i=0; i<=5; i++) s += con[i] / ++y;
return (-t + ::log(con[7]*s/x));
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}
// Dump the orbit, etc information to the given output stream.
// @throw Invalid Request if the required data has not been stored.
void BDSEphemeris::dumpBody(std::ostream& os) const
{
try {
OrbitEph::dumpBody(os);
os << " BeiDou-SPECIFIC PARAMETERS\n"
<< scientific << setprecision(8)
<< "Tgd (B1/B3) : " << setw(16) << Tgd13 << " meters" << endl
<< "Tgd (B2/B3) : " << setw(16) << Tgd23 << " meters" << endl
<< "HOW time : " << setw(6) << HOWtime << " (sec of BDS week "
<< setw(4) << static_cast<BDSWeekSecond>(ctToe).getWeek() << ")"
<< " fitDuration: " << setw(2) << fitDuration << " hours" << endl
<< "TransmitTime: " << OrbitEph::timeDisplay(transmitTime) << endl
<< "Accuracy : " << fixed << setprecision(2)
<< getAccuracy() << " meters" << endl
<< "IODC: " << IODC << " IODE: " << IODE << " health: " << health
<< endl;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}
// Compute and return the mapping function for wet component of the
// troposphere.
// param elevation Elevation of satellite as seen at receiver,
//
// computing the derivative - do it numerically instead
// note that sin[elev] = cos[zenith]
// f(1) a
// map = ------------ where f(x) = x + ---------
// f(sin[elev]) b
// x + -----
// x + c
//
// a (x+c) x(x^2 + xc + b) + a(x+c)
// f(x) = x + ------------ = ------------------------
// x^2 + xc + b x^2 + xc + b
//
// x^3 + x^2 c + x(a+b) + ac
// = -------------------------
// x^2 + xc + b
//
// so -f(1)f'(x) -map(x)f'(x) N'D-D'N
// map'(x) = ---------- = -------------, where f'(x) = -------
// f^2(x) f(x) D^2
//
// [3x^2+2xc+a+b][x^2+xc+b]-[2x+c][x^3+x^2c+x(a+b)+ac]
// f'(x) = x' --------------------------------------------------------------
// [x^2 + xc + b]^2
//
// [3x^4 + x^3(5c) + x^2(a+4b+2c^2) + x(a+b+2bc) + ab+b^2]
// + [-2x^4 - x^3(3c) - x^2(2a+2b+c^2) - x(c(3a+b)) - ac^2]
// = x' ----------------------------------------------------------
// x^4 + x^3(2c) + x^2(2b+c^2) + x(2bc) + b^2
//
// x^4 + x^3(2c) + x^2(-a+2b+c^2) + x(a+b+3bc+3ac) + ab+b^2-ac^2
// = x' -------------------------------------------------------------
// x^4 + x^3(2c) + x^2(2b+c^2) + x(2bc) + b^2
//
double GlobalTropModel::wet_mapping_function(double elevation) const
throw(InvalidTropModel)
{
try { testValidity(); }
catch(InvalidTropModel& e) { GPSTK_RETHROW(e); }
if(elevation < 3.0) { return 0.0; }
static const double bw = 0.00146;
static const double cw = 0.04391;
double am(0.0), aa(0.0);
for(int i=0; i<55; i++) {
am += (AWetMean[i]*aP[i] + BWetMean[i]*bP[i]) * 1.0e-5;
aa += (AWetAmp[i]*aP[i] + BWetAmp[i]*bP[i]) * 1.0e-5;
}
double aw = am + aa*::cos(dayfactor);
double sine = ::sin(elevation*DEG_TO_RAD);
//std::cout << "sine " << std::fixed << std::setprecision(16) << sine
// << std::endl;
//std::cout << "aw bw cw " << std::fixed << std::setprecision(16)
// << aw << " " << bw << " " << cw << std::endl;
double f1(1.0 + aw/(1.0 + bw/(1.0 + cw)));
double f(sine + aw/(sine + bw/(sine + cw)));
double map = f1/f;
//// NB might be easier numerically... map' = map(elev+eps)-map(elev-eps)/2eps
//if(doDeriv) {
// double apb(aw+bw);
// double cw2(cw*cw);
// double tmp(sine*(sine + 2*cw) + 2*bw+cw2);
// double fpN(sine*(sine*(tmp-aw) + apb*(1.+3*cw)) + bw*apb-aw*cw2);
// double fpD(sine*(sine*tmp + 2*bw));
// double sinep(::cos(elevation*DEG_TO_RAD));
// deriv = - map * (sinep*fpN/fpD) / f;
//}
return map;
} // end GlobalTropModel::wet_mapping_function()
//------------------------------------------------------------------------------------
// Consider the sun and the earth as seen from the satellite. Let the sun be a circle
// of angular radius r, center in direction s, and the earth be a (larger) circle
// of angular radius R, center in direction e. The circles overlap if |e-s| < R+r;
// complete overlap if |e-s| < R-r. The satellite is in penumbra if R-r < |e-s| < R+r,// it is in umbra if |e-s| < R-r.
// Let L == |e-s|. What is the area of overlap in penumbra : R-r < L < R+r ?
// Call the two points where the circles intersect p1 and p2. Draw a line from e to s;
// call the points where this line intersects the two circles r1 and R1, respectively.
// Draw lines from e to s, e to p1, e to p2, s to p1 and s to p2. Call the angle
// between e-s and e-p1 alpha, and that between s-e and s-p1, beta.
// Draw a rectangle with top and bottom parallel to e-s passing through p1 and p2,
// and with sides passing through s and r1. Similarly for e and R1. Note that the
// area of intersection lies within the intersection of these two rectangles.
// Call the area of the rectangle outside the circles A and B. The height H of the
// rectangles is
// H = 2Rsin(alpha) = 2rsin(beta)
// also L = rcos(beta)+Rcos(alpha)
// The area A will be the area of the rectangle
// minus the area of the wedge formed by the angle 2*alpha
// minus the area of the two triangles which meet at s :
// A = RH - (2alpha/2pi)*pi*R*R - 2*(1/2)*(H/2)Rcos(alpha)
// Similarly
// B = rH - (2beta/2pi)*pi*r*r - 2*(1/2)*(H/2)rcos(beta)
// The area of intersection will be the area of the rectangular intersection
// minus the area A
// minus the area B
// Intersection = H(R+r-L) - A - B
// = HR+Hr-HL -HR+alpha*R*R+(H/2)Rcos(alpha) -Hr+beta*r*r+(H/2)rcos(beta)
// Cancel terms, and substitute for L using above equation L = ..
// = -(H/2)rcos(beta)-(H/2)Rcos(alpha)+alpha*R*R+beta*r*r
// substitute for H/2
// = -R*R*sin(alpha)cos(alpha)-r*r*sin(beta)cos(beta)+alpha*R*R+beta*r*r
// Intersection = R*R*[alpha-sin(alpha)cos(alpha)]+r*r*[beta-sin(beta)cos(beta)]
// Solve for alpha and beta in terms of R, r and L using the H and L relations above
// (r/R)cos(beta)=(L/R)-cos(alpha)
// (r/R)sin(beta)=sin(alpha)
// so
// (r/R)^2 = (L/R)^2 - (2L/R)cos(alpha) + 1
// cos(alpha) = (R/2L)(1+(L/R)^2-(r/R)^2)
// cos(beta) = (L/r) - (R/r)cos(alpha)
// and 0 <= alpha or beta <= pi
//
// AngRadEarth angular radius of the earth as seen at the satellite
// AngRadSun angular radius of the sun as seen at the satellite
// AngSeparation angular distance of the sun from the earth
// return fraction (0 <= f <= 1) of area of sun covered by earth
// units only need be consistent
double ShadowFactor(double AngRadEarth, double AngRadSun, double AngSeparation)
{
try {
if(AngSeparation >= AngRadEarth+AngRadSun) return 0.0;
if(AngSeparation <= fabs(AngRadEarth-AngRadSun)) return 1.0;
double r=AngRadSun, R=AngRadEarth, L=AngSeparation;
if(AngRadSun > AngRadEarth) { r=AngRadEarth; R=AngRadSun; }
double cosalpha = (R/L)*(1.0+(L/R)*(L/R)-(r/R)*(r/R))/2.0;
double cosbeta = (L/r) - (R/r)*cosalpha;
double sinalpha = ::sqrt(1-cosalpha*cosalpha);
double sinbeta = ::sqrt(1-cosbeta*cosbeta);
double alpha = ::asin(sinalpha);
double beta = ::asin(sinbeta);
double shadow = r*r*(beta-sinbeta*cosbeta)+R*R*(alpha-sinalpha*cosalpha);
shadow /= ::acos(-1.0)*AngRadSun*AngRadSun;
return shadow;
}
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); }
}
//------------------------------------------------------------------------------------
// Consider the sun and the earth as seen from the satellite. Let the sun be a circle
// of angular radius r, center in direction s, and the earth be a (larger) circle
// of angular radius R, center in direction e. The circles overlap if |e-s| < R+r;
// complete overlap if |e-s| < R-r. The satellite is in penumbra if R-r < |e-s| < R+r,// it is in umbra if |e-s| < R-r.
// Let L == |e-s|. What is the area of overlap in penumbra : R-r < L < R+r ?
// Call the two points where the circles intersect p1 and p2. Draw a line from e to s;
// call the points where this line intersects the two circles r1 and R1, respectively.
// Draw lines from e to s, e to p1, e to p2, s to p1 and s to p2. Call the angle
// between e-s and e-p1 alpha, and that between s-e and s-p1, beta.
// Draw a rectangle with top and bottom parallel to e-s passing through p1 and p2,
// and with sides passing through s and r1. Similarly for e and R1. Note that the
// area of intersection lies within the intersection of these two rectangles.
// Call the area of the rectangle outside the circles A and B. The height H of the
// rectangles is
// H = 2Rsin(alpha) = 2rsin(beta)
// also L = rcos(beta)+Rcos(alpha)
// The area A will be the area of the rectangle
// minus the area of the wedge formed by the angle 2*alpha
// minus the area of the two triangles which meet at s :
// A = RH - (2alpha/2pi)*pi*R*R - 2*(1/2)*(H/2)Rcos(alpha)
// Similarly
// B = rH - (2beta/2pi)*pi*r*r - 2*(1/2)*(H/2)rcos(beta)
// The area of intersection will be the area of the rectangular intersection
// minus the area A
// minus the area B
// Intersection = H(R+r-L) - A - B
// = HR+Hr-HL -HR+alpha*R*R+(H/2)Rcos(alpha) -Hr+beta*r*r+(H/2)rcos(beta)
// Cancel terms, and substitute for L using above equation L = ..
// = -(H/2)rcos(beta)-(H/2)Rcos(alpha)+alpha*R*R+beta*r*r
// substitute for H/2
// = -R*R*sin(alpha)cos(alpha)-r*r*sin(beta)cos(beta)+alpha*R*R+beta*r*r
// Intersection = R*R*[alpha-sin(alpha)cos(alpha)]+r*r*[beta-sin(beta)cos(beta)]
// Solve for alpha and beta in terms of R, r and L using the H and L relations above
// (r/R)cos(beta)=(L/R)-cos(alpha)
// (r/R)sin(beta)=sin(alpha)
// so
// (r/R)^2 = (L/R)^2 - (2L/R)cos(alpha) + 1
// cos(alpha) = (R/2L)(1+(L/R)^2-(r/R)^2)
// cos(beta) = (L/r) - (R/r)cos(alpha)
// and 0 <= alpha or beta <= pi
//
// Rearth angular radius of the earth as seen at the satellite
// Rsun angular radius of the sun as seen at the satellite
// dES angular distance of the sun from the earth
// return fraction (0 <= f <= 1) of area of sun covered by earth
// units only need be consistent
double shadowFactor(double Rearth, double Rsun, double dES)
{
try {
if(dES >= Rearth+Rsun) return 0.0;
if(dES <= fabs(Rearth-Rsun)) return 1.0;
double r=Rsun, R=Rearth, L=dES;
if(Rsun > Rearth) { r=Rearth; R=Rsun; }
double cosalpha = (R/L)*(1.0+(L/R)*(L/R)-(r/R)*(r/R))/2.0;
double cosbeta = (L/r) - (R/r)*cosalpha;
double sinalpha = ::sqrt(1-cosalpha*cosalpha);
double sinbeta = ::sqrt(1-cosbeta*cosbeta);
double alpha = ::asin(sinalpha);
double beta = ::asin(sinbeta);
double shadow = r*r*(beta-sinbeta*cosbeta)+R*R*(alpha-sinalpha*cosalpha);
shadow /= ::acos(-1.0)*Rsun*Rsun;
return shadow;
}
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); }
}
/// Chi-square-distribution percent point function, or inverse of the Chisq CDF.
/// This function(alpha,N) == Y where alpha = ChisqCDF(Y,N).
/// Ref http://www.itl.nist.gov/div898/handbook/ 1.3.6.6.6
/// @param alpha probability or significance level of the test, >=0 and < 1
/// @param n degrees of freedom of sample, n > 0
/// @return X the statistic (an RSS of variances) at this probability
double invChisqCDF(double alpha, int n) throw(Exception)
{
try {
if(alpha < 0 || alpha >= 1)
GPSTK_THROW(Exception("Invalid probability argument"));
if(n <= 0)
GPSTK_THROW(Exception("Non-positive degree of freedom"));
static const double eps(1000000*std::numeric_limits<double>().epsilon());
if(alpha < eps)
return 0.0;
if(1.0-alpha < eps)
GPSTK_THROW(Exception("Invalid probability -- too close to 1.0"));
// find X such that alpha == ChisqCDF(X,n); use bracket method
// we know a0 = ChisqCDF(F0,n) where a0=F0=0
double X,X0(0.0),X1,a;
// first find X1 such that a1 = ChisqCDF(X1,N) and a1 > alpha
X1 = 2.0;
while((a = ChisqCDF(X1,n)) <= alpha) { X1 *= 2.0; }
// bracket
int niter(0); // iteration count
while(1) {
X = (X0+X1)/2.0;
a = ChisqCDF(X,n);
//LOG(INFO) << "LOOP invChisqCDF X = " << niter << " " << std::fixed
//<< std::setprecision(15) << X0 << " < " << X << " < " << X1
//<< " and a = " << a << " alpha = " << alpha << " a-alpha = "
//<< std::scientific << std::setprecision(2) << a-alpha << " eps " << eps
//<< " fabs(a-alpha)-eps " << ::fabs(alpha-a)-eps;
if(::fabs(alpha-a) < eps) break;
if(a > alpha) { X1 = X; } else { X0 = X; }
if(++niter > 100) GPSTK_THROW(Exception("Failed to converge"));
}
return X;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}
// -----------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
// Compute the azimuth and nadir angle, in the satellite body frame,
// of receiver Position RX as seen at the satellite Position SV. The nadir angle
// is measured from the Z axis, which points to Earth center, and azimuth is
// measured from the X axis.
// @param Position SV Satellite position
// @param Position RX Receiver position
// @param Matrix<double> Rot Rotation matrix (3,3), output of SatelliteAttitude
// @param double nadir Output nadir angle in degrees
// @param double azimuth Output azimuth angle in degrees
void SatelliteNadirAzimuthAngles(const Position& SV,
const Position& RX,
const Matrix<double>& Rot,
double& nadir,
double& azimuth)
throw(Exception)
{
try {
if(Rot.rows() != 3 || Rot.cols() != 3) {
Exception e("Rotation matrix invalid");
GPSTK_THROW(e);
}
double d;
Position RmS;
// RmS points from satellite to receiver
RmS = RX - SV;
RmS.transformTo(Position::Cartesian);
d = RmS.mag();
if(d == 0.0) {
Exception e("Satellite and Receiver Positions identical");
GPSTK_THROW(e);
}
RmS = (1.0/d) * RmS;
Vector<double> XYZ(3),Body(3);
XYZ(0) = RmS.X();
XYZ(1) = RmS.Y();
XYZ(2) = RmS.Z();
Body = Rot * XYZ;
nadir = ::acos(Body(2)) * RAD_TO_DEG;
azimuth = ::atan2(Body(1),Body(0)) * RAD_TO_DEG;
if(azimuth < 0.0) azimuth += 360.;
}
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); }
}
T Median(T *xd, const int nd, bool save_flag=true)
throw(Exception)
{
if(!xd || nd < 2) {
Exception e("Invalid input");
GPSTK_THROW(e);
}
try {
int i;
T med, *save;
if(save_flag) {
save = new T[nd];
if(!save) {
Exception e("Could not allocate temporary array");
GPSTK_THROW(e);
}
for(i=0; i<nd; i++) save[i]=xd[i];
}
QSort(xd,nd);
if(nd%2)
med = xd[(nd+1)/2-1];
else
med = (xd[nd/2-1]+xd[nd/2])/T(2);
// restore original data from temporary
if(save_flag) {
for(i=0; i<nd; i++) xd[i]=save[i];
delete[] save;
}
return med;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
} // end Median
CommonTime WeekSecond::convertToCommonTime() const
{
try
{
//int dow = static_cast<int>( sow * DAY_PER_SEC );
// Appears to have rounding issues on 32-bit platforms
int dow = static_cast<int>( sow / SEC_PER_DAY );
// NB this assumes MJDEpoch is an integer - what if epoch H:M:S != 0:0:0 ?
long jday = MJD_JDAY + MJDEpoch() + (7 * week) + dow;
double sod(sow - SEC_PER_DAY * dow);
CommonTime ct;
return ct.set( jday,
static_cast<long>(sod),
sod - static_cast<long>(sod),
timeSystem );
}
catch (InvalidParameter& ip)
{
GPSTK_RETHROW(ip);
}
}
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;
}
/// Change the file system, keeping fileType, fileSys, and fileSysSat
/// consistent.
/// @param string str beginning with system character or "M" for mixed
void setFileSystem(const std::string& str) throw(Exception)
{
try {
if(str[0] == 'M' || str[0] == 'm') {
if(version < 3) {
Exception e("RINEX version 2 'Mixed' Nav files do not exist");
GPSTK_THROW(e);
}
fileType = "NAVIGATION";
fileSys = "MIXED";
fileSysSat = SatID(-1, SatID::systemMixed);
}
else {
RinexSatID sat(std::string(1,str[0]));
fileSysSat = SatID(sat);
fileSys = StringUtils::asString(sat.systemChar())
+ ": (" + sat.systemString3()+")";
if(version >= 3) {
fileType = "NAVIGATION";
}
else { // RINEX 2
if(sat.system == SatID::systemGPS)
fileType = "N (GPS Nav)";
else if(sat.system == SatID::systemGlonass)
fileType = "G (GLO Nav)";
else if(sat.system == SatID::systemGeosync)
fileType = "H (GEO Nav)";
else {
Exception e( std::string("RINEX version 2 ") +
sat.systemString3() +
std::string(" Nav files do not exist") );
GPSTK_THROW(e);
}
}
}
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}
// This function returns the health status of the SV.
bool GalEphemeris::isHealthy(void) const
{
try {
OrbitEph::isHealthy(); // ignore the return value; for dataLoaded check
// from RINEX 3.02 spec, A8:
if(health & 0x5) // E1b, E5b and both Tgds are valid
return true;
if(health & 0x2) // E5a and Tgba are valid
return true;
// but there seems to be contradictory information:
//if(health & 0x1) // b1) // E1b DVS is good
//if(health & 0x6) // b110) // E1b HS is good
//if(health & 0x8) // b1000) // E5a DVS is good
//if(health & 0x30) // b110000) // E5a HS is good
//if(health & 0x40) // b1000000) // E5b DVS is good
//if(health & 0x180) // b110000000) // E5b HS is good
return false;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}
// -----------------------------------------------------------------------------------
// Compute the phase windup, in cycles, given the time, the unit vector from receiver
// to transmitter, and the west and north unit vectors at the receiver, all in ECEF.
// YR is the West unit vector, XR is the North unit vector, at the receiver.
// shadow is the fraction of the sun's area not visible at the satellite.
double PhaseWindup(DayTime& tt, // epoch of interest
Position& SV, // satellite position
Position& Rx2Tx, // unit vector from receiver to satellite
Position& YR, // west unit vector at receiver
Position& XR, // north unit vector at receiver
double& shadow) // fraction of sun not visible at satellite
{
try {
double d,windup=0.0;
Position DR,DT;
Position TR = -1.0 * Rx2Tx; // transmitter to receiver
// get satellite attitude
Position XT,YT,ZT;
Matrix<double> Att = SatelliteAttitude(tt, SV, shadow);
XT = Position(Att(0,0),Att(0,1),Att(0,2)); // Cartesian is default
YT = Position(Att(1,0),Att(1,1),Att(1,2));
ZT = Position(Att(2,0),Att(2,1),Att(2,2));
// compute effective dipoles at receiver and transmitter
DR = XR - TR * TR.dot(XR) + Position(TR.cross(YR));
DT = XT - TR * TR.dot(XT) - Position(TR.cross(YT));
// normalize
d = 1.0/DR.mag();
DR = d * DR;
d = 1.0/DT.mag();
DT = d * DT;
windup = ::acos(DT.dot(DR)) / TWO_PI;
return windup;
}
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); }
}
ComputeIonoModel& ComputeIonoModel::setklobucharModel(const std::string& brdcFile)
{
if( isRinexNavFile( brdcFile ) )
{
RinexNavStream nstrm(brdcFile.c_str());
nstrm.exceptions(fstream::failbit);
try
{
RinexNavHeader rnh;
nstrm >> rnh;
nstrm.close();
setKlobucharModel(rnh.ionAlpha,rnh.ionBeta);
}
catch (Exception& e)
{
nstrm.close();
GPSTK_RETHROW(e);
}
}
else
{
// 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);
}
}
// adjustBeginningValidity determines the beginValid and endValid times.
// @throw Invalid Request if the required data has not been stored.
void GPSEphemeris::adjustValidity(void)
{
try {
OrbitEph::adjustValidity(); // for dataLoaded check
// Beginning of Validity
// New concept. Admit the following.
// (a.) The collection system may not capture the data at earliest transmit.
// (b.) The collection system may not capture the three SFs consecutively.
// Consider a couple of IS-GPS-200 promises,
// (c.) By definition, beginning of validity == beginning of transmission.
// (d.) Except for uploads, cutovers will only happen on hour boundaries
// (e.) Cutovers can be detected by non-even Toc.
// (f.) Even uploads will cutover on a frame (30s) boundary.
// Therefore,
// 1.) If Toc is NOT even two hour interval, pick lowest HOW time,
// round back to even 30s. That's the earliest Xmit time we can prove.
// NOTE: For the case where this is the SECOND SF 1/2/3 after an upload,
// this may yield a later time as such a SF 1/2/3 will be on a even
// hour boundary. Unfortunately, we have no way of knowing whether
// this item is first or second after upload without additional information
// 2.) If Toc IS even two hour interval, pick time from SF 1,
// round back to nearest EVEN two hour boundary. This assumes collection
// SOMETIME in first hour of transmission. Could be more
// complete by looking at fit interval and IODC to more accurately
// determine earliest transmission time.
long longToc = static_cast<GPSWeekSecond>(ctToc).getSOW();
long XmitWeek = static_cast<GPSWeekSecond>(transmitTime).getWeek();
double XmitSOW = 0.0;
if ( (longToc % 7200) != 0) // NOT an even two hour change
{
long Xmit = HOWtime - (HOWtime % 30);
XmitSOW = (double) Xmit;
}
else
{
long Xmit = HOWtime - HOWtime % 7200;
XmitSOW = (double) Xmit;
}
beginValid = GPSWeekSecond( XmitWeek, XmitSOW, TimeSystem::GPS );
// End of Validity.
// The end of validity is calculated from the fit interval
// and the Toe. The fit interval is either trivial
// (if fit interval flag==0, fit interval is 4 hours)
// or a look-up table based on the IODC.
// Round the Toe value to the hour to elminate confusion
// due to possible "small offsets" indicating uploads
long epochWeek = static_cast<GPSWeekSecond>(ctToe).getWeek();
double Toe = static_cast<GPSWeekSecond>(ctToe).getSOW();
long ToeOffset = (long) Toe % 3600;
double adjToe = Toe; // Default case
if (ToeOffset)
{
adjToe += 3600.0 - (double)ToeOffset; // If offset, then adjust to remove
}
long endFitSOW = adjToe + (fitDuration/2)*3600;
short endFitWk = epochWeek;
if (endFitSOW >= FULLWEEK)
{
endFitSOW -= FULLWEEK;
endFitWk++;
}
endValid = GPSWeekSecond(endFitWk, endFitSOW, TimeSystem::GPS);
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}
int PRSolution3::RAIM2Compute(const CommonTime& Tr,
vector<SatID>& Satellite,
const vector<double>& Pseudorange,
const XvtStore<SatID>& Eph,
TropModel *pTropModel)
throw(Exception)
{
try
{
int iret,N,Nreject,MinSV;
size_t i, j;
vector<bool> UseSat, UseSave;
vector<int> GoodIndexes;
// Use these to save the 'best' solution within the loop.
int BestNIter=0;
double BestRMS=0.0, BestSL=0.0, BestConv=0.0;
Vector<double> BestSol(3,0.0);
vector<bool> BestUse;
BestRMS = -1.0; // this marks the 'Best' set as unused.
Matrix<double> BestCovariance;
// ----------------------------------------------------------------
// initialize
Valid = false;
bool hasGlonass = false;
bool hasOther = false;
Vector<int> satSystems(Satellite.size());
//Check if we have a mixed system
for ( i = 0; i < Satellite.size(); ++i)
{
satSystems[i] = ((Satellite[i].system == SatID::systemGlonass) ? (1) : (0) );
if (satSystems[i] == 1) hasGlonass = true;
else hasOther = true;
}
// Save the input solution
// (for use in rejection when ResidualCriterion is false).
if (!hasGlonass && Solution.size() != 4)
{
Solution.resize(4);
}
else if (hasGlonass && hasOther && Solution.size() != 5)
{
Solution.resize(5);
}
Solution = 0.0;
// ----------------------------------------------------------------
// fill the SVP matrix, and use it for every solution
// NB this routine can set Satellite[.]=negative when no ephemeris
i = PrepareAutonomousSolution(Tr, Satellite, Pseudorange, Eph, SVP,
Debug?pDebugStream:0);
if(Debug && pDebugStream) {
*pDebugStream << "In RAIMCompute after PAS(): Satellites:";
for(j=0; j<Satellite.size(); j++) {
RinexSatID rs(::abs(Satellite[j].id), Satellite[j].system);
*pDebugStream << " " << (Satellite[j].id < 0 ? "-":"") << rs;
}
*pDebugStream << endl;
*pDebugStream << " SVP matrix("
<< SVP.rows() << "," << SVP.cols() << ")" << endl;
*pDebugStream << fixed << setw(16) << setprecision(3) << SVP << endl;
}
if (i) return i; // return is 0 (ok) or -4 (no ephemeris)
// count how many good satellites we have
// Initialize UseSat based on Satellite, and build GoodIndexes.
// UseSat is used to mark good sats (true) and those to ignore (false).
// UseSave saves the original so it can be reused for each solution.
// Let GoodIndexes be all the indexes of Satellites that are good:
// UseSat[GoodIndexes[.]] == true by definition
for (N=0,i=0; i<Satellite.size(); i++)
{
if (Satellite[i].id > 0)
{
N++;
UseSat.push_back(true);
GoodIndexes.push_back(i);
}
else
{
UseSat.push_back(false);
}
}
UseSave = UseSat;
// don't even try if there are not 4 good satellites
if (!hasGlonass && N < 4)
return -3;
else if (hasGlonass && hasOther && N < 5)
return -3;
// minimum number of sats needed for algorithm
MinSV = 5; // this would be RAIM
// ( not really RAIM || not RAIM at all - just one solution)
if (!ResidualCriterion || NSatsReject==0)
MinSV=4;
// how many satellites can RAIM reject, if we have to?
// default is -1, meaning as many as possible
Nreject = NSatsReject;
if (Nreject == -1 || Nreject > N-MinSV)
Nreject=N-MinSV;
// ----------------------------------------------------------------
// now compute the solution, first with all the data. If this fails,
// reject 1 satellite at a time and try again, then 2, etc.
// Slopes for each satellite are computed (cf. the RAIM algorithm)
Vector<double> Slopes(Pseudorange.size());
// Residuals stores the post-fit data residuals.
Vector<double> Residuals(Satellite.size(),0.0);
// compute all the Combinations of N satellites taken 4 at a time
Combinations Combo(N,N-4);
// compute a solution for each combination of marked satellites
do{
// Mark the satellites for this combination
UseSat = UseSave;
for (i = 0; i < GoodIndexes.size(); i++)
{
if (Combo.isSelected(i))
UseSat[i]=false;
}
// ----------------------------------------------------------------
// Compute a solution given the data; ignore ranges for marked
// satellites. Fill Vector 'Slopes' with slopes for each unmarked
// satellite.
// Return 0 ok
// -1 failed to converge
// -2 singular problem
// -3 not enough good data
NIterations = MaxNIterations; // pass limits in
Convergence = ConvergenceLimit;
iret = AutonomousPRSolution(Tr, UseSat, SVP, pTropModel, Algebraic,
NIterations, Convergence, Solution, Covariance, Residuals, Slopes,
pDebugStream, ((hasGlonass && hasOther)?(&satSystems):(NULL)) );
// ----------------------------------------------------------------
// Compute RMS residual...
// and in the usual case
RMSResidual = RMS(Residuals);
// ... and find the maximum slope
MaxSlope = 0.0;
if (iret == 0)
{
for (i=0; i<UseSat.size(); i++)
{
if (UseSat[i] && Slopes(i)>MaxSlope)
MaxSlope=Slopes[i];
}
}
// ----------------------------------------------------------------
// print solution with diagnostic information
if (Debug && pDebugStream)
{
GPSWeekSecond weeksec(Tr);
*pDebugStream << "RPS " << setw(2) << "4"
<< " " << setw(4) << weeksec.week
<< " " << fixed << setw(10) << setprecision(3) << weeksec.sow
<< " " << setw(2) << N-4 << setprecision(6)
<< " " << setw(16) << Solution(0)
<< " " << setw(16) << Solution(1)
<< " " << setw(16) << Solution(2)
<< " " << setw(14) << Solution(3);
if (hasGlonass && hasOther)
*pDebugStream << " " << setw(16) << Solution(4);
*pDebugStream << " " << setw(12) << RMSResidual
<< " " << fixed << setw(5) << setprecision(1) << MaxSlope
<< " " << NIterations
<< " " << scientific << setw(8) << setprecision(2)<< Convergence;
// print the SatID for good sats
for (i=0; i<UseSat.size(); i++)
{
if (UseSat[i])
*pDebugStream << " " << setw(3)<< Satellite[i].id;
else
*pDebugStream << " " << setw(3) << -::abs(Satellite[i].id);
}
// also print the return value
*pDebugStream << " (" << iret << ")" << endl;
}// end debug print
// ----------------------------------------------------------------
// deal with the results of AutonomousPRSolution()
if (iret)
{ // failure for this combination
RMSResidual = 0.0;
Solution = 0.0;
}
else
{ // success
// save 'best' solution for later
if (BestRMS < 0.0 || RMSResidual < BestRMS)
{
BestRMS = RMSResidual;
BestSol = Solution;
BestUse = UseSat;
BestSL = MaxSlope;
BestConv = Convergence;
BestNIter = NIterations;
}
// assign to the combo
ComboSolution.push_back(Solution);
ComboSolSat.push_back(UseSat);
// quit immediately?
if ((ReturnAtOnce) && RMSResidual < RMSLimit)
break;
}
// get the next Combinations and repeat
} while(Combo.Next() != -1);
/*
// end of the stage
if (BestRMS > 0.0 && BestRMS < RMSLimit)
{ // success
iret=0;
}
*/
// ----------------------------------------------------------------
// copy out the best solution and return
Convergence = BestConv;
NIterations = BestNIter;
RMSResidual = BestRMS;
Solution = BestSol;
MaxSlope = BestSL;
Covariance =BestCovariance;
for (Nsvs=0,i=0; i<BestUse.size(); i++)
{
if (!BestUse[i])
Satellite[i].id = -::abs(Satellite[i].id);
else
Nsvs++;
}
// FIXME should this be removed? I don't even know what slope is.
if (iret==0 && BestSL > SlopeLimit)
iret=1;
if (iret==0 && BestSL > SlopeLimit/2.0 && Nsvs == 5)
iret=1;
if (iret>=0 && BestRMS >= RMSLimit)
iret=2;
if (iret==0)
Valid=true;
return iret;
}
catch(Exception& e)
{
GPSTK_RETHROW(e);
}
} // end PRSolution2::RAIMCompute()
// -----------------------------------------------------------------------------------
// Same as UpEastNorth, but using geocentric coordinates, so that the -Up
// direction will meet the center of Earth.
Matrix<double> UpEastNorthGeocentric(Position& P) throw(Exception)
{
try { return UpEastNorth(P, true); }
catch(Exception& e) { GPSTK_RETHROW(e); }
}
// -----------------------------------------------------------------------------------
// 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); }
}
/* 'Data' Matrix of data containing observation data in rows, one
* row per observation and complying with this format:
* x y z P
* Where x,y,z are satellite coordinates in an ECEF system
* and P is pseudorange (corrected as much as possible,
* specially from satellite clock errors), all expresed
* in meters.
*
* 'X' Vector of position solution, in meters. There may be
* another solution, that may be accessed with vector
* "SecondSolution" if "ChooseOne" is set to "false".
*
* Return values:
* 0 Ok
* -1 Not enough good data
* -2 Singular problem
*/
int Bancroft::Compute( Matrix<double>& Data,
Vector<double>& X )
throw(Exception)
{
try
{
int N = Data.rows();
Matrix<double> B(0,4); // Working matrix
// Let's test the input data
if( testInput )
{
double satRadius = 0.0;
// Check each row of B Matrix
for( int i=0; i < N; i++ )
{
// If Data(i,3) -> Pseudorange is NOT between the allowed
// range, then drop line immediately
if( !( (Data(i,3) >= minPRange) && (Data(i,3) <= maxPRange) ) )
{
continue;
}
// Let's compute distance between Earth center and
// satellite position
satRadius = RSS(Data(i,0), Data(i,1) , Data(i,2));
// If satRadius is NOT between the allowed range, then drop
// line immediately
if( !( (satRadius >= minRadius) && (satRadius <= maxRadius) ) )
{
continue;
}
// If everything is ok so far, then extract the good
// data row and add it to working matrix
MatrixRowSlice<double> goodRow(Data,i);
B = B && goodRow;
}
// Let's redefine "N" and check if we have enough data rows
// left in a single step
if( (N = B.rows()) < 4 )
{
return -1; // We need at least 4 data rows
}
} // End of 'if( testInput )...'
else
{
// No input filtering. Working matrix (B) and
// input matrix (Data) are equal
B = Data;
}
Matrix<double> BT=transpose(B);
Matrix<double> BTBI(4,4), M(4,4,0.0);
Vector<double> aux(4), alpha(N), solution1(4), solution2(4);
// Temporary storage for BT*B. It will be inverted later
BTBI = BT * B;
// Let's try to invert BTB matrix
try
{
BTBI = inverseChol( BTBI );
}
catch(...)
{
return -2;
}
// Now, let's compute alpha vector
for( int i=0; i < N; i++ )
{
// First, fill auxiliar vector with corresponding satellite
// position and pseudorange
aux(0) = B(i,0);
aux(1) = B(i,1);
aux(2) = B(i,2);
aux(3) = B(i,3);
alpha(i) = 0.5 * Minkowski(aux, aux);
}
Vector<double> tau(N,1.0), BTBIBTtau(4), BTBIBTalpha(4);
BTBIBTtau = BTBI * BT * tau;
BTBIBTalpha = BTBI * BT * alpha;
// Now, let's find the coeficients of the second order-equation
double a(Minkowski(BTBIBTtau, BTBIBTtau));
double b(2.0 * (Minkowski(BTBIBTtau, BTBIBTalpha) - 1.0));
double c(Minkowski(BTBIBTalpha, BTBIBTalpha));
// Calculate discriminant and exit if negative
double discriminant = b*b - 4.0 * a * c;
if (discriminant < 0.0)
{
return -2;
}
// Find possible DELTA values
double DELTA1 = ( -b + SQRT(discriminant) ) / ( 2.0 * a );
double DELTA2 = ( -b - SQRT(discriminant) ) / ( 2.0 * a );
// We need to define M matrix
M(0,0) = 1.0;
M(1,1) = 1.0;
M(2,2) = 1.0;
M(3,3) = - 1.0;
// Find possible position solutions with their implicit radii
solution1 = M * BTBI * ( BT * DELTA1 * tau + BT * alpha );
double radius1(RSS(solution1(0), solution1(1), solution1(2)));
solution2 = M * BTBI * ( BT * DELTA2 * tau + BT * alpha );
double radius2(RSS(solution2(0), solution2(1), solution2(2)));
// Let's choose the right solution
if ( ChooseOne )
{
if ( ABS(CloseTo-radius1) < ABS(CloseTo-radius2) )
{
X = solution1;
}
else
{
X = solution2;
}
}
else
{
// Both solutions will be reported
X = solution1;
SecondSolution = solution2;
}
return 0;
} // end of first "try"
catch(Exception& e)
{
GPSTK_RETHROW(e);
}
} // end Bancroft::Compute()
/* Method to get the description of a given value
*
* @param variable Variable name.
* @param section Section the variable belongs to.
*
*/
string ConfDataReader::getValueDescription( 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);
try
{
// Auxiliar variable to store current 'issueException' state
bool exceptionState( getIssueException() );
// If 'fallback2Default' is set, and this is NOT the 'DEFAULT'
// section, we need to temporarily disable 'issueException'.
// This implies that there is no fallback for 'DEFAULT' variables
if( (section != "DEFAULT") && (section != "") )
{
if( getFallback2Default() )
{
setIssueException(false);
}
}
// Check if section and variable exist
if( ifExist(variable, section) )
{
// Reset 'issueException' to its correct value before continue
setIssueException( exceptionState );
return confData[section][variable].valueComment;
}
else
{
// Reset 'issueException' to its correct value before continue
setIssueException( exceptionState );
// If 'fallback2Default' is set, check also in 'DEFAULT' section
if ( getFallback2Default() )
{
if( ifExist(variable) )
{
return confData["DEFAULT"][variable].valueComment;
}
else
{
return "";
}
}
else
{
return "";
} // End of 'if ( getFallback2Default() )'
} // End of 'if( ifExist(variable, section) )'
}
catch (ConfigurationException& e)
{
GPSTK_RETHROW(e);
}
} // End of method 'ConfDataReader::getValueDescription()'
/* 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()'
RinexSatID(const std::string& str)
throw(Exception)
{
try { fromString(str); }
catch(Exception& e) { GPSTK_RETHROW(e); }
}
