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;
}
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;
}
