//------------------------------------------------------------------------------
//
// 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]
}
