/// <summary> /// 由地心地固空间直角坐标计算大地坐标。 /// </summary> /// <param name="pos">空间直角坐标</param> /// <param name="date">儒略日</param> /// <param name="ellipsoid">参考椭球</param> /// <param name="unit">角度单位</param> /// <returns></returns> public static GeoCoord XyzToGeoCoord(IXYZ pos, Julian date, Geo.Referencing.Ellipsoid ellipsoid = null, AngleUnit unit = AngleUnit.Degree) { if (ellipsoid == null) { ellipsoid = Geo.Referencing.Ellipsoid.WGS84; } double f = ellipsoid.Flattening; double a = ellipsoid.SemiMajorAxis; double TwoPi = 2 * CoordConsts.PI; double x = pos.X; double y = pos.Y; double z = pos.Z; double theta = (GeoMath.AcTan(pos.Y, pos.X) - date.GetGreenwichMeanSiderealTime()) % TwoPi; theta = theta % TwoPi; if (theta < 0.0) { // "wrap" negative modulo theta += TwoPi; } double r = Math.Sqrt(x * x + y * y); double e2 = f * (2.0 - f); double lat = GeoMath.AcTan(z, r); const double DELTA = 1.0e-07; double phi; double c; do { phi = lat; c = 1.0 / Math.Sqrt(1.0 - e2 * GeoMath.Sqr(Sin(phi))); lat = GeoMath.AcTan(pos.Z + a * c * e2 * Sin(phi), r); }while (Math.Abs(lat - phi) > DELTA); double Altitude = (r / Cos(lat)) - a * c; if (unit == AngleUnit.Degree) { lat *= AngularConvert.RadToDegMultiplier; theta *= AngularConvert.RadToDegMultiplier; } double Lat = lat; double Lon = theta; return(new GeoCoord(Lon, Lat, Altitude, unit)); }
// /////////////////////////////////////////////////////////////////////////// private bool DeepPeriodics(ref double e, ref double xincc, ref double omgadf, ref double xnode, ref double xmam, double tsince) { // Lunar-solar periodics double sinis = Math.Sin(xincc); double cosis = Math.Cos(xincc); double sghs = 0.0; double shs = 0.0; double sh1 = 0.0; double pe = 0.0; double pinc = 0.0; double pl = 0.0; double sghl = 0.0; double zm = dp_zmos + zns * tsince; double zf = zm + 2.0 * zes * Math.Sin(zm); double sinzf = Math.Sin(zf); double f2 = 0.5 * sinzf * sinzf - 0.25; double f3 = -0.5 * sinzf * Math.Cos(zf); double ses = dp_se2 * f2 + dp_se3 * f3; double sis = dp_si2 * f2 + dp_si3 * f3; double sls = dp_sl2 * f2 + dp_sl3 * f3 + dp_sl4 * sinzf; sghs = dp_sgh2 * f2 + dp_sgh3 * f3 + dp_sgh4 * sinzf; shs = dp_sh2 * f2 + dp_sh3 * f3; zm = dp_zmol + znl * tsince; zf = zm + 2.0 * zel * Math.Sin(zm); sinzf = Math.Sin(zf); f2 = 0.5 * sinzf * sinzf - 0.25; f3 = -0.5 * sinzf * Math.Cos(zf); double sel = dp_ee2 * f2 + dp_e3 * f3; double sil = dp_xi2 * f2 + dp_xi3 * f3; double sll = dp_xl2 * f2 + dp_xl3 * f3 + dp_xl4 * sinzf; sghl = dp_xgh2 * f2 + dp_xgh3 * f3 + dp_xgh4 * sinzf; sh1 = dp_xh2 * f2 + dp_xh3 * f3; pe = ses + sel; pinc = sis + sil; pl = sls + sll; double pgh = sghs + sghl; double ph = shs + sh1; xincc = xincc + pinc; e = e + pe; if (dp_xqncl >= 0.2) { // Apply periodics directly ph = ph / m_sinio; pgh = pgh - m_cosio * ph; omgadf = omgadf + pgh; xnode = xnode + ph; xmam = xmam + pl; } else { // Apply periodics with Lyddane modification double sinok = Math.Sin(xnode); double cosok = Math.Cos(xnode); double alfdp = sinis * sinok; double betdp = sinis * cosok; double dalf = ph * cosok + pinc * cosis * sinok; double dbet = -ph * sinok + pinc * cosis * cosok; alfdp = alfdp + dalf; betdp = betdp + dbet; double xls = xmam + omgadf + cosis * xnode; double dls = pl + pgh - pinc * xnode * sinis; xls = xls + dls; xnode = GeoMath.AcTan(alfdp, betdp); xmam = xmam + pl; omgadf = xls - xmam - Math.Cos(xincc) * xnode; } return(true); }
bool gp_sync; // geopotential synchronous // /////////////////////////////////////////////////////////////////////////// public NoradSDP4(Orbit orbit) : base(orbit) { double sinarg = Math.Sin(Orbit.ArgPerigee); double cosarg = Math.Cos(Orbit.ArgPerigee); // Deep space initialization Julian jd = Orbit.Epoch; dp_thgr = jd.GetGreenwichMeanSiderealTime(); double eq = Orbit.Eccentricity; double aqnv = 1.0 / Orbit.SemiMajor; dp_xqncl = Orbit.Inclination; double xmao = Orbit.MeanAnomaly; double xpidot = m_omgdot + m_xnodot; double sinq = Math.Sin(Orbit.RAAN); double cosq = Math.Cos(Orbit.RAAN); dp_omegaq = Orbit.ArgPerigee; #region Lunar / Solar terms // Initialize lunar solar terms double day = jd.FromJan0_12h_1900(); double dpi_xnodce = 4.5236020 - 9.2422029E-4 * day; double dpi_stem = Math.Sin(dpi_xnodce); double dpi_ctem = Math.Cos(dpi_xnodce); double dpi_zcosil = 0.91375164 - 0.03568096 * dpi_ctem; double dpi_zsinil = Math.Sqrt(1.0 - dpi_zcosil * dpi_zcosil); double dpi_zsinhl = 0.089683511 * dpi_stem / dpi_zsinil; double dpi_zcoshl = Math.Sqrt(1.0 - dpi_zsinhl * dpi_zsinhl); double dpi_c = 4.7199672 + 0.22997150 * day; double dpi_gam = 5.8351514 + 0.0019443680 * day; dp_zmol = GeoMath.ModTwoPI(dpi_c - dpi_gam); double dpi_zx = 0.39785416 * dpi_stem / dpi_zsinil; double dpi_zy = dpi_zcoshl * dpi_ctem + 0.91744867 * dpi_zsinhl * dpi_stem; dpi_zx = GeoMath.AcTan(dpi_zx, dpi_zy) + dpi_gam - dpi_xnodce; double dpi_zcosgl = Math.Cos(dpi_zx); double dpi_zsingl = Math.Sin(dpi_zx); dp_zmos = 6.2565837 + 0.017201977 * day; dp_zmos = GeoMath.ModTwoPI(dp_zmos); const double zcosis = 0.91744867; const double zsinis = 0.39785416; const double zsings = -0.98088458; const double zcosgs = 0.1945905; const double c1ss = 2.9864797E-6; double zcosg = zcosgs; double zsing = zsings; double zcosi = zcosis; double zsini = zsinis; double zcosh = cosq; double zsinh = sinq; double cc = c1ss; double zn = zns; double ze = zes; double xnoi = 1.0 / Orbit.MeanMotion; double a1; double a3; double a7; double a8; double a9; double a10; double a2; double a4; double a5; double a6; double x1; double x2; double x3; double x4; double x5; double x6; double x7; double x8; double z31; double z32; double z33; double z1; double z2; double z3; double z11; double z12; double z13; double z21; double z22; double z23; double s3; double s2; double s4; double s1; double s5; double s6; double s7; double se = 0.0; double si = 0.0; double sl = 0.0; double sgh = 0.0; double sh = 0.0; double eosq = GeoMath.Sqr(Orbit.Eccentricity); // Apply the solar and lunar terms on the prevObj pass, then re-apply the // solar terms again on the second pass. for (int pass = 1; pass <= 2; pass++) { // Do solar terms a1 = zcosg * zcosh + zsing * zcosi * zsinh; a3 = -zsing * zcosh + zcosg * zcosi * zsinh; a7 = -zcosg * zsinh + zsing * zcosi * zcosh; a8 = zsing * zsini; a9 = zsing * zsinh + zcosg * zcosi * zcosh; a10 = zcosg * zsini; a2 = m_cosio * a7 + m_sinio * a8; a4 = m_cosio * a9 + m_sinio * a10; a5 = -m_sinio * a7 + m_cosio * a8; a6 = -m_sinio * a9 + m_cosio * a10; x1 = a1 * cosarg + a2 * sinarg; x2 = a3 * cosarg + a4 * sinarg; x3 = -a1 * sinarg + a2 * cosarg; x4 = -a3 * sinarg + a4 * cosarg; x5 = a5 * sinarg; x6 = a6 * sinarg; x7 = a5 * cosarg; x8 = a6 * cosarg; z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * eosq; z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * eosq; z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * eosq; z11 = -6.0 * a1 * a5 + eosq * (-24.0 * x1 * x7 - 6.0 * x3 * x5); z12 = -6.0 * (a1 * a6 + a3 * a5) + eosq * (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); z13 = -6.0 * a3 * a6 + eosq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); z21 = 6.0 * a2 * a5 + eosq * (24.0 * x1 * x5 - 6.0 * x3 * x7); z22 = 6.0 * (a4 * a5 + a2 * a6) + eosq * (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); z23 = 6.0 * a4 * a6 + eosq * (24.0 * x2 * x6 - 6.0 * x4 * x8); z1 = z1 + z1 + m_betao2 * z31; z2 = z2 + z2 + m_betao2 * z32; z3 = z3 + z3 + m_betao2 * z33; s3 = cc * xnoi; s2 = -0.5 * s3 / m_betao; s4 = s3 * m_betao; s1 = -15.0 * eq * s4; s5 = x1 * x3 + x2 * x4; s6 = x2 * x3 + x1 * x4; s7 = x2 * x4 - x1 * x3; se = s1 * zn * s5; si = s2 * zn * (z11 + z13); sl = -zn * s3 * (z1 + z3 - 14.0 - 6.0 * eosq); sgh = s4 * zn * (z31 + z33 - 6.0); if (Orbit.Inclination < 5.2359877E-2) { sh = 0.0; } else { sh = -zn * s2 * (z21 + z23); } dp_ee2 = 2.0 * s1 * s6; dp_e3 = 2.0 * s1 * s7; dp_xi2 = 2.0 * s2 * z12; dp_xi3 = 2.0 * s2 * (z13 - z11); dp_xl2 = -2.0 * s3 * z2; dp_xl3 = -2.0 * s3 * (z3 - z1); dp_xl4 = -2.0 * s3 * (-21.0 - 9.0 * eosq) * ze; dp_xgh2 = 2.0 * s4 * z32; dp_xgh3 = 2.0 * s4 * (z33 - z31); dp_xgh4 = -18.0 * s4 * ze; dp_xh2 = -2.0 * s2 * z22; dp_xh3 = -2.0 * s2 * (z23 - z21); if (pass == 1) { // Do lunar terms dp_sse = se; dp_ssi = si; dp_ssl = sl; dp_ssh = sh / m_sinio; dp_ssg = sgh - m_cosio * dp_ssh; dp_se2 = dp_ee2; dp_si2 = dp_xi2; dp_sl2 = dp_xl2; dp_sgh2 = dp_xgh2; dp_sh2 = dp_xh2; dp_se3 = dp_e3; dp_si3 = dp_xi3; dp_sl3 = dp_xl3; dp_sgh3 = dp_xgh3; dp_sh3 = dp_xh3; dp_sl4 = dp_xl4; dp_sgh4 = dp_xgh4; zcosg = dpi_zcosgl; zsing = dpi_zsingl; zcosi = dpi_zcosil; zsini = dpi_zsinil; zcosh = dpi_zcoshl * cosq + dpi_zsinhl * sinq; zsinh = sinq * dpi_zcoshl - cosq * dpi_zsinhl; zn = znl; const double c1l = 4.7968065E-7; cc = c1l; ze = zel; } } #endregion dp_sse = dp_sse + se; dp_ssi = dp_ssi + si; dp_ssl = dp_ssl + sl; dp_ssg = dp_ssg + sgh - m_cosio / m_sinio * sh; dp_ssh = dp_ssh + sh / m_sinio; // Geopotential resonance initialization for 12 hour orbits gp_reso = false; gp_sync = false; double g310; double f220; double bfact = 0.0; // Determine if orbit is 12- or 24-hour resonant. // Mean motion is given in radians per second. if ((Orbit.MeanMotion > 0.0034906585) && (Orbit.MeanMotion < 0.0052359877)) { // Orbit is within the Clarke Belt (period is 24-hour resonant). // Synchronous resonance terms initialization gp_reso = true; gp_sync = true; #region 24-hour resonant double g200 = 1.0 + eosq * (-2.5 + 0.8125 * eosq); g310 = 1.0 + 2.0 * eosq; double g300 = 1.0 + eosq * (-6.0 + 6.60937 * eosq); f220 = 0.75 * (1.0 + m_cosio) * (1.0 + m_cosio); double f311 = 0.9375 * m_sinio * m_sinio * (1.0 + 3 * m_cosio) - 0.75 * (1.0 + m_cosio); double f330 = 1.0 + m_cosio; f330 = 1.875 * f330 * f330 * f330; const double q22 = 1.7891679e-06; const double q33 = 2.2123015e-07; const double q31 = 2.1460748e-06; dp_del1 = 3.0 * m_xnodp * m_xnodp * aqnv * aqnv; dp_del2 = 2.0 * dp_del1 * f220 * g200 * q22; dp_del3 = 3.0 * dp_del1 * f330 * g300 * q33 * aqnv; dp_del1 = dp_del1 * f311 * g310 * q31 * aqnv; dp_xlamo = xmao + Orbit.RAAN + Orbit.ArgPerigee - dp_thgr; bfact = m_xmdot + xpidot - thdt; bfact = bfact + dp_ssl + dp_ssg + dp_ssh; #endregion } else if (((Orbit.MeanMotion >= 8.26E-3) && (Orbit.MeanMotion <= 9.24E-3)) && (eq >= 0.5)) { // Period is 12-hour resonant gp_reso = true; #region 12-hour resonant double eoc = eq * eosq; double g201 = -0.306 - (eq - 0.64) * 0.440; double g211; double g322; double g410; double g422; double g520; if (eq <= 0.65) { g211 = 3.616 - 13.247 * eq + 16.290 * eosq; g310 = -19.302 + 117.390 * eq - 228.419 * eosq + 156.591 * eoc; g322 = -18.9068 + 109.7927 * eq - 214.6334 * eosq + 146.5816 * eoc; g410 = -41.122 + 242.694 * eq - 471.094 * eosq + 313.953 * eoc; g422 = -146.407 + 841.880 * eq - 1629.014 * eosq + 1083.435 * eoc; g520 = -532.114 + 3017.977 * eq - 5740.0 * eosq + 3708.276 * eoc; } else { g211 = -72.099 + 331.819 * eq - 508.738 * eosq + 266.724 * eoc; g310 = -346.844 + 1582.851 * eq - 2415.925 * eosq + 1246.113 * eoc; g322 = -342.585 + 1554.908 * eq - 2366.899 * eosq + 1215.972 * eoc; g410 = -1052.797 + 4758.686 * eq - 7193.992 * eosq + 3651.957 * eoc; g422 = -3581.69 + 16178.11 * eq - 24462.77 * eosq + 12422.52 * eoc; if (eq <= 0.715) { g520 = 1464.74 - 4664.75 * eq + 3763.64 * eosq; } else { g520 = -5149.66 + 29936.92 * eq - 54087.36 * eosq + 31324.56 * eoc; } } double g533; double g521; double g532; if (eq < 0.7) { g533 = -919.2277 + 4988.61 * eq - 9064.77 * eosq + 5542.21 * eoc; g521 = -822.71072 + 4568.6173 * eq - 8491.4146 * eosq + 5337.524 * eoc; g532 = -853.666 + 4690.25 * eq - 8624.77 * eosq + 5341.4 * eoc; } else { g533 = -37995.78 + 161616.52 * eq - 229838.2 * eosq + 109377.94 * eoc; g521 = -51752.104 + 218913.95 * eq - 309468.16 * eosq + 146349.42 * eoc; g532 = -40023.88 + 170470.89 * eq - 242699.48 * eosq + 115605.82 * eoc; } double sini2 = m_sinio * m_sinio; double cosi2 = m_cosio * m_cosio; f220 = 0.75 * (1.0 + 2.0 * m_cosio + cosi2); double f221 = 1.5 * sini2; double f321 = 1.875 * m_sinio * (1.0 - 2.0 * m_cosio - 3.0 * cosi2); double f322 = -1.875 * m_sinio * (1.0 + 2.0 * m_cosio - 3.0 * cosi2); double f441 = 35.0 * sini2 * f220; double f442 = 39.3750 * sini2 * sini2; double f522 = 9.84375 * m_sinio * (sini2 * (1.0 - 2.0 * m_cosio - 5.0 * cosi2) + 0.33333333 * (-2.0 + 4.0 * m_cosio + 6.0 * cosi2)); double f523 = m_sinio * (4.92187512 * sini2 * (-2.0 - 4.0 * m_cosio + 10.0 * cosi2) + 6.56250012 * (1.0 + 2.0 * m_cosio - 3.0 * cosi2)); double f542 = 29.53125 * m_sinio * (2.0 - 8.0 * m_cosio + cosi2 * (-12.0 + 8.0 * m_cosio + 10.0 * cosi2)); double f543 = 29.53125 * m_sinio * (-2.0 - 8.0 * m_cosio + cosi2 * (12.0 + 8.0 * m_cosio - 10.0 * cosi2)); double xno2 = m_xnodp * m_xnodp; double ainv2 = aqnv * aqnv; double temp1 = 3.0 * xno2 * ainv2; const double root22 = 1.7891679E-6; const double root32 = 3.7393792E-7; const double root44 = 7.3636953E-9; const double root52 = 1.1428639E-7; const double root54 = 2.1765803E-9; double temp = temp1 * root22; dp_d2201 = temp * f220 * g201; dp_d2211 = temp * f221 * g211; temp1 = temp1 * aqnv; temp = temp1 * root32; dp_d3210 = temp * f321 * g310; dp_d3222 = temp * f322 * g322; temp1 = temp1 * aqnv; temp = 2.0 * temp1 * root44; dp_d4410 = temp * f441 * g410; dp_d4422 = temp * f442 * g422; temp1 = temp1 * aqnv; temp = temp1 * root52; dp_d5220 = temp * f522 * g520; dp_d5232 = temp * f523 * g532; temp = 2.0 * temp1 * root54; dp_d5421 = temp * f542 * g521; dp_d5433 = temp * f543 * g533; dp_xlamo = xmao + Orbit.RAAN + Orbit.RAAN - dp_thgr - dp_thgr; bfact = m_xmdot + m_xnodot + m_xnodot - thdt - thdt; bfact = bfact + dp_ssl + dp_ssh + dp_ssh; #endregion } if (gp_reso || gp_sync) { dp_xfact = bfact - m_xnodp; // Initialize integrator dp_xli = dp_xlamo; dp_xni = m_xnodp; // dp_atime = 0.0; // performed by runtime dp_stepp = 720.0; dp_stepn = -720.0; dp_step2 = 259200.0; } }
/// <summary> /// 计算卫星位置 /// </summary> /// <param name="incl">轨道倾角</param> /// <param name="omega"></param> /// <param name="e">轨道偏心率</param> /// <param name="a">轨道长半轴</param> /// <param name="xl"></param> /// <param name="xnode"></param> /// <param name="xn"></param> /// <param name="tsince">和参考时间的间隔(分钟)</param> /// <returns></returns> protected TimedMotionState FinalPosition(double incl, double omega, double e, double a, double xl, double xnode, double xn, double tsince) { if ((e * e) > 1.0) { throw new Exception("Error in satellite data"); } double beta = Math.Sqrt(1.0 - e * e); // Long period periodics double axn = e * Math.Cos(omega); double temp = 1.0 / (a * beta * beta); double xll = temp * m_xlcof * axn; double aynl = temp * m_aycof; double xlt = xl + xll; double ayn = e * Math.Sin(omega) + aynl; // Solve Kepler's Equation double capu = GeoMath.ModTwoPI(xlt - xnode); double temp2 = capu; double temp3 = 0.0; double temp4 = 0.0; double temp5 = 0.0; double temp6 = 0.0; double sinepw = 0.0; double cosepw = 0.0; bool fDone = false; for (int i = 1; (i <= 10) && !fDone; i++) { sinepw = Math.Sin(temp2); cosepw = Math.Cos(temp2); temp3 = axn * sinepw; temp4 = ayn * cosepw; temp5 = axn * cosepw; temp6 = ayn * sinepw; double epw = (capu - temp4 + temp3 - temp2) / (1.0 - temp5 - temp6) + temp2; if (Math.Abs(epw - temp2) <= 1.0e-06) { fDone = true; } else { temp2 = epw; } } // Short period preliminary quantities double ecose = temp5 + temp6; double esine = temp3 - temp4; double elsq = axn * axn + ayn * ayn; temp = 1.0 - elsq; double pl = a * temp; double r = a * (1.0 - ecose); double temp1 = 1.0 / r; double rdot = OrbitConsts.Xke * Math.Sqrt(a) * esine * temp1; double rfdot = OrbitConsts.Xke * Math.Sqrt(pl) * temp1; temp2 = a * temp1; double betal = Math.Sqrt(temp); temp3 = 1.0 / (1.0 + betal); double cosu = temp2 * (cosepw - axn + ayn * esine * temp3); double sinu = temp2 * (sinepw - ayn - axn * esine * temp3); double u = GeoMath.AcTan(sinu, cosu); double sin2u = 2.0 * sinu * cosu; double cos2u = 2.0 * cosu * cosu - 1.0; temp = 1.0 / pl; temp1 = OrbitConsts.Ck2 * temp; temp2 = temp1 * temp; // Update for short periodics double rk = r * (1.0 - 1.5 * temp2 * betal * m_x3thm1) + 0.5 * temp1 * m_x1mth2 * cos2u; double uk = u - 0.25 * temp2 * m_x7thm1 * sin2u; double xnodek = xnode + 1.5 * temp2 * m_cosio * sin2u; double xinck = incl + 1.5 * temp2 * m_cosio * m_sinio * cos2u; double rdotk = rdot - xn * temp1 * m_x1mth2 * sin2u; double rfdotk = rfdot + xn * temp1 * (m_x1mth2 * cos2u + 1.5 * m_x3thm1); // Orientation vectors double sinuk = Math.Sin(uk); double cosuk = Math.Cos(uk); double sinik = Math.Sin(xinck); double cosik = Math.Cos(xinck); double sinnok = Math.Sin(xnodek); double cosnok = Math.Cos(xnodek); double xmx = -sinnok * cosik; double xmy = cosnok * cosik; double ux = xmx * sinuk + cosnok * cosuk; double uy = xmy * sinuk + sinnok * cosuk; double uz = sinik * sinuk; double vx = xmx * cosuk - cosnok * sinuk; double vy = xmy * cosuk - sinnok * sinuk; double vz = sinik * cosuk; // Position double x = rk * ux; double y = rk * uy; double z = rk * uz; XYZ vecPos = new XYZ(x, y, z); DateTime gmt = Orbit.EpochTime.AddMinutes(tsince); // Validate on altitude double altKm = (vecPos.Magnitude() * (OrbitConsts.RadiusOfEquator / OrbitConsts.Ae)); if (altKm < OrbitConsts.RadiusOfEquator) { throw new Exception("高度错误," + gmt + Orbit.SatNameLong); } // Velocity double xdot = rdotk * ux + rfdotk * vx; double ydot = rdotk * uy + rfdotk * vy; double zdot = rdotk * uz + rfdotk * vz; XYZ vecVel = new XYZ(xdot, ydot, zdot); return(new TimedMotionState(vecPos, vecVel, new Julian(gmt))); }