//------------------------------------------------------------------------------ // // Elements // // Purpose: // // Computes the osculating Keplerian elements from the satellite state vector // for elliptic orbits // // Input/Output: // // GM Gravitational coefficient // (gravitational constant * mass of central body) // y State vector (x,y,z,vx,vy,vz) // <return> Keplerian elements (a,e,i,Omega,omega,M) with // a Semimajor axis // e Eccentricity // i Inclination [rad] // Omega Longitude of the ascending node [rad] // omega Argument of pericenter [rad] // M Mean anomaly [rad] // // Notes: // // The state vector and GM must be given in consistent units, // e.g. [m], [m/s] and [m^3/s^2]. The resulting unit of the semimajor // axis is implied by the unity of y, e.g. [m]. // // The function cannot be used with state vectors describing a circular // or non-inclined orbit. // //------------------------------------------------------------------------------ /// <summary> /// 由某点的位置和速度,求解开普勒轨道根数。 /// </summary> /// <param name="GM"> Gravitational coefficien<t/param> /// <param name="y"> State vector (x,y,z,vx,vy,vz) </param> /// <returns></returns> public static Geo.Algorithm.Vector Elements(double GM, Geo.Algorithm.Vector stateVector) { // Variables Geo.Algorithm.Vector r = new Geo.Algorithm.Vector(3), v = new Geo.Algorithm.Vector(3), h = new Geo.Algorithm.Vector(3); double H, u, R; double eCosE, eSinE, e2, E, nu; double a, e, i, Omega, omega, M; r = stateVector.Slice(0, 2); // Position v = stateVector.Slice(3, 5); // Velocity h = r.Cross3D(v); // Areal velocity H = h.Norm(); Omega = Math.Atan2(h[0], -h[1]); // Long. ascend. node Omega = MathUtil.Modulo(Omega, OrbitConsts.TwoPI); i = Math.Atan2(Math.Sqrt(h[0] * h[0] + h[1] * h[1]), h[2]); // Inclination u = Math.Atan2(r[2] * H, -r[0] * h[1] + r[1] * h[0]); // Arg. of latitude R = r.Norm(); // Distance a = 1.0 / (2.0 / R - v.Dot(v) / GM); // Semi-major axis eCosE = 1.0 - R / a; // e*cos(E) eSinE = r.Dot(v) / Math.Sqrt(GM * a); // e*Math.Sin(E) e2 = eCosE * eCosE + eSinE * eSinE; e = Math.Sqrt(e2); // Eccentricity E = Math.Atan2(eSinE, eCosE); // Eccentric anomaly M = MathUtil.Modulo(E - eSinE, OrbitConsts.TwoPI); // Mean anomaly nu = Math.Atan2(Math.Sqrt(1.0 - e2) * eSinE, eCosE - e2); // True anomaly omega = MathUtil.Modulo(u - nu, OrbitConsts.TwoPI); // Arg. of perihelion // Keplerian elements vector return(new Geo.Algorithm.Vector(a, e, i, Omega, omega, M)); }
/// <summary> ///计算航天器是否在太阳光照中。1 为是, 0 为否。 可用于计算太阳光压。 /// Computes the fractional illumination of a spacecraft in the /// vicinity of the Earth assuming a cylindrical shadow model /// </summary> /// <param name="satXyz_m">Spacecraft position vector [m]</param> /// <param name="sunXyz_m"> Sun position vector [m]</param> /// <returns> Illumination factor: /// nu=0 Spacecraft in Earth shadow /// nu=1 Spacecraft fully illuminated by the Sun</returns> public static double Illumination(Geo.Algorithm.Vector satXyz_m, Geo.Algorithm.Vector sunXyz_m) { Geo.Algorithm.Vector e_Sun = sunXyz_m.DirectionUnit(); // Sun direction unit vector double s = satXyz_m.Dot(e_Sun); // Projection of s/c position return((s > 0 || (satXyz_m - s * e_Sun).Norm() > OrbitConsts.RadiusOfEarth) ? 1.0 : 0.0); }
/// <summary> /// 从两个位置向量和中间时间计算扇区三角形比率 /// Computes the sector-triangle ratio from two position vectors and /// the intermediate time /// </summary> /// <param name="r_a">r_a Position at time t_a</param> /// <param name="r_b">Position at time t_b</param> /// <param name="tau"> Normalized time (Math.Sqrt(GM)*(t_a-t_b))</param> /// <returns> Sector-triangle ratio</returns> private static double FindEta(Geo.Algorithm.Vector r_a, Geo.Algorithm.Vector r_b, double tau) { // Constants const int maxit = 30; const double delta = 100.0 * eps_mach; // Variables int i; double kappa, m, l, s_a, s_b, eta_min, eta1, eta2, F1, F2, d_eta; // Auxiliary quantities s_a = r_a.Norm(); s_b = r_b.Norm(); kappa = Math.Sqrt(2.0 * (s_a * s_b + r_a.Dot(r_b))); m = tau * tau / Math.Pow(kappa, 3); l = (s_a + s_b) / (2.0 * kappa) - 0.5; eta_min = Math.Sqrt(m / (l + 1.0)); // Start with Hansen's approximation eta2 = (12.0 + 10.0 * Math.Sqrt(1.0 + (44.0 / 9.0) * m / (l + 5.0 / 6.0))) / 22.0; eta1 = eta2 + 0.1; // Secant method F1 = F(eta1, m, l); F2 = F(eta2, m, l); i = 0; while (Math.Abs(F2 - F1) > delta) { d_eta = -F2 * (eta2 - eta1) / (F2 - F1); eta1 = eta2; F1 = F2; while (eta2 + d_eta <= eta_min) { d_eta *= 0.5; } eta2 += d_eta; F2 = F(eta2, m, l); ++i; if (i == maxit) { throw new ArgumentException("WARNING: Convergence problems in FindEta"); break; } } return(eta2); }
/// <summary> /// Scalar Measurement Update /// </summary> /// <param name="z"></param> /// <param name="g"></param> /// <param name="sigma"></param> /// <param name="G"></param> public void MeasUpdate(double z, // Measurement at new epoch double g, // Modelled measurement double sigma, // Standard deviation Geo.Algorithm.Vector G) // Partials dg/dx { Geo.Algorithm.Vector K = new Geo.Algorithm.Vector(n); // Kalman gain double Inv_W = sigma * sigma; // Inverse weight (measurement covariance) // Kalman gain K = P * G / (Inv_W + G.Dot(P * G)); // State update x = x + K * (z - g); // Covariance update P = (Matrix.CreateIdentity(n) - MatrixUtil.Dyadic(K, G)) * P; }
/// <summary> /// 本地空间直角坐标到极坐标的转换,结果包括微分。 AzEl Computes azimuth, elevation and partials from local tangent coordinates /// </summary> /// <param name="localEnz">本地水平坐标ENZ, Topocentric local tangent coordinates (East-North-Zenith frame)</param> /// <param name="azimuthRad"> A Azimuth [rad] </param> /// <param name="elevationRad"> E Elevation [rad]</param> /// <param name="dAds"> dAds Partials of azimuth w.r.t. s</param> /// <param name="dEds"> dEds Partials of elevation w.r.t. s</param> static public void LocalEnzToPolar(Geo.Algorithm.Vector localEnz, out double azimuthRad, out double elevationRad, out Geo.Algorithm.Vector dAds, out Geo.Algorithm.Vector dEds) { double rho = Math.Sqrt(localEnz[0] * localEnz[0] + localEnz[1] * localEnz[1]); // Angles azimuthRad = Math.Atan2(localEnz[0], localEnz[1]); azimuthRad = ((azimuthRad < 0.0) ? azimuthRad + OrbitConsts.TwoPI : azimuthRad); elevationRad = Math.Atan(localEnz[2] / rho); // Partials dAds = new Geo.Algorithm.Vector(localEnz[1] / (rho * rho), -localEnz[0] / (rho * rho), 0.0); dEds = new Geo.Algorithm.Vector(-localEnz[0] * localEnz[2] / rho, -localEnz[1] * localEnz[2] / rho, rho) / localEnz.Dot(localEnz); }
//------------------------------------------------------------------------------ // // Elements // // Purpose: // // Computes orbital elements from two given position vectors and // associated times // // Input/Output: // // GM Gravitational coefficient // (gravitational constant * mass of central body) // Mjd_a Time t_a (Modified Julian Date) // Mjd_b Time t_b (Modified Julian Date) // r_a Position vector at time t_a // r_b Position vector at time t_b // <return> Keplerian elements (a,e,i,Omega,omega,M) // a Semimajor axis // e Eccentricity // i Inclination [rad] // Omega Longitude of the ascending node [rad] // omega Argument of pericenter [rad] // M Mean anomaly [rad] // at time t_a // // Notes: // // The function cannot be used with state vectors describing a circular // or non-inclined orbit. // //------------------------------------------------------------------------------ /// <summary> /// 计算轨道根数,由两个已知位置。 /// </summary> /// <param name="GM"></param> /// <param name="Mjd_a"></param> /// <param name="Mjd_b"></param> /// <param name="r_a"></param> /// <param name="r_b"></param> /// <returns></returns> public static Geo.Algorithm.Vector Elements(double GM, double Mjd_a, double Mjd_b, Geo.Algorithm.Vector r_a, Geo.Algorithm.Vector r_b) { // Variables double tau, eta, p; double n, nu, E, u; double s_a, s_b, s_0, fac, sinhH; double cos_dnu, sin_dnu, ecos_nu, esin_nu; double a, e, i, Omega, omega, M; Geo.Algorithm.Vector e_a = new Geo.Algorithm.Vector(3), r_0 = new Geo.Algorithm.Vector(3), e_0 = new Geo.Algorithm.Vector(3), W = new Geo.Algorithm.Vector(3); // Calculate vector r_0 (fraction of r_b perpendicular to r_a) // and the magnitudes of r_a,r_b and r_0 s_a = r_a.Norm(); e_a = r_a / s_a; s_b = r_b.Norm(); fac = r_b.Dot(e_a); r_0 = r_b - fac * e_a; s_0 = r_0.Norm(); e_0 = r_0 / s_0; // Inclination and ascending node W = e_a.Cross3D(e_0); Omega = Math.Atan2(W[0], -W[1]); // Long. ascend. node Omega = MathUtil.Modulo(Omega, OrbitConsts.TwoPI); i = Math.Atan2(Math.Sqrt(W[0] * W[0] + W[1] * W[1]), W[2]); // Inclination if (i == 0.0) { u = Math.Atan2(r_a[1], r_a[0]); } else { u = Math.Atan2(+e_a[2], -e_a[0] * W[1] + e_a[1] * W[0]); } // Semilatus rectum tau = Math.Sqrt(GM) * 86400.0 * Math.Abs(Mjd_b - Mjd_a); eta = FindEta(r_a, r_b, tau); p = Math.Pow(s_a * s_0 * eta / tau, 2); // Eccentricity, true anomaly and argument of perihelion cos_dnu = fac / s_b; sin_dnu = s_0 / s_b; ecos_nu = p / s_a - 1.0; esin_nu = (ecos_nu * cos_dnu - (p / s_b - 1.0)) / sin_dnu; e = Math.Sqrt(ecos_nu * ecos_nu + esin_nu * esin_nu); nu = Math.Atan2(esin_nu, ecos_nu); omega = MathUtil.Modulo(u - nu, OrbitConsts.TwoPI); // Perihelion distance, semimajor axis and mean motion a = p / (1.0 - e * e); n = Math.Sqrt(GM / Math.Abs(a * a * a)); // Mean anomaly and time of perihelion passage if (e < 1.0) { E = Math.Atan2(Math.Sqrt((1.0 - e) * (1.0 + e)) * esin_nu, ecos_nu + e * e); M = MathUtil.Modulo(E - e * Math.Sin(E), OrbitConsts.TwoPI); } else { sinhH = Math.Sqrt((e - 1.0) * (e + 1.0)) * esin_nu / (e + e * ecos_nu); M = e * sinhH - Math.Log(sinhH + Math.Sqrt(1.0 + sinhH * sinhH)); } // Keplerian elements vector return(new Geo.Algorithm.Vector(a, e, i, Omega, omega, M)); }
/// <summary> /// 计算地球重力场引起的加速度。 /// Computes the acceleration due to the harmonic gravity field of the central body /// </summary> /// <param name="satXyz"> r Satellite position vector in the inertial system</param> /// <param name="earthRotationMatrix"> E Transformation matrix to body-fixed system</param> /// <param name="GM"> GM Gravitational coefficient</param> /// <param name="R_ref">R_ref Reference radius </param> /// <param name="CS">CS Spherical harmonics coefficients (un-normalized)</param> /// <param name="n_max">n_max Maximum degree </param> /// <param name="m_max"> m_max Maximum order (m_max<=n_max; m_max=0 for zonals, only)</param> /// <returns>Acceleration (a=d^2r/dt^2)</returns> public static Geo.Algorithm.Vector AccelerOfHarmonicGraviFiled(Geo.Algorithm.Vector satXyz, Matrix earthRotationMatrix, double GM, double radiusOfRefer, Matrix spherHarmoCoeff, int maxDegree, int maxOrder) { // Local variables int n, m; // Loop counters double r_sqr, rho, Fac; // Auxiliary quantities double x0, y0, z0; // Normalized coordinates double ax, ay, az; // Acceleration vector double C, S; // Gravitational coefficients Geo.Algorithm.Vector r_bf = new Geo.Algorithm.Vector(3); // Body-fixed position Geo.Algorithm.Vector a_bf = new Geo.Algorithm.Vector(3); // Body-fixed acceleration Matrix V = new Matrix(maxDegree + 2, maxDegree + 2); // Harmonic functions Matrix W = new Matrix(maxDegree + 2, maxDegree + 2); // work array (0..n_max+1,0..n_max+1) // Body-fixed position r_bf = earthRotationMatrix * satXyz; // Auxiliary quantities r_sqr = r_bf.Dot(r_bf); // Square of distance rho = radiusOfRefer * radiusOfRefer / r_sqr; x0 = radiusOfRefer * r_bf[0] / r_sqr; // Normalized y0 = radiusOfRefer * r_bf[1] / r_sqr; // coordinates z0 = radiusOfRefer * r_bf[2] / r_sqr; // Evaluate harmonic functions // V_nm = (R_ref/r)^(n+1) * P_nm(sin(phi)) * cos(m*lambda) // and // W_nm = (R_ref/r)^(n+1) * P_nm(sin(phi)) * sin(m*lambda) // up to degree and order n_max+1 // // Calculate zonal terms V(n,0); set W(n,0)=0.0 V[0, 0] = radiusOfRefer / Math.Sqrt(r_sqr); W[0, 0] = 0.0; V[1, 0] = z0 * V[0, 0]; W[1, 0] = 0.0; for (n = 2; n <= maxDegree + 1; n++) { V[n, 0] = ((2 * n - 1) * z0 * V[n - 1, 0] - (n - 1) * rho * V[n - 2, 0]) / n; W[n, 0] = 0.0; } // Calculate tesseral and sectorial terms for (m = 1; m <= maxOrder + 1; m++) { // Calculate V(m,m) .. V(n_max+1,m) V[m, m] = (2 * m - 1) * (x0 * V[m - 1, m - 1] - y0 * W[m - 1, m - 1]); W[m, m] = (2 * m - 1) * (x0 * W[m - 1, m - 1] + y0 * V[m - 1, m - 1]); if (m <= maxDegree) { V[m + 1, m] = (2 * m + 1) * z0 * V[m, m]; W[m + 1, m] = (2 * m + 1) * z0 * W[m, m]; } for (n = m + 2; n <= maxDegree + 1; n++) { V[n, m] = ((2 * n - 1) * z0 * V[n - 1, m] - (n + m - 1) * rho * V[n - 2, m]) / (n - m); W[n, m] = ((2 * n - 1) * z0 * W[n - 1, m] - (n + m - 1) * rho * W[n - 2, m]) / (n - m); } } // // Calculate accelerations ax,ay,az // ax = ay = az = 0.0; for (m = 0; m <= maxOrder; m++) { for (n = m; n <= maxDegree; n++) { if (m == 0) { C = spherHarmoCoeff[n, 0]; // = C_n,0 ax -= C * V[n + 1, 1]; ay -= C * W[n + 1, 1]; az -= (n + 1) * C * V[n + 1, 0]; } else { C = spherHarmoCoeff[n, m]; // = C_n,m S = spherHarmoCoeff[m - 1, n]; // = S_n,m Fac = 0.5 * (n - m + 1) * (n - m + 2); ax += +0.5 * (-C * V[n + 1, m + 1] - S * W[n + 1, m + 1]) + Fac * (+C * V[n + 1, m - 1] + S * W[n + 1, m - 1]); ay += +0.5 * (-C * W[n + 1, m + 1] + S * V[n + 1, m + 1]) + Fac * (-C * W[n + 1, m - 1] + S * V[n + 1, m - 1]); az += (n - m + 1) * (-C * V[n + 1, m] - S * W[n + 1, m]); } } } // Body-fixed acceleration a_bf = (GM / (radiusOfRefer * radiusOfRefer)) * new Geo.Algorithm.Vector(ax, ay, az); // Inertial acceleration return(earthRotationMatrix.Transpose() * a_bf); }
/// <summary> /// 计算大气密度,基于Harris-Priesterm模型。 /// Computes the atmospheric density for the modified Harris-Priester model. /// </summary> /// <param name="Mjd_TT">Mjd_TT Terrestrial Time (Modified Julian Date)</param> /// <param name="xyz">r_tod Satellite position vector in the inertial system [m]</param> /// <returns> Density [kg/m^3]</returns> public static double AtmosDensity_HP(double Mjd_TT, Geo.Algorithm.Vector xyz) { // Constants const double upper_limit = 1000.0; // Upper height limit [km] const double lower_limit = 100.0; // Lower height limit [km] const double ra_lag = 0.523599; // Right ascension lag [rad] const int n_prm = 3; // Harris-Priester parameter // 2(6) low(high) inclination // Harris-Priester atmospheric density model parameters // Height [km], minimum density, maximum density [gm/km^3] const int N_Coef = 50; Geo.Algorithm.Vector h = new Geo.Algorithm.Vector(Data_h, N_Coef); Geo.Algorithm.Vector c_min = new Geo.Algorithm.Vector(Data_c_min, N_Coef); Geo.Algorithm.Vector c_max = new Geo.Algorithm.Vector(Data_c_max, N_Coef); // Variables int i, ih; // Height section variables double height; // Earth flattening double dec_Sun, ra_Sun, c_dec; // Sun declination, right asc. double c_psi2; // Harris-Priester modification double density, h_min, h_max, d_min, d_max; // Height, density parameters Geo.Algorithm.Vector r_Sun = new Geo.Algorithm.Vector(3); // Sun position Geo.Algorithm.Vector u = new Geo.Algorithm.Vector(3); // Apex of diurnal bulge // Satellite height var geoCoord = Geo.Coordinates.CoordTransformer.XyzToGeoCoord(new Geo.Coordinates.XYZ(xyz[0], xyz[1], xyz[2])); height = geoCoord.Height / 1000.0; // [km] // Exit with zero density outside height model limits if (height >= upper_limit || height <= lower_limit) { return(0.0); } // Sun right ascension, declination r_Sun = CelestialUtil.Sun(Mjd_TT); ra_Sun = Math.Atan2(r_Sun[1], r_Sun[0]); dec_Sun = Math.Atan2(r_Sun[2], Math.Sqrt(Math.Pow(r_Sun[0], 2) + Math.Pow(r_Sun[1], 2))); // Unit vector u towards the apex of the diurnal bulge in inertial geocentric coordinates c_dec = cos(dec_Sun); u[0] = c_dec * cos(ra_Sun + ra_lag); u[1] = c_dec * sin(ra_Sun + ra_lag); u[2] = sin(dec_Sun); // Cosine of half angle between satellite position vector and apex of diurnal bulge c_psi2 = 0.5 + 0.5 * xyz.Dot(u) / xyz.Norm(); // Height index search and exponential density interpolation ih = 0; // section index reset for (i = 0; i <= N_Coef - 1; i++) // loop over N_Coef height regimes { if (height >= h[i] && height < h[i + 1]) { ih = i; break; } // ih identifies height section } h_min = (h[ih] - h[ih + 1]) / Math.Log(c_min[ih + 1] / c_min[ih]); h_max = (h[ih] - h[ih + 1]) / Math.Log(c_max[ih + 1] / c_max[ih]); d_min = c_min[ih] * Math.Exp((h[ih] - height) / h_min); d_max = c_max[ih] * Math.Exp((h[ih] - height) / h_max); // Density computation density = d_min + (d_max - d_min) * Math.Pow(c_psi2, n_prm); return(density * 1.0e-12); // [kg/m^3] }