//------------------------------------------------------------------------------
//
// 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);
}
