// Compute satellite relativity correction (sec) at the given time
// throw Invalid Request if the required satData has not been stored.
double svRelativity(double t)
{
Geo.Referencing.IEllipsoid ell = GnssSystem.GetGnssSystem(prn.SatelliteType).Ellipsoid;
double twoPI = 2.0 * PI;
double sqrtgm = SQRT(ell.GM);
double elapte = t - ctToe;
// Compute A at time of interest
double Ak = A + Adot * elapte; // LNAV: Adot==0
//double dnA = dn + 0.5*dndot*elapte; // LNAV: dndot==0
double Ahalf = SQRT(A);
double amm = (sqrtgm / (A * Ahalf)) + dn; // Eqn specifies A0 not Ak
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));
return(REL_CONST * ecc * SQRT(Ak) * sin(ea));
}
/// <summary>
/// 根据轨道根数计算卫星位置。
/// 核心计算程序。
/// </summary>
/// <param name="secOfWeek">GPS周秒</param>
/// <param name="isGeosynchronous">是否是地球同步轨道卫星</param>
/// <returns></returns>
public static Geo.Coordinates.XYZ GetSatXyz(EphemerisParam eph, double gpsSecOfWeek, SatelliteType satelliteType = SatelliteType.G, bool isGeosynchronous = false)
{
double e = eph.Eccentricity;
double A, en0, tk, en, M, E, sinE, cosE, f, u, cos2u, sin2u, duk, drk, dik;
double uk, rk, eyek, cosEyek, xpk, ypk, omgk, cosOmgk, sinOmgk, xk, yk, zk;
double secOfWeek = gpsSecOfWeek;
if (satelliteType == SatelliteType.C)//如果是北斗，需要转换为北斗时间来计算。
{
secOfWeek = gpsSecOfWeek - 14;
}
//*** time since orbit reference epoch
tk = Time.GetDifferSecondOfWeek(secOfWeek, eph.Toe);
Geo.Referencing.IEllipsoid ellipsoid = GnssSystem.GetGnssSystem(satelliteType).Ellipsoid;
double GM = ellipsoid.GM; // MU_GPS;
double omge = ellipsoid.AngleVelocity; // OMGE;
//平均角速度
A = eph.SqrtA * eph.SqrtA;
en0 = Math.Sqrt(GM) / (A * eph.SqrtA);
//改正平均角度
en = en0 + eph.DeltaN;
//*** mean anomaly, M
M = eph.MeanAnomaly + en * tk;
//*** solve kepler's equation for eccentric anomaly, E
E = KeplerEqForEccAnomaly(M, e);
//var E0 = KeplerEqForEccAnomalyOld(M, e);
//var E1 = KeplerEqForEccAnomaly1(M, e);
// double E1 = KeplerEqForEccAnomaly1(eph, M);
sinE = Math.Sin(E);
cosE = Math.Cos(E);
//*** true anomaly, L
f = Math.Atan2(Math.Sqrt(1.0 - e * e) * sinE, cosE - e);
//*** argument of latitude, rightHandSide
u = f + eph.ArgumentOfPerigee;
cos2u = Math.Cos(u + u);
sin2u = Math.Sin(u + u);
//*** corrections to the arg. of lat., radius, and inclination
duk = eph.Cuc * cos2u + eph.Cus * sin2u;
drk = eph.Crc * cos2u + eph.Crs * sin2u;
dik = eph.Cic * cos2u + eph.Cis * sin2u;
//*** correct the arg. of lat., radius, and inclination
uk = u + duk;
rk = A * (1.0 - e * cosE) + drk;
eyek = eph.Inclination + eph.EyeDot * tk + dik;
//*** position in the orbital plane
xpk = rk * Math.Cos(uk);
ypk = rk * Math.Sin(uk);
cosEyek = Math.Cos(eyek);
//*** correct the longitude of the ascending node
XYZ xyz = new XYZ();
//同步轨道卫星，如北斗
if (isGeosynchronous)
{
omgk = eph.LongOfAscension + eph.OmegaDot * tk - omge * eph.Toe;
cosOmgk = Math.Cos(omgk);
sinOmgk = Math.Sin(omgk);
//*** compute ecbf coordinates
double xg = xpk * cosOmgk - ypk * sinOmgk * cosEyek;
double yg = xpk * sinOmgk + ypk * cosOmgk * cosEyek;
double zg = ypk * Math.Sin(eyek);
double sino = Math.Sin(omge * tk);
double coso = Math.Cos(omge * tk);
double x = xg * coso + yg * sino * COS_5 + zg * sino * SIN_5;
double y = -xg * sino + yg * coso * COS_5 + zg * coso * SIN_5;
double z = -yg * SIN_5 + zg * COS_5;
xyz = new XYZ(x, y, z);
}
else
{
omgk = eph.LongOfAscension + (eph.OmegaDot - omge) * tk - omge * eph.Toe;
cosOmgk = Math.Cos(omgk);
sinOmgk = Math.Sin(omgk);
//*** compute ecbf coordinates
xk = xpk * cosOmgk - ypk * sinOmgk * cosEyek;
yk = xpk * sinOmgk + ypk * cosOmgk * cosEyek;
zk = ypk * Math.Sin(eyek);
xyz = new XYZ(xk, yk, zk);
}
return(xyz);
//以下为测试比较
XYZ xyz2 = RtkLibOrbitUtil.GetPos(secOfWeek, eph);
//XYZ xyzGpstk = GetPosXYZ(eph, secOfWeek);
// return xyzGpstk;
}
// 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);
}
