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