//------------------------------------------------------------------------------ // // 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> /// 计算大气延迟。 Computes the acceleration due to the atmospheric drag. /// </summary> /// <param name="Mjd_TT">Mjd_TT Terrestrial Time (Modified Julian Date)</param> /// <param name="satXyz">r Satellite position vector in the inertial system [m]</param> /// <param name="satVelocity"> v Satellite velocity vector in the inertial system [m/s]</param> /// <param name="transMatrix"> T Transformation matrix to true-of-date inertial system</param> /// <param name="Area">Area Cross-section [m^2]</param> /// <param name="mass"> mass Spacecraft mass [kg]</param> /// <param name="atmosDragCoeff"> coefOfDrag Drag coefficient</param> /// <returns>Acceleration (a=d^2r/dt^2) [m/s^2]</returns> public static Geo.Algorithm.Vector AccelerDragOfAtmos(double Mjd_TT, Geo.Algorithm.Vector satXyz, Geo.Algorithm.Vector satVelocity, Matrix transMatrix, double Area, double mass, double atmosDragCoeff) { // Earth angular velocity vector [rad/s] double[] Data_omega = { 0.0, 0.0, 7.29212e-5 }; Geo.Algorithm.Vector omega = new Geo.Algorithm.Vector(Data_omega, 3); // Variables double v_abs, dens; Geo.Algorithm.Vector r_tod = new Geo.Algorithm.Vector(3), v_tod = new Geo.Algorithm.Vector(3); Geo.Algorithm.Vector v_rel = new Geo.Algorithm.Vector(3), a_tod = new Geo.Algorithm.Vector(3); Matrix T_trp = new Matrix(3, 3); // Transformation matrix to ICRF/EME2000 system T_trp = transMatrix.Transpose(); // Position and velocity in true-of-date system r_tod = transMatrix * satXyz; v_tod = transMatrix * satVelocity; // Velocity relative to the Earth's atmosphere v_rel = v_tod - omega.Cross3D(r_tod); v_abs = v_rel.Norm(); // Atmospheric density due to modified Harris-Priester model dens = TerrestrialUtil.AtmosDensity_HP(Mjd_TT, r_tod); // Acceleration a_tod = -0.5 * atmosDragCoeff * (Area / mass) * dens * v_abs * v_rel; return(T_trp * a_tod); }
//------------------------------------------------------------------------------ // // 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)); }
//------------------------------------------------------------------------------ // // StatePartials // // Purpose: // // Computes the partial derivatives of the satellite state vector with respect // to the orbital elements for elliptic, Keplerian orbits // // Input/Output: // // GM Gravitational coefficient // (gravitational constant * mass of central body) // Kep Keplerian elements (a,e,i,Omega,omega,M) at epoch with // a Semimajor axis // e Eccentricity // i Inclination [rad] // Omega Longitude of the ascending node [rad] // omega Argument of pericenter [rad] // M Mean anomaly at epoch [rad] // dt Time since epoch // <return> Partials derivatives of the state vector (x,y,z,vx,vy,vz) at time // dt with respect to the epoch orbital elements // // Notes: // // The semimajor axis a=Kep(0), dt and GM must be given in consistent units, // e.g. [m], [s] and [m^3/s^2]. The resulting units of length and velocity // are implied by the units of GM, e.g. [m] and [m/s]. // // The function cannot be used with circular or non-inclined orbit. // //------------------------------------------------------------------------------ /// <summary> /// 计算卫星状态向量相对于椭圆轨道元素的偏导数 /// </summary> /// <param name="GM"></param> /// <param name="kepElements"></param> /// <param name="dt"></param> /// <returns></returns> private static Matrix StatePartials(double GM, Geo.Algorithm.Vector kepElements, double dt) { // Variables int k; double a, e, i, Omega, omega, M, M0, n, dMda; double E, cosE, sinE, fac, r, v, x, y, vx, vy; Matrix PQW = new Matrix(3, 3); Geo.Algorithm.Vector P = new Geo.Algorithm.Vector(3), Q = new Geo.Algorithm.Vector(3), W = new Geo.Algorithm.Vector(3), e_z = new Geo.Algorithm.Vector(3), N = new Geo.Algorithm.Vector(3); Geo.Algorithm.Vector dPdi = new Geo.Algorithm.Vector(3), dPdO = new Geo.Algorithm.Vector(3), dPdo = new Geo.Algorithm.Vector(3), dQdi = new Geo.Algorithm.Vector(3), dQdO = new Geo.Algorithm.Vector(3), dQdo = new Geo.Algorithm.Vector(3); Geo.Algorithm.Vector dYda = new Geo.Algorithm.Vector(6), dYde = new Geo.Algorithm.Vector(6), dYdi = new Geo.Algorithm.Vector(6), dYdO = new Geo.Algorithm.Vector(6), dYdo = new Geo.Algorithm.Vector(6), dYdM = new Geo.Algorithm.Vector(6); Matrix dYdA = new Matrix(6, 6); // Keplerian elements at epoch a = kepElements[0]; Omega = kepElements[3]; e = kepElements[1]; omega = kepElements[4]; i = kepElements[2]; M0 = kepElements[5]; // Mean and eccentric anomaly n = Math.Sqrt(GM / (a * a * a)); M = M0 + n * dt; E = EccAnom(M, e); // Perifocal coordinates cosE = Math.Cos(E); sinE = Math.Sin(E); fac = Math.Sqrt((1.0 - e) * (1.0 + e)); r = a * (1.0 - e * cosE); // Distance v = Math.Sqrt(GM * a) / r; // Velocity x = +a * (cosE - e); y = +a * fac * sinE; vx = -v * sinE; vy = +v * fac * cosE; // Transformation to reference system (Gaussian vectors) and partials PQW = Matrix.RotateZ3D(-Omega) * Matrix.RotateX3D(-i) * Matrix.RotateZ3D(-omega); P = PQW.Col(0); Q = PQW.Col(1); W = PQW.Col(2); e_z = new Geo.Algorithm.Vector(0, 0, 1); N = e_z.Cross3D(W); N = N / N.Norm(); dPdi = N.Cross3D(P); dPdO = e_z.Cross3D(P); dPdo = Q; dQdi = N.Cross3D(Q); dQdO = e_z.Cross3D(Q); dQdo = -P; // Partials w.r.t. semimajor axis, eccentricity and mean anomaly at time dt dYda = ((x / a) * P + (y / a) * Q).Stack((-vx / (2 * a)) * P + (-vy / (2 * a)) * Q); dYde = (((-a - Math.Pow(y / fac, 2) / r) * P + (x * y / (r * fac * fac)) * Q).Stack( (vx * (2 * a * x + e * Math.Pow(y / fac, 2)) / (r * r)) * P + ((n / fac) * Math.Pow(a / r, 2) * (x * x / r - Math.Pow(y / fac, 2) / a)) * Q)); dYdM = ((vx * P + vy * Q) / n).Stack((-n * Math.Pow(a / r, 3)) * (x * P + y * Q)); // Partials w.r.t. inlcination, node and argument of pericenter dYdi = (x * dPdi + y * dQdi).Stack(vx * dPdi + vy * dQdi); dYdO = (x * dPdO + y * dQdO).Stack(vx * dPdO + vy * dQdO); dYdo = (x * dPdo + y * dQdo).Stack(vx * dPdo + vy * dQdo); // Derivative of mean anomaly at time dt w.r.t. the semimajor axis at epoch dMda = -1.5 * (n / a) * dt; // Combined partial derivative matrix of state with respect to epoch elements for (k = 0; k < 6; k++) { dYdA[k, 0] = dYda[k] + dYdM[k] * dMda; dYdA[k, 1] = dYde[k]; dYdA[k, 2] = dYdi[k]; dYdA[k, 3] = dYdO[k]; dYdA[k, 4] = dYdo[k]; dYdA[k, 5] = dYdM[k]; } return(dYdA); }