/// <summary>
/// 用反替换法求解向量x[]的LSQ问题
/// 通过反向替换解决向量x []的LSQ问题
/// Solve the LSQ problem for vector x[] by backsubstitution
/// </summary>
/// <param name="x"></param>
public void Solve(Geo.Algorithm.Vector x)
{
// Variables
int i, j; i = j = 0;
double Sum = 0.0;
// Check for singular matrix
for (i = 0; i < ParamCount; i++)
{
if (SquareRoot[i, i] == 0.0)
{
Console.Write(" ERROR: Singular matrix R in LSQ::Solve()");//
}
}
;
// Solve Rx=d for x_n,...,x_1 by backsubstitution
x[ParamCount - 1] = RightHandSide[ParamCount - 1] / SquareRoot[ParamCount - 1, ParamCount - 1];
for (i = ParamCount - 2; i >= 0; i--)
{
Sum = 0.0;
for (j = i + 1; j < ParamCount; j++)
{
Sum += SquareRoot[i, j] * x[j];
}
x[i] = (RightHandSide[i] - Sum) / SquareRoot[i, i];
}
;
}
/// <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);
}
/// <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 azimuth and elevation from local tangent coordinates
/// </summary>
/// <param name="localEnz"> s 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="Range"> Range [m]</param>
public static void LocalEnzToPolar(Geo.Algorithm.Vector localEnz, out double azimuthRad, out double elevationRad, out double range)
{
range = localEnz.Norm();
azimuthRad = Math.Atan2(localEnz[0], localEnz[1]);
azimuthRad = ((azimuthRad < 0.0) ? azimuthRad + OrbitConsts.TwoPI : azimuthRad);
elevationRad = Math.Atan(localEnz[2] / Math.Sqrt(localEnz[0] * localEnz[0] + localEnz[1] * localEnz[1]));
}
/// <summary>
/// 计算绕地航天器主要的加速度。
/// Computes the acceleration of an Earth orbiting satellite due to
/// - the Earth's harmonic gravity field,
/// - the gravitational perturbations of the Sun and Moon
/// - the solar radiation pressure and
/// - the atmospheric drag
/// </summary>
/// <param name="Mjd_TT"> Mjd_TT Terrestrial Time (Modified Julian Date)</param>
/// <param name="satXyzIcrf"> r 卫星在国际天球参考框架的位置 Satellite position vector in the ICRF/EME2000 system</param>
/// <param name="satVelocityIcrf"> v 卫星在国际天球参考框架的速度 Satellite velocity vector in the ICRF/EME2000 system</param>
/// <param name="Area"> Area Cross-section </param>
/// <param name="mass"> mass 质量 Spacecraft mass</param>
/// <param name="solarRadiPresCoeff"> coefOfRadiation 光压系数 Radiation pressure coefficient</param>
/// <param name="atmDragCoefficient"> coefOfDrag 大气阻力系数 Drag coefficient</param>
/// <returns> Acceleration (a=d^2r/dt^2) in the ICRF/EME2000 system</returns>
public static Geo.Algorithm.Vector AccelerOfMainForces(double Mjd_TT, Geo.Algorithm.Vector satXyzIcrf, Geo.Algorithm.Vector satVelocityIcrf, double Area, double mass, double solarRadiPresCoeff, double atmDragCoefficient)
{
double Mjd_UT1;
Geo.Algorithm.Vector a = new Geo.Algorithm.Vector(3), r_Sun = new Geo.Algorithm.Vector(3), r_Moon = new Geo.Algorithm.Vector(3);
Matrix T = new Matrix(3, 3), E = new Matrix(3, 3);
// Acceleration due to harmonic gravity field
Mjd_UT1 = Mjd_TT;
T = IERS.NutMatrix(Mjd_TT) * IERS.PrecessionMatrix(MJD_J2000, Mjd_TT);
E = IERS.GreenwichHourAngleMatrix(Mjd_UT1) * T;
a = AccelerOfHarmonicGraviFiled(satXyzIcrf, E, Grav.GM, Grav.R_ref, Grav.CS, Grav.n_max, Grav.m_max);
// Luni-solar perturbations
r_Sun = CelestialUtil.Sun(Mjd_TT);
r_Moon = CelestialUtil.Moon(Mjd_TT);
a += AccelerOfPointMass(satXyzIcrf, r_Sun, OrbitConsts.GM_Sun);
a += AccelerOfPointMass(satXyzIcrf, r_Moon, OrbitConsts.GM_Moon);
// Solar radiation pressure
a += CelestialUtil.Illumination(satXyzIcrf, r_Sun) * AccelerOfSolarRadiPressure(satXyzIcrf, r_Sun, Area, mass, solarRadiPresCoeff, OrbitConsts.PressureOfSolarRadiationPerAU, AU);
// Atmospheric drag
a += AccelerDragOfAtmos(Mjd_TT, satXyzIcrf, satVelocityIcrf, T, Area, mass, atmDragCoefficient);
// Acceleration
return(a);
}
/// <summary>
/// Ephemeris computation
/// </summary>
/// <param name="Y0"></param>
/// <param name="N_Step"></param>
/// <param name="Step"></param>
/// <param name="p"></param>
/// <param name="Eph"></param>
static void Ephemeris(Geo.Algorithm.Vector Y0, int N_Step, double Step, ForceModelOption p, Geo.Algorithm.Vector[] Eph)
{
int i;
double t, t_end;
double relerr, abserr; // Accuracy requirements
DeIntegrator Orb = new DeIntegrator(Deriv, 6, p); // Object for integrating the eq. of motion
Geo.Algorithm.Vector Y = new Geo.Algorithm.Vector(Y0);
relerr = 1.0e-13;
abserr = 1.0e-6;
t = 0.0;
// Y = Y0;
Orb.Init(t, relerr, abserr);
for (i = 0; i <= N_Step; i++)
{
t_end = Step * i;
var Yresult = Orb.Integ(t_end, Y);
Eph[i] = Yresult;
}
;
//var prevObj = Eph[0];
//Console.WriteLine(prevObj);
//var last = Eph[Eph.Length -1];
//Console.WriteLine(last);
}
//------------------------------------------------------------------------------
//
// 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));
}
//------------------------------------------------------------------------------
//
// TwoBody
//
// Purpose:
//
// Propagates a given state vector and computes the state transition matrix
// for elliptical Keplerian orbits
//
// Input/Output:
//
// GM Gravitational coefficient
// (gravitational constant * mass of central body)
// Y0 Epoch state vector (x,y,z,vx,vy,vz)_0
// dt Time since epoch
// Y State vector (x,y,z,vx,vy,vz)
// dYdY0 State transition matrix d(x,y,z,vx,vy,vz)/d(x,y,z,vx,vy,vz)_0
//
// Notes:
//
// The state vector, dt and GM must be given in consistent units,
// e.g. [m], [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].
//
// Due to the internal use of Keplerian elements, the function cannot be
// used with epoch state vectors describing a circular or non-inclined orbit.
//
//------------------------------------------------------------------------------
/// <summary>
/// 二体问题计算Propagates a given state vector and computes the state transition matrix
/// for elliptical Keplerian orbits
/// </summary>
/// <param name="GM">Gravitational coefficient (gravitational constant * mass of central body) </param>
/// <param name="Y0">Epoch state vector (x,y,z,vx,vy,vz)_0</param>
/// <param name="dt"> Time since epoch</param>
/// <param name="Y">State vector (x,y,z,vx,vy,vz)</param>
/// <param name="dYdY0"> State transition matrix d(x,y,z,vx,vy,vz)/d(x,y,z,vx,vy,vz)_0</param>
public static void TwoBody(double GM, Geo.Algorithm.Vector Y0, double dt,
ref Geo.Algorithm.Vector Y, ref Matrix dYdY0)
{
// Variables
int k;
double a, e, i, n, sqe2, naa;
double P_aM, P_eM, P_eo, P_io, P_iO;
Geo.Algorithm.Vector A0 = new Geo.Algorithm.Vector(6);
Matrix dY0dA0 = new Matrix(6, 6), dYdA0 = new Matrix(6, 6), dA0dY0 = new Matrix(6, 6);
// Orbital elements at epoch
A0 = Elements(GM, Y0);
a = A0[0]; e = A0[1]; i = A0[2];
n = Math.Sqrt(GM / (a * a * a));
// Propagated state
Y = State(GM, A0, dt);
// State vector partials w.r.t epoch elements
dY0dA0 = StatePartials(GM, A0, 0.0);
dYdA0 = StatePartials(GM, A0, dt);
// Poisson brackets
sqe2 = Math.Sqrt((1.0 - e) * (1.0 + e));
naa = n * a * a;
P_aM = -2.0 / (n * a); // P(a,M) = -P(M,a)
P_eM = -(1.0 - e) * (1.0 + e) / (naa * e); // P(e,M) = -P(M,e)
P_eo = +sqe2 / (naa * e); // P(e,omega) = -P(omega,e)
P_io = -1.0 / (naa * sqe2 * Math.Tan(i)); // P(i,omega) = -P(omega,i)
P_iO = +1.0 / (naa * sqe2 * Math.Sin(i)); // P(i,Omega) = -P(Omega,i)
// Partials of epoch elements w.r.t. epoch state
for (k = 0; k < 3; k++)
{
dA0dY0[0, k] = +P_aM * dY0dA0[k + 3, 5];
dA0dY0[0, k + 3] = -P_aM * dY0dA0[k, 5];
dA0dY0[1, k] = +P_eo * dY0dA0[k + 3, 4] + P_eM * dY0dA0[k + 3, 5];
dA0dY0[1, k + 3] = -P_eo * dY0dA0[k, 4] - P_eM * dY0dA0[k, 5];
dA0dY0[2, k] = +P_iO * dY0dA0[k + 3, 3] + P_io * dY0dA0[k + 3, 4];
dA0dY0[2, k + 3] = -P_iO * dY0dA0[k, 3] - P_io * dY0dA0[k, 4];
dA0dY0[3, k] = -P_iO * dY0dA0[k + 3, 2];
dA0dY0[3, k + 3] = +P_iO * dY0dA0[k, 2];
dA0dY0[4, k] = -P_eo * dY0dA0[k + 3, 1] - P_io * dY0dA0[k + 3, 2];
dA0dY0[4, k + 3] = +P_eo * dY0dA0[k, 1] + P_io * dY0dA0[k, 2];
dA0dY0[5, k] = -P_aM * dY0dA0[k + 3, 0] - P_eM * dY0dA0[k + 3, 1];
dA0dY0[5, k + 3] = +P_aM * dY0dA0[k, 0] + P_eM * dY0dA0[k, 1];
}
;
// State transition matrix
dYdY0 = dYdA0 * dA0dY0;
}
public void Integ(double tout, ref Geo.Algorithm.Vector y)
{
do
{
Integ_(ref t, tout, ref y);
if (State == DE_STATE.DE_INVPARAM)
{
var info = "ERROR: invalid parameters in public Integ";
throw new ArgumentException(info);
// << std::endl; exit(1);
}
if (State == DE_STATE.DE_BADACC)
{
var info = "WARNING: Accuracy requirement not achieved in public Integ";
Console.WriteLine(info);
}
if (State == DE_STATE.DE_STIFF)
{
var info = "WARNING: Stiff problem suspected in public Integ";
Console.WriteLine(info);
}
}while (State > DE_STATE.DE_DONE);
}
} // Square-root information matrix
// (Upper right triangular matrix)
/// <summary>
///Constructor LSQ class (implementation)
/// </summary>
/// <param name="n">Number of estimation parameters</param>
public LsqEstimater(int n)
{
ParamCount = (n);
// Allocate storage for R and d and initialize to zero
RightHandSide = new Geo.Algorithm.Vector(ParamCount);
SquareRoot = new Matrix(ParamCount, ParamCount);
}
Matrix P; // Covariance matrix
/// <summary>
/// Constructor, EKF class (implementation)
/// </summary>
/// <param name="n_"></param>
public ExtendedKalmanFilter(int n_)
{
n = (n_); // Number of estimation parameters
t = (0.0); // Epoch
// Allocate storage and initialize to zero
x = new Geo.Algorithm.Vector(n);
P = new Matrix(n, n);
}
//
// Time Update
//
public void TimeUpdate(double t_, // New epoch
Geo.Algorithm.Vector x_, // Propagetd state
Matrix Phi) // State transition matrix
{
t = t_; // Next time step
x = x_; // Propagated state
P = Phi * P * Phi.Transpose(); // Propagated covariance
}
//
// Interpolation
//
public Geo.Algorithm.Vector Intrp(
double tout, // Desired output point
ref Geo.Algorithm.Vector y // Solution vector
)
{
Interpolate(tout, ref y, ref ypout); // Interpolate and discard interpolated
return(ypout); // derivative ypout
}
/// <summary>
/// 计算由质点万有引力引起的扰动加速度。 Computes the perturbational acceleration due to a point mass
/// </summary>
/// <param name="satXyz"> r Satellite position vector </param>
/// <param name="massXyz"> s Point mass position vector</param>
/// <param name="GM"> GM Gravitational coefficient of point mass</param>
/// <returns> Acceleration (a=d^2r/dt^2)</returns>
public static Geo.Algorithm.Vector AccelerOfPointMass(Geo.Algorithm.Vector satXyz, Geo.Algorithm.Vector massXyz, double GM)
{
//Relative position vector of satellite w.r.t. point mass
Geo.Algorithm.Vector d = satXyz - massXyz;
// Acceleration
return((-GM) * (d / Math.Pow(d.Norm(), 3) + massXyz / Math.Pow(massXyz.Norm(), 3)));
}
static void Main0(string[] args)
{
// Ground station
const double lon_Sta = 11.0 * OrbitConsts.RadPerDeg; // [rad]
const double lat_Sta = 48.0 * OrbitConsts.RadPerDeg; // [rad]
const double alt_h = 0.0e3; // [m]
GeoCoord StaGeoCoord = new GeoCoord(lon_Sta, lat_Sta, alt_h); // Geodetic coordinates
// Spacecraft orbit
double Mjd_Epoch = DateUtil.DateToMjd(1997, 01, 01, 0, 0, 0); // Epoch
const double a = 960.0e3 + OrbitConsts.RadiusOfEarth; // Semimajor axis [m]
const double e = 0.0; // Eccentricity
const double i = 97.0 * OrbitConsts.RadPerDeg; // Inclination [rad]
const double Omega = 130.7 * OrbitConsts.RadPerDeg; // RA ascend. node [rad]
const double omega = 0.0 * OrbitConsts.RadPerDeg; // Argument of latitude [rad]
const double M0 = 0.0 * OrbitConsts.RadPerDeg; // Mean anomaly at epoch [rad]
Vector kepElements = new Geo.Algorithm.Vector(a, e, i, Omega, omega, M0); // Keplerian elements
// Variables
double Mjd_UTC, dt;
// Station
var R_Sta = StaGeoCoord.ToXyzVector(OrbitConsts.RadiusOfEarth, OrbitConsts.FlatteningOfEarth); // Geocentric position vector
Matrix E = StaGeoCoord.ToLocalNez_Matrix(); // Transformation to
// local tangent coordinates
// Header
var info = "Exercise 2-4: Topocentric satellite motion" + "\r\n"
+ " Date UTC Az El Dist" + "\r\n"
+ "yyyy/mm/dd hh:mm:ss.sss [deg] [deg] [km]";
Console.WriteLine(info);
// Orbit
for (int Minute = 6; Minute <= 24; Minute++)
{
Mjd_UTC = Mjd_Epoch + Minute / 1440.0; // Time
dt = (Mjd_UTC - Mjd_Epoch) * 86400.0; // Time since epoch [s]
Geo.Algorithm.Vector r = Kepler.State(OrbitConsts.GM_Earth, kepElements, dt).Slice(0, 2); // Inertial position vector
Matrix U = Matrix.RotateZ3D(IERS.GetGmstRad(Mjd_UTC)); // Earth rotation
var enz = E * (U * r - R_Sta); // Topocentric position vector
double Azim = 0, Elev = 0, Dist;
GeoCoord.LocalEnzToPolar(enz, out Azim, out Elev, out Dist); // Azimuth, Elevation
info = DateUtil.MjdToDateTimeString(Mjd_UTC) + " " + String.Format("{0, 9:F3}", (Azim * OrbitConsts.DegPerRad)) + " " + String.Format("{0, 9:F3}", (Elev * OrbitConsts.DegPerRad))
+ " " + String.Format("{0, 9:F3}", (Dist / 1000.0));
Console.WriteLine(info);
}
;
Console.ReadKey();
}
/// <summary>
/// Interpolation
/// </summary>
/// <param name="xout"></param>
/// <param name="yout"></param>
/// <param name="ypout"></param>
public void Interpolate(double xout, ref Geo.Algorithm.Vector yout, ref Geo.Algorithm.Vector ypout)
{
// Variables
int i, j, ki;
double eta, gamma, hi, psijm1;
double temp1, term;
double[] g = new double[14], rho = new double[14], w = new double[14];
g[1] = 1.0;
rho[1] = 1.0;
hi = xout - x;
ki = kold + 1;
// Initialize w[*] for computing g[*]
for (i = 1; i <= ki; i++)
{
temp1 = i;
w[i] = 1.0 / temp1;
}
// Compute g[*]
term = 0.0;
for (j = 2; j <= ki; j++)
{
psijm1 = psi[j - 1];
gamma = (hi + term) / psijm1;
eta = hi / psijm1;
for (i = 1; i <= ki + 1 - j; i++)
{
w[i] = gamma * w[i] - eta * w[i + 1];
}
g[j] = w[1];
rho[j] = gamma * rho[j - 1];
term = psijm1;
}
;
// Interpolate for the solution yout and for
// the derivative of the solution ypout
ypout = new Geo.Algorithm.Vector(yout.Dimension);
yout = new Geo.Algorithm.Vector(yout.Dimension);
for (j = 1; j <= ki; j++)
{
i = ki + 1 - j;
yout = yout + g[i] * phi.Col(i);
ypout = ypout + rho[i] * phi.Col(i);
}
;
yout = yy + hi * yout;
// return yy + hi * yout;
// Console.WriteLine(yout);
}
/// <summary>
/// 获取原始值
/// </summary>
/// <param name="index"></param>
/// <returns></returns>
protected Geo.Algorithm.Vector GetRawValue(int index)
{
Geo.Algorithm.Vector v = new Geo.Algorithm.Vector(Dimension);
for (int i = 0; i < Dimension; i++)
{
v[i] = Inputs[i][index];
}
return(v);
}
static void Main0(string[] args)
{
// Constants
const double GM = 1.0; // Gravitational coefficient
const double e = 0.1; // Eccentricity
const double t_end = 20.0; // End time
Geo.Algorithm.Vector Kep = new Geo.Algorithm.Vector(1.0, e, 0.0, 0.0, 0.0, 0.0); // (a,e,i,Omega,omega,M)
Geo.Algorithm.Vector y_ref = Kepler.State(GM, Kep, t_end); // Reference solution
int[] Steps = { 50, 100, 250, 500, 750, 1000, 1500, 2000 };
// Variables
// Function call count
int iCase;
double t, h; // Time and step aboutSize
Geo.Algorithm.Vector y = new Geo.Algorithm.Vector(6); // State vector
RungeKutta4StepIntegrator Orbit = new RungeKutta4StepIntegrator(f_Kep6D, 6, (Action)nCallsPlus); // Object for integrating the
// differential equation
// defined by f_Kep6D using the
// 4th-order Runge-Kutta method
// Header
var info = "Exercise 4-1: Runge-Kutta 4th-order integration" + "\r\n" + "\r\n"
+ " Problem D1 (e=0.1)" + "\r\n" + "\r\n"
+ " N_fnc Accuracy Digits " + "\r\n";
Console.WriteLine(info);
// Loop over test cases
for (iCase = 0; iCase < 8; iCase++)
{
// Step aboutSize
h = t_end / Steps[iCase];
// Initial values
t = 0.0;
y = new Geo.Algorithm.Vector(1.0 - e, 0.0, 0.0, 0.0, Math.Sqrt((1 + e) / (1 - e)), 0.0);
nCalls = 0;
// Integration from t=t to t=t_end
for (int i = 1; i <= Steps[iCase]; i++)
{
Orbit.Step(ref t, ref y, h);
}
// Output
info = String.Format("{0, 6:D}{1, 14:E4}{2, 9:F}", nCalls, (y - y_ref).Norm(), -Math.Log10((y - y_ref).Norm()));
Console.WriteLine(info);
}
;
Console.ReadKey();
}
} // Covariance matrix
public Geo.Algorithm.Vector StdDev()
{ // Standard deviation
Geo.Algorithm.Vector Sigma = new Geo.Algorithm.Vector(n);
for (int i = 0; i < n; i++)
{
Sigma[i] = Math.Sqrt(P[i, i]);
}
return(Sigma);
}
public void Init(double t_, Geo.Algorithm.Vector x_, Geo.Algorithm.Vector sigma)
{
t = t_; x = x_;
//P = new Matrix(n,n);
for (int i = 0; i < n; i++)
{
P[i, i] = sigma[i] * sigma[i];
}
}
/// <summary>
/// Update of filter parameters
/// </summary>
/// <param name="t_"></param>
/// <param name="x_"></param>
/// <param name="Phi"></param>
/// <param name="Qdt"></param>
public void TimeUpdate(double t_, // New epoch
Geo.Algorithm.Vector x_, // Propagetd state
Matrix Phi, // State transition matrix
Matrix Qdt) // Accumulated process noise
{
t = t_; // Next time step
x = x_; // Propagated state
P = Phi * P * Phi.Transpose() + Qdt; // Propagated covariance + noise
}
//------------------------------------------------------------------------------
//
// Accel
//
// Purpose:
//
// Computes the acceleration of an Earth orbiting satellite due to
// the Earth's harmonic gravity field up to degree and order 10
//
// Input/Output:
//
// Mjd_UT Modified Julian Date (Universal Time)
// r Satellite position vector in the true-of-date system
// n,m Gravity model degree and order
// <return> Acceleration (a=d^2r/dt^2) in the true-of-date system
//
//------------------------------------------------------------------------------
static Geo.Algorithm.Vector Accel(double Mjd_UT, Geo.Algorithm.Vector r, int n, int m)
{
// Earth rotation matrix
Matrix U = Matrix.RotateZ3D(IERS.GetGmstRad(Mjd_UT));
// Acceleration due to harmonic gravity field
return(Force.AccelerOfHarmonicGraviFiled(r, U, OrbitConsts.GM_Earth, Force.Grav.R_ref, Force.Grav.CS, n, m));
}
/// <summary>
/// 计算由太阳光压引起的加速度。 Computes the acceleration due to solar radiation pressure assuming
/// the spacecraft surface normal to the Sun direction
/// Notes: r, r_sun, Area, mass, P0 and AU must be given in consistent units,
/// e.g. m, m^2, kg and N/m^2.
/// </summary>
/// <param name="satXyz">r Spacecraft position vector </param>
/// <param name="sunXyz"> r_Sun Sun position vector </param>
/// <param name="Area">Area Cross-section </param>
/// <param name="mass"> mass Spacecraft mass</param>
/// <param name="solarRadiPressCoef"> coefOfRadiation Solar radiation pressure coefficient</param>
/// <param name="solarRadiPressurePerAU">P0 Solar radiation pressure at 1 AU </param>
/// <param name="AstroUnit">AU Length of one Astronomical Unit </param>
/// <returns>Acceleration (a=d^2r/dt^2)</returns>
public static Geo.Algorithm.Vector AccelerOfSolarRadiPressure(Geo.Algorithm.Vector satXyz, Geo.Algorithm.Vector sunXyz,
double Area, double mass, double solarRadiPressCoef,
double solarRadiPressurePerAU, double AstroUnit)
{
//Relative position vector of spacecraft w.r.t. Sun
Geo.Algorithm.Vector d = satXyz - sunXyz;
//Acceleration
return(solarRadiPressCoef * (Area / mass) * solarRadiPressurePerAU * (AstroUnit * AstroUnit) * d / Math.Pow(d.Norm(), 3));
}
// Degrees per radian
static void Main0(string[] args)
{
// Position and velocity
Geo.Algorithm.Vector r = new Geo.Algorithm.Vector(+10000.0e3, +40000.0e3, -5000.0e3); // [m]
Geo.Algorithm.Vector v = new Geo.Algorithm.Vector(-1.500e3, +1.000e3, -0.100e3); // [m/s]
// Variables
int i;
Geo.Algorithm.Vector y = new Geo.Algorithm.Vector(6), Kep = new Geo.Algorithm.Vector(6);
// Orbital elements
y = r.Stack(v);
Kep = Kepler.Elements(OrbitConsts.GM_Earth, y);
// Output
var info = "Exercise 2-3: Osculating elements";
Console.WriteLine(info);
info = "State vector:";
Console.WriteLine(info);
info = " Position ";
Console.Write(info);
for (i = 0; i < 3; i++)
{
Console.Write(r[i] / 1000.0 + ", ");
//Console.WriteLine(info);
}
;
info = " [km]";
Console.WriteLine(info);
info = " Velocity ";
Console.Write(info);
for (i = 0; i < 3; i++)
{
Console.Write(v[i] / 1000.0 + ", ");
}
Console.WriteLine(" [km/s]");
Console.WriteLine();
Console.WriteLine("Orbital elements:");
Console.WriteLine(" Semimajor axis " + String.Format("{0:f3}", (Kep[0] / 1000.0)) + " km");
Console.WriteLine(" Eccentricity " + String.Format("{0:f3}", Kep[1]));
Console.WriteLine(" Inclination " + String.Format("{0:f3}", Kep[2] * OrbitConsts.DegPerRad) + " deg");
Console.WriteLine(" RA ascend. node " + String.Format("{0:f3}", Kep[3] * OrbitConsts.DegPerRad) + " deg");
Console.WriteLine(" Arg. of perigee " + String.Format("{0:f3}", Kep[4] * OrbitConsts.DegPerRad) + " deg");
Console.WriteLine(" Mean anomaly " + String.Format("{0:f3}", Kep[5] * OrbitConsts.DegPerRad) + " deg");
Console.ReadKey();
}
/// <summary>
/// 计算模糊度，部分模糊度固定策略。
/// </summary>
/// <param name="floatSolution">按照Q从小到大的顺序排列后的带权向量。</param>
/// <param name="MaxRatio">最大比例</param>
/// <returns></returns>
public static WeightedVector GetAmbiguity(WeightedVector floatSolution, double MaxRatio = 3, double defaultRms = 1e-20)
{
//integer least square
#region RTKLIB lambda
var currentFloat = floatSolution;
int i = 0;
var lambda = new LambdaAmbiguitySearcher(currentFloat);
double[] N1 = new double[currentFloat.Count * 2]; for (i = 0; i < currentFloat.Count; i++)
{
N1[i] = Math.Round(currentFloat[i]);
}
double[] s = new double[2];
int info = 0;
info = lambda.getLambda(ref N1, ref s);
//if (info == -1 || s[0]<=0.0) return 0; //固定失败
double ratio = s[1] / s[0];
while (info == -1 || ratio <= MaxRatio) //检验没有通过
{
//去掉一个最大的方差值，继续固定。
var nextParamCount = currentFloat.Count - 1;
if (nextParamCount < 1)
{
return(new WeightedVector());
}
currentFloat = currentFloat.GetWeightedVector(0, nextParamCount);
//固定
N1 = new double[currentFloat.Count * 2]; for (i = 0; i < currentFloat.Count; i++)
{
N1[i] = Math.Round(currentFloat[i]);
}
s = new double[2];
lambda = new LambdaAmbiguitySearcher(currentFloat);
info = lambda.getLambda(ref N1, ref s);
if (s[0] == 0 && s[1] != 0) //
{
break;
}
ratio = s[1] / s[0];
}
if (info == -1)
{
return(new WeightedVector());
}
#endregion
var integers = ArrayUtil.GetSubArray <double>(N1, 0, currentFloat.Count);
Vector Vector = new Geo.Algorithm.Vector(integers, currentFloat.ParamNames);
return(new WeightedVector(Vector, defaultRms));
}
/// <summary>
/// Computes the second time derivative of the position vector for the
/// normalized (GM=1) Kepler's problem in three dimensions.
/// pAux is expected to point to an integer variable that will be incremented
/// by one on each call of f_Kep3D
///
/// </summary>
/// <param name="t"></param>
/// <param name="r"></param>
/// <param name="v"></param>
/// <param name="a"></param>
/// <param name="pAux"></param>
static void f_Kep3D(double t, Geo.Algorithm.Vector r, Geo.Algorithm.Vector v, ref Geo.Algorithm.Vector a, Action pAux)
{
// Pointer to auxiliary integer variable used as function call counter
//int* pCalls = static_cast<int*>(pAux);
// 2nd order derivative d^2(r)/dt^2 of the position vector
a = new Geo.Algorithm.Vector(-r / (Math.Pow(r.Norm(), 3)));
// Increment function call count
//(*pCalls)++;
pAux();
}
/// <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);
}
/// <summary>
/// Initialization of backwards differences from initial conditions
/// </summary>
/// <param name="t_0"></param>
/// <param name="r_0"></param>
/// <param name="v_0"></param>
/// <param name="h_"></param>
public void Init(double t_0, Geo.Algorithm.Vector r_0, Geo.Algorithm.Vector v_0, double h_)
{
// Order of method
const int m = 4;
// Coefficients gamma/delta of 1st/2nd order Moulton/Cowell corrector method
double[] gc = { +1.0, -1 / 2.0, -1 / 12.0, -1 / 24.0, -19 / 720.0 };
double[] dc = { +1.0, -1.0, +1 / 12.0, 0.0, -1 / 240.0, -1 / 240.0 };
int i, j;
double t = t_0;
Geo.Algorithm.Vector r = r_0;
Geo.Algorithm.Vector v = v_0;
// Save step aboutSize
this.StepSize = h_;
// Create table of accelerations at past times t-3h, t-2h, and t-h using
// RK4 steps
DevFunc(t, r, v, ref D[0], pAux); // D[i]=a(t-ih)
for (i = 1; i <= m - 1; i++)
{
RK4(ref t, ref r, ref v, -StepSize); DevFunc(t, r, v, ref D[i], pAux);
}
;
// Compute backwards differences
for (i = 1; i <= m - 1; i++)
{
for (j = m - 1; j >= i; j--)
{
D[j] = D[j - 1] - D[j];
}
}
// Initialize backwards sums using 4th order GJ corrector
S1 = v_0 / StepSize; for (i = 1; i <= m; i++)
{
S1 -= gc[i] * D[i - 1];
}
S2 = r_0 / (StepSize * StepSize) - dc[1] * S1; for (i = 2; i <= m + 1; i++)
{
S2 -= dc[i] * D[i - 2];
}
}
/// <summary>
/// Integration step
/// </summary>
/// <param name="t">Value of the independent variable; updated by t+h</param>
/// <param name="y">Value of y(t); updated by y(t+h)</param>
/// <param name="h">Step aboutSize</param>
public void Step(ref double t, ref Geo.Algorithm.Vector y, double h)
{
// Elementary RK4 step
k_1 = DevFunc(t, y, pAux);
k_2 = DevFunc(t + h / 2.0, (y + (h / 2.0) * k_1), pAux);
k_3 = DevFunc(t + h / 2.0, (y + (h / 2.0) * k_2), pAux);
k_4 = DevFunc(t + h, (y + h * k_3), pAux);
y = y + (h / 6.0) * (k_1 + 2.0 * k_2 + 2.0 * k_3 + k_4);
// Update independent variable
t = t + h;
}
/// <summary>
/// Dyadic product。两个n,m维矩阵向量，点乘为 n × m 矩阵。
/// </summary>
/// <param name="left"></param>
/// <param name="right"></param>
/// <returns></returns>
public static Matrix Dyadic(Geo.Algorithm.Vector left, Geo.Algorithm.Vector right)
{
Matrix Mat = new Matrix(left.Dimension, right.Dimension);
for (int i = 0; i < left.Dimension; i++)
{
for (int j = 0; j < right.Dimension; j++)
{
Mat.Data[i][j] = left.Data[i] * right.Data[j];
}
}
return(Mat);
}
