public static RmsedXYZ GetPosXYZ(EphemerisParam eph, double secOfWeek)
{
GpstkOrbitUtil o = new GpstkOrbitUtil(eph);
Xvt xvt = o.svXvt(secOfWeek);
XYZ xyzGpstk = XYZ.Parse(xvt.x);
XYZ velercity = XYZ.Parse(xvt.v);
return(new RmsedXYZ(xyzGpstk, velercity));
}
// Compute satellite position at the given time.
public Xvt svXvt(double t)
{
// if(!dataLoadedFlag)
// GPSTK_THROW(InvalidRequest("Data not loaded"));
Xvt sv = new Xvt();
double E; // eccentric anomaly
// double delea; // delta eccentric anomaly during iteration
double tk; // elapsed time since Toe
//double elaptc; // elapsed time since Toc
//double dtc,dtr;
double q, sinE, cosE;
double GSTA, GCTA;
double en;
double M; // mean anomaly
// double F;
double G; // temporary real variables
double u, u_u, cos2u, sin2u, du, dr, di, U, R, f, eyek;
double omgk, cosu, sinu, xip, yip, cosOmgk, sinOmgk, cosEyek, sinEyek;
double xef, yef, zef, dek, dlk, div, domk, duv, drv;
double dxp, dyp, vxef, vyef, vzef;
Geo.Referencing.IEllipsoid ell = GnssSystem.GetGnssSystem(prn.SatelliteType).Ellipsoid;
double sqrtGM = SQRT(ell.GM);
double twoPI = 2.0e0 * PI;
double e; // eccentricity
double eyeDot; // dt inclination
double sqrtA = SQRT(A); // A is semi-major axis of orbit
double ToeSOW = this.ctToe; // GPSWeekSecond(ctToe).sow; // SOW is time-system-independent
e = ecc;
eyeDot = idot;
// Compute time since ephemeris & clock epochs
tk = Time.GetDifferSecondOfWeek(t, ctToe);// t - ctToe;
// Compute A at time of interest (LNAV: Adot==0)
double Ak = A + Adot * tk;
// Compute mean motion (LNAV: dndot==0)
double dnA = dn + 0.5 * dndot * tk;
en = (sqrtGM / (A * sqrtA)) + dnA; // Eqn specifies A0, not Ak
// In-plane angles
// meana - Mean anomaly
// ea - Eccentric anomaly
// truea - True anomaly
M = M0 + tk * en;
M = fmod(M, twoPI);
E = OrbitUtil.KeplerEqForEccAnomaly(M, e);
//E = M + e * sin(M);
//int loop_cnt = 1;
//do
//{
// F = M - (E - e * sin(E));
// G = 1.0 - e * cos(E);
// delea = F / G;
// E = E + delea;
// loop_cnt++;
//} while ((fabs(delea) > 1.0e-11) && (loop_cnt <= 20));
// Compute clock corrections
sv.relcorr = svRelativity(t);
sv.clkbias = svClockBias(t);
sv.clkdrift = svClockDrift(t);
// sv.frame = ReferenceFrame.WGS84;
// Compute true anomaly
sinE = sin(E);
cosE = cos(E);
q = SQRT(1.0 - e * e);
G = 1.0 - e * cosE;
// G*SIN(TA) AND G*COS(TA)
GSTA = q * sinE;
GCTA = cosE - e;
// True anomaly
f = atan2(GSTA, GCTA);
// Argument of lat and correction terms (2nd harmonic)
u = f + Omega;
u_u = 2.0 * u;
cos2u = cos(u_u);
sin2u = sin(u_u);
du = cos2u * Cuc + sin2u * Cus;
dr = cos2u * Crc + sin2u * Crs;
di = cos2u * Cic + sin2u * Cis;
// U = updated argument of lat, R = radius, AINC = inclination
U = u + du;
R = Ak * G + dr;
eyek = eye0 + eyeDot * tk + di;
// Longitude of ascending node (ANLON)
omgk = OMEGA0 + (OMEGAdot - ell.AngleVelocity) * tk
- ell.AngleVelocity * ToeSOW;
// In plane location
cosu = cos(U);
sinu = sin(U);
xip = R * cosu;
yip = R * sinu;
// Angles for rotation to earth fixed
cosOmgk = cos(omgk);
sinOmgk = sin(omgk);
cosEyek = cos(eyek);
sinEyek = sin(eyek);
// Earth fixed coordinates in meters
xef = xip * cosOmgk - yip * cosEyek * sinOmgk;
yef = xip * sinOmgk + yip * cosEyek * cosOmgk;
zef = yip * sinEyek;
sv.x[0] = xef;
sv.x[1] = yef;
sv.x[2] = zef;
// Compute velocity of rotation coordinates
dek = en * Ak / R;
dlk = sqrtA * q * sqrtGM / (R * R);
div = eyeDot - 2.0e0 * dlk * (Cic * sin2u - Cis * cos2u);
domk = OMEGAdot - ell.AngleVelocity;
duv = dlk * (1.0 + 2.0 * (Cus * cos2u - Cuc * sin2u));
drv = Ak * e * dek * sinE - 2.0 * dlk * (Crc * sin2u - Crs * cos2u);
dxp = drv * cosu - R * sinu * duv;
dyp = drv * sinu + R * cosu * duv;
// Calculate velocities
vxef = dxp * cosOmgk - xip * sinOmgk * domk - dyp * cosEyek * sinOmgk
+ yip * (sinEyek * sinOmgk * div - cosEyek * cosOmgk * domk);
vyef = dxp * sinOmgk + xip * cosOmgk * domk + dyp * cosEyek * cosOmgk
- yip * (sinEyek * cosOmgk * div + cosEyek * sinOmgk * domk);
vzef = dyp * sinEyek + yip * cosEyek * div;
// Move results into output variables
sv.v[0] = vxef;
sv.v[1] = vyef;
sv.v[2] = vzef;
return(sv);
}
