/// <summary>
/// 6个轨道根数计算卫星在天球坐标系中的位置，角度以弧度为单位。
/// 本函数仅用于课程学习计算。
/// </summary>
/// <param name="deltaTime">距离参考时刻的时间差，单位秒</param>
/// <param name="eye">轨道平面倾角,弧度</param>
/// <param name="Ω">升交点赤经,弧度</param>
/// <param name="a">轨道椭圆长半径的平方根</param>
/// <param name="e">轨道椭圆离心率</param>
/// <param name="w">近升角距,弧度</param>
/// <param name="M0">参考时刻τ卫星的平近点角,弧度</param>
public static MotionState GetSatPos(
double deltaTime,
double eye,
double Ω,
double a,
double e,
double w,
double M0
)
{
double SqrtA = Math.Sqrt(a);
//计算平均角速度n
double sqrtA3 = a * SqrtA;
double n = Math.Sqrt(GM) / sqrtA3;
//2）计算平近点角M和偏近点角E
double M = M0 + n * deltaTime;
//偏近点角E, E=M+eSinE
double E = OrbitUtil.KeplerEqForEccAnomaly(M, e);
//计算卫星向径的模r
double r = a * (1 - e * Math.Cos(E));
//计算卫星在轨道平面直角坐标系中的坐标（x’,y’）
double x = a * (Math.Cos(E) - e);
double y = a * Math.Sqrt(1 - e * e) * Math.Sin(E);
//2.5.1.2 卫星在天球坐标系中的位置
XYZ plainXyz = new XYZ(x, y, 0);
var celestialXyz = plainXyz.RotateZ(-w).RotateX(-eye).RotateZ(-Ω);
//计算速度
double aaanr = (a * a * n) / r;
double vx = aaanr * (-Math.Sin(E));
double vy = aaanr * Math.Sqrt(1 - e * e) * Math.Cos(E);
XYZ plainVxy = new XYZ(vx, vy, 0);
var celestialVXyz = plainVxy.RotateZ(-w).RotateX(-eye).RotateZ(-Ω);
MotionState state = new MotionState(celestialXyz, celestialVXyz);
return(state);
}
/// <summary>
/// compute eccentric anomaly at time TProduct from broadcast elements
/// </summary>
/// <param name="time"></param>
/// <returns></returns>
public double EccentricAnomaly(double time)
{
double a, en0, tk, en, emk;
if ((time - Toe) > 302400.0)
{
time = time - 604800.0; //*** handle week rollover
}
if ((time - Toe) < -302400.0)
{
time = time + 604800.0;
}
a = SqrtA * SqrtA; //*** semimajor axis
en0 = Math.Sqrt(GM / (a * a * a)); //*** mean motion
tk = time - Toe; //*** time since orbit reference epoch
en = en0 + DeltaN; //*** corrected mean motion
emk = MeanAnomaly + en * tk; //*** mean anomaly, M
return(OrbitUtil.KeplerEqForEccAnomaly(emk, Eccentricity)); //*** solve kepler's eq for ecc anomaly, E
}
// 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);
}