double BrcKeplerOrbit::svRelativity(const CommonTime& t) const
throw( InvalidRequest )
{
GPSEllipsoid ell;
double twoPI = 2.0e0 * PI;
double sqrtgm = SQRT(ell.gm());
double elapte = t - getOrbitEpoch();
double amm = (sqrtgm / (A*Ahalf)) + dn;
double meana,F,G,delea;
meana = M0 + elapte * amm;
meana = fmod(meana, twoPI);
double ea = meana + ecc * ::sin(meana);
int loop_cnt = 1;
do {
F = meana - ( ea - ecc * ::sin(ea));
G = 1.0 - ecc * ::cos(ea);
delea = F/G;
ea = ea + delea;
loop_cnt++;
} while ( (ABS(delea) > 1.0e-11 ) && (loop_cnt <= 20) );
double dtr = REL_CONST * ecc * Ahalf * ::sin(ea);
return dtr;
}
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
Xvt BrcKeplerOrbit::svXvt(const CommonTime& t) const
throw(InvalidRequest)
{
Xvt sv;
GPSWeekSecond gpsws = (Toe);
double ToeSOW = gpsws.sow;
double ea; // eccentric anomaly //
double delea; // delta eccentric anomaly during iteration */
double elapte; // elapsed time since Toe
//double elaptc; // elapsed time since Toc
double q,sinea,cosea;
double GSTA,GCTA;
double amm;
double meana; // mean anomaly
double F,G; // temporary real variables
double alat,talat,c2al,s2al,du,dr,di,U,R,truea,AINC;
double ANLON,cosu,sinu,xip,yip,can,san,cinc,sinc;
double xef,yef,zef,dek,dlk,div,domk,duv,drv;
double dxp,dyp,vxef,vyef,vzef;
GPSEllipsoid ell;
double sqrtgm = SQRT(ell.gm());
// Check for ground transmitter
double twoPI = 2.0e0 * PI;
double lecc; // eccentricity
double tdrinc; // dt inclination
lecc = ecc;
tdrinc = idot;
// Compute time since ephemeris & clock epochs
elapte = t - getOrbitEpoch();
//CommonTime orbEp = getOrbitEpoch();
//elapte = t - orbEp;
// Compute mean motion
amm = (sqrtgm / (A*Ahalf)) + dn;
// In-plane angles
// meana - Mean anomaly
// ea - Eccentric anomaly
// truea - True anomaly
meana = M0 + elapte * amm;
meana = fmod(meana, twoPI);
ea = meana + lecc * ::sin(meana);
int loop_cnt = 1;
do {
F = meana - ( ea - lecc * ::sin(ea));
G = 1.0 - lecc * ::cos(ea);
delea = F/G;
ea = ea + delea;
loop_cnt++;
} while ( (fabs(delea) > 1.0e-11 ) && (loop_cnt <= 20) );
// Compute true anomaly
q = SQRT( 1.0e0 - lecc*lecc);
sinea = ::sin(ea);
cosea = ::cos(ea);
G = 1.0e0 - lecc * cosea;
// G*SIN(TA) AND G*COS(TA)
GSTA = q * sinea;
GCTA = cosea - lecc;
// True anomaly
truea = atan2 ( GSTA, GCTA );
// Argument of lat and correction terms (2nd harmonic)
alat = truea + w;
talat = 2.0e0 * alat;
c2al = ::cos( talat );
s2al = ::sin( talat );
du = c2al * Cuc + s2al * Cus;
dr = c2al * Crc + s2al * Crs;
di = c2al * Cic + s2al * Cis;
// U = updated argument of lat, R = radius, AINC = inclination
U = alat + du;
R = A*G + dr;
AINC = i0 + tdrinc * elapte + di;
// Longitude of ascending node (ANLON)
ANLON = OMEGA0 + (OMEGAdot - ell.angVelocity()) *
elapte - ell.angVelocity() * ToeSOW;
// In plane location
cosu = ::cos( U );
sinu = ::sin( U );
xip = R * cosu;
yip = R * sinu;
// Angles for rotation to earth fixed
can = ::cos( ANLON );
san = ::sin( ANLON );
cinc = ::cos( AINC );
sinc = ::sin( AINC );
// Earth fixed - meters
xef = xip*can - yip*cinc*san;
yef = xip*san + yip*cinc*can;
zef = yip*sinc;
sv.x[0] = xef;
sv.x[1] = yef;
sv.x[2] = zef;
// Compute velocity of rotation coordinates
dek = amm * A / R;
dlk = Ahalf * q * sqrtgm / (R*R);
div = tdrinc - 2.0e0 * dlk *
( Cic * s2al - Cis * c2al );
domk = OMEGAdot - ell.angVelocity();
duv = dlk*(1.e0+ 2.e0 * (Cus*c2al - Cuc*s2al) );
drv = A * lecc * dek * sinea - 2.e0 * dlk *
( Crc * s2al - Crs * c2al );
dxp = drv*cosu - R*sinu*duv;
dyp = drv*sinu + R*cosu*duv;
// Calculate velocities
vxef = dxp*can - xip*san*domk - dyp*cinc*san
+ yip*( sinc*san*div - cinc*can*domk);
vyef = dxp*san + xip*can*domk + dyp*cinc*can
- yip*( sinc*can*div + cinc*san*domk);
vzef = dyp*sinc + yip*cinc*div;
// Move results into output variables
sv.v[0] = vxef;
sv.v[1] = vyef;
sv.v[2] = vzef;
return sv;
}
