double CorrectedEphemerisRange::ComputeAtTransmitSvTime(
const CommonTime& tt_nom,
const double& pr,
const Position& rx,
const SatID sat,
const XvtStore<SatID>& eph)
{
try {
svPosVel = eph.getXvt(sat, tt_nom);
// compute rotation angle in the time of signal transit
// While this is quite similiar to rotateEarth, its not the same
// and jcl doesn't know which is really correct
// BWT this uses the measured pseudorange, corrected for SV clock and
// relativity, to compute the time of flight; rotateEarth uses the value
// computed from the receiver position and the ephemeris. They should be
// very nearly the same, and multiplying by angVel/c should make the angle
// of rotation very nearly identical.
GPSEllipsoid ell;
double range(pr/ell.c() - svPosVel.clkbias - svPosVel.relcorr);
double rotation_angle = -ell.angVelocity() * range;
svPosVel.x[0] = svPosVel.x[0] - svPosVel.x[1] * rotation_angle;
svPosVel.x[1] = svPosVel.x[1] + svPosVel.x[0] * rotation_angle;
svPosVel.x[2] = svPosVel.x[2];
rawrange =rx.slantRange(svPosVel.x);
updateCER(rx);
return rawrange - svclkbias - relativity;
}
catch (Exception& e) {
GPSTK_RETHROW(e);
}
}
void CorrectedEphemerisRange::rotateEarth(const Position& Rx)
{
GPSEllipsoid ellipsoid;
double tof = RSS(svPosVel.x[0]-Rx.X(),
svPosVel.x[1]-Rx.Y(),
svPosVel.x[2]-Rx.Z())/ellipsoid.c();
double wt = ellipsoid.angVelocity()*tof;
double sx = ::cos(wt)*svPosVel.x[0] + ::sin(wt)*svPosVel.x[1];
double sy = -::sin(wt)*svPosVel.x[0] + ::cos(wt)*svPosVel.x[1];
svPosVel.x[0] = sx;
svPosVel.x[1] = sy;
sx = ::cos(wt)*svPosVel.v[0] + ::sin(wt)*svPosVel.v[1];
sy = -::sin(wt)*svPosVel.v[0] + ::cos(wt)*svPosVel.v[1];
svPosVel.v[0] = sx;
svPosVel.v[1] = sy;
}
// Compute the corrected range at RECEIVE time, from receiver at position Rx,
// to the GPS satellite given by SatID sat, as well as all the CER quantities,
// given the nominal receive time tr_nom and an EphemerisStore. Note that this
// routine does not intrinsicly account for the receiver clock error
// like the ComputeAtTransmitTime routine does.
double CorrectedEphemerisRange::ComputeAtReceiveTime(
const CommonTime& tr_nom,
const Position& Rx,
const SatID sat,
const XvtStore<SatID>& Eph)
{
try {
int nit;
double tof,tof_old;
GPSEllipsoid ellipsoid;
nit = 0;
tof = 0.07; // initial guess 70ms
do {
// best estimate of transmit time
transmit = tr_nom;
transmit -= tof;
tof_old = tof;
// get SV position
try {
svPosVel = Eph.getXvt(sat, transmit);
}
catch(InvalidRequest& e) {
GPSTK_RETHROW(e);
}
rotateEarth(Rx);
// update raw range and time of flight
rawrange = RSS(svPosVel.x[0]-Rx.X(),
svPosVel.x[1]-Rx.Y(),
svPosVel.x[2]-Rx.Z());
tof = rawrange/ellipsoid.c();
} while(ABS(tof-tof_old)>1.e-13 && ++nit<5);
updateCER(Rx);
return (rawrange-svclkbias-relativity);
}
catch(gpstk::Exception& e) {
GPSTK_RETHROW(e);
}
} // end CorrectedEphemerisRange::ComputeAtReceiveTime
