//------------------------------------------------------------------------------
//
// 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 derivative of the state vector for the normalized (GM=1)
/// Kepler's problem in three dimensions
/// Note:
///
/// pAux is expected to point to an integer variable that will be incremented
// by one on each call of Deriv
/// </summary>
/// <param name="t"></param>
/// <param name="y"></param>
/// <param name="pAux"></param>
/// <returns></returns>
static Geo.Algorithm.Vector f_Kep6D(double t, Geo.Algorithm.Vector y, Object pAux)
{
// Pointer to auxiliary integer variable used as function call counter
// State vector derivative
Geo.Algorithm.Vector r = y.Slice(0, 2);
Geo.Algorithm.Vector v = y.Slice(3, 5);
Geo.Algorithm.Vector yp = v.Stack(-r / (Math.Pow(r.Norm(), 3)));
// Increment function call count
((Action)pAux)();
return(yp);
}
/// <summary>
/// 导数、微分
/// Computes the derivative of the state vector .
/// pAux is expected to point to a variable of type AuxDataRecord, which is
/// used to communicate with the other program sections and to hold satData
/// between subsequent calls of this function
/// </summary>
/// <param name="t">历元</param>
/// <param name="y">卫星状态，坐标和速度</param>
/// <param name="pAux">附加选项</param>
static Geo.Algorithm.Vector Deriv(double t, Geo.Algorithm.Vector y, Object pAux)
{
// Pointer to auxiliary satData record
ForceModelOption p = (ForceModelOption)pAux;// static_cast<ForceModelOption*>(pAux);
// Time
double Mjd_TT = p.Mjd0_TT + t / 86400.0;
// State vector components
Geo.Algorithm.Vector r = y.Slice(0, 2);
Geo.Algorithm.Vector v = y.Slice(3, 5);
// Acceleration
AccelerationCalculator calculator = new AccelerationCalculator(p, IERS);
var a = calculator.GetAcceleration(Mjd_TT, r, v);
// State vector derivative
Geo.Algorithm.Vector yp = v.Stack(a);
return(yp);
}
//------------------------------------------------------------------------------
//
// f_Kep6D_
//
// Purpose:
//
// Computes the derivative of the state vector for the normalized (GM=1)
// Kepler's problem in three dimensions
//
// Note:
//
// pAux is expected to point to a variable of type AuxDataRecord, which is
// used to communicate with the other program sections and to hold satData
// between subsequent calls of this function
//
//------------------------------------------------------------------------------
static Geo.Algorithm.Vector f_Kep6D_(double t, Geo.Algorithm.Vector y, Object pAux)
{
// State vector derivative
Geo.Algorithm.Vector r = y.Slice(0, 2);
Geo.Algorithm.Vector v = y.Slice(3, 5);
Geo.Algorithm.Vector yp = v.Stack(-r / (Math.Pow(r.Norm(), 3)));
// Pointer to auxiliary satData record
AuxDataRecord p = (AuxDataRecord)pAux;// static_cast<AuxDataRecord*>(pAux);
// Write current time, step aboutSize and radius; store time for next step
if (t - p.t > 1.0e-10)
{
p.n_step++;
var info = String.Format("{0, 5:D}{1,12:F6}{2,12:F6}{3,12:F3}", p.n_step, t, t - p.t, r.Norm());
Console.WriteLine(info);
p.t = t;
}
;
pAux = p;
return(yp);
}
static void Main(string[] args)
{
// Constants
const double relerr = 1.0e-13; // Relative and absolute
const double abserr = 1.0e-6; // accuracy requirement
const double a = OrbitConsts.RadiusOfEarth + 650.0e3; // Semi-major axis [m]
const double e = 0.001; // Eccentricity
const double inc = OrbitConsts.RadPerDeg * 51.0; // Inclination [rad]
double n = Math.Sqrt(OrbitConsts.GM_Earth / (a * a * a)); // Mean motion
Matrix Scale = Matrix.CreateDiagonal(new Geo.Algorithm.Vector(1, 1, 1, 1 / n, 1 / n, 1 / n)); // Velocity
Matrix scale = Matrix.CreateDiagonal(new Geo.Algorithm.Vector(1, 1, 1, n, n, n)); // normalization
const double t_end = 86400.0;
const double step = 300.0;
// Variables
int i, j; // Loop counters
double Mjd0; // Epoch (Modified Julian Date)
double t; // Integration time [s]
double max, max_Kep; // Matrix error norm
AuxParam7 Aux1 = new AuxParam7(), Aux2 = new AuxParam7(); // Auxiliary parameter records
// Model parameters for reference solution (full 10x10 gravity model) and
// simplified variational equations
// Epoch state and transition matrix
Mjd0 = OrbitConsts.MJD_J2000;
Aux1.Mjd_0 = Mjd0; // Reference epoch
Aux1.n_a = 10; // Degree of gravity field for trajectory computation
Aux1.m_a = 10; // Order of gravity field for trajectory computation
Aux1.n_G = 10; // Degree of gravity field for variational equations
Aux1.m_G = 10; // Order of gravity field for variational equations
Aux2.Mjd_0 = Mjd0; // Reference epoch
Aux2.n_a = 2; // Degree of gravity field for trajectory computation
Aux2.m_a = 0; // Order of gravity field for trajectory computation
Aux2.n_G = 2; // Degree of gravity field for variational equations
Aux2.m_G = 0; // Order of gravity field for variational equations
DeIntegrator Int1 = new DeIntegrator(VarEqn, 42, Aux1); // Object for integrating the variat. eqns.
DeIntegrator Int2 = new DeIntegrator(VarEqn, 42, Aux2); //
Geo.Algorithm.Vector y0 = new Geo.Algorithm.Vector(6), y = new Geo.Algorithm.Vector(6); // State vector
Geo.Algorithm.Vector yPhi1 = new Geo.Algorithm.Vector(42), yPhi2 = new Geo.Algorithm.Vector(42); // State and transition matrix vector
Matrix Phi1 = new Matrix(6, 6), Phi2 = new Matrix(6, 6); // State transition matrix
Matrix Phi_Kep = new Matrix(6, 6); // State transition matrix (Keplerian)
y0 = Kepler.State(OrbitConsts.GM_Earth, new Geo.Algorithm.Vector(a, e, inc, 0.0, 0.0, 0.0));
for (i = 0; i <= 5; i++)
{
yPhi1[i] = y0[i];
for (j = 0; j <= 5; j++)
{
yPhi1[6 * (j + 1) + i] = (i == j ? 1 : 0);
}
}
yPhi2 = new Geo.Algorithm.Vector(yPhi1);
// Initialization
t = 0.0;
Int1.Init(t, relerr, abserr);
Int2.Init(t, relerr, abserr);
// Header
var endl = "\r\n";
var info = "Exercise 7-1: State transition matrix" + endl
+ endl
+ " Time [s] Error (J2) Error(Kep)" + endl;
// Steps
while (t < t_end)
{
// New output time
t += step;
// Integration
Int1.Integ(t, ref yPhi1); // Reference
Int2.Integ(t, ref yPhi2); // Simplified
//info= setprecision(5) + setw(12)+yPhi1 + endl;
//info= setprecision(5) + setw(12) + yPhi2 + endl;
// Extract and normalize state transition matrices
for (j = 0; j <= 5; j++)
{
Phi1.SetCol(j, yPhi1.Slice(6 * (j + 1), 6 * (j + 1) + 5));
}
for (j = 0; j <= 5; j++)
{
Phi2.SetCol(j, yPhi2.Slice(6 * (j + 1), 6 * (j + 1) + 5));
}
Phi1 = Scale * Phi1 * scale;
Phi2 = Scale * Phi2 * scale;
// Keplerian state transition matrix (normalized)
Kepler.TwoBody(OrbitConsts.GM_Earth, y0, t, ref y, ref Phi_Kep);
Phi_Kep = Scale * Phi_Kep * scale;
// Matrix error norms
max = Max(Matrix.CreateIdentity(6) - Phi1 * InvSymp(Phi2));
max_Kep = Max(Matrix.CreateIdentity(6) - Phi1 * InvSymp(Phi_Kep));
// Output
// info = String.Format( "{0, 10:F }{1, 10:F }{2, 10:F } ", t, max , max_Kep );
info = String.Format("{0, 10:F2}{1, 10:F2}{2, 12:F2}", t, max, max_Kep);
Console.WriteLine(info);
}
;
// Output
info = endl
+ "Time since epoch [s]" + endl
+ endl
+ String.Format("{0, 10:F}", t) + endl
+ endl;
Console.Write(info);
info = "State transition matrix (reference)" + endl
+ endl
+ Phi1 + endl;
Console.Write(info);
info = "J2 State transition matrix" + endl
+ endl
+ Phi2 + endl;
Console.Write(info);
info = "Keplerian State transition matrix" + endl
+ endl
+ Phi_Kep + endl;
Console.Write(info);
Console.ReadKey();
}
/// <summary>
/// Computes the variational equations, i.e. the derivative of the state vector
/// and the state transition matrix
/// </summary>
/// <param name="t">t Time since epoch (*pAux).Mjd_0 in [s]</param>
/// <param name="yPhi"> yPhi (6+36)-dim vector comprising the state vector (y) and the
/// state transition matrix (Phi) in column wise storage order</param>
/// <param name="yPhip"> yPhip Derivative of yPhi</param>
/// <param name="pAux">pAux Pointer; pAux is expected to point to a variable of type
/// AuxDataRecord, which is used to communicate with the other
/// program sections and to hold satData between subsequent calls
/// of this function</param>
static Geo.Algorithm.Vector VarEqn(double t, Geo.Algorithm.Vector yPhi, Object pAux) // void* pAux )
{
// Variables
AuxParam7 p; // Auxiliary satData pointer
int i, j; // Loop counters
double Mjd; // Modified Julian Date
Geo.Algorithm.Vector r = new Geo.Algorithm.Vector(3), v = new Geo.Algorithm.Vector(3); // Position, velocity
Geo.Algorithm.Vector a = new Geo.Algorithm.Vector(3); // Acceleration
Matrix G = new Matrix(3, 3); // Gradient of acceleration
Matrix Phi = new Matrix(6, 6); // State transition matrix
Matrix Phip = new Matrix(6, 6); // Time derivative of state transition matrix
Matrix dfdy = new Matrix(6, 6); //
// Pointer to auxiliary satData record
p = (AuxParam7)(pAux);
// Time
Mjd = p.Mjd_0 + t / 86400.0;
// State vector components
r = yPhi.Slice(0, 2);
v = yPhi.Slice(3, 5);
// State transition matrix
for (j = 0; j <= 5; j++)
{
Phi.SetCol(j, yPhi.Slice(6 * (j + 1), 6 * (j + 1) + 5));
}
// Acceleration and gradient
a = Accel(Mjd, r, p.n_a, p.m_a);
G = Gradient(Mjd, r, p.n_G, p.m_G);
// Time derivative of state transition matrix
for (i = 0; i <= 2; i++)
{
for (j = 0; j <= 2; j++)
{
dfdy[i, j] = 0.0; // dv/dr[i,j]
dfdy[i + 3, j] = G[i, j]; // da/dr[i,j]
dfdy[i, j + 3] = (i == j ? 1 : 0); // dv/dv[i,j]
dfdy[i + 3, j + 3] = 0.0; // da/dv[i,j]
}
;
}
;
Phip = dfdy * Phi;
// Derivative of combined state vector and state transition matrix
var yPhip = new Geo.Algorithm.Vector(42);
for (i = 0; i <= 2; i++)
{
yPhip[i] = v[i]; // dr/dt[i]
yPhip[i + 3] = a[i]; // dv/dt[i]
}
;
for (i = 0; i <= 5; i++)
{
for (j = 0; j <= 5; j++)
{
yPhip[6 * (j + 1) + i] = Phip[i, j]; // dPhi/dt[i,j]
}
}
return(yPhip);
}
