/// <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);
}
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();
}
//------------------------------------------------------------------------------
//
// 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));
}
//------------------------------------------------------------------------------
//
// Gradient
//
// Purpose:
//
// Computes the gradient of the Earth's harmonic gravity field
//
// 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> Gradient (G=da/dr) in the true-of-date system
//
//------------------------------------------------------------------------------
static Matrix Gradient(double Mjd_UT, Geo.Algorithm.Vector r, int n, int m)
{
// Constants
const double d = 1.0; // Position increment [m]
// Variables
int i;
Geo.Algorithm.Vector a = new Geo.Algorithm.Vector(3), da = new Geo.Algorithm.Vector(3), dr = new Geo.Algorithm.Vector(3);
Matrix U = new Matrix(3, 3), G = new Matrix(3, 3);
// Earth rotation matrix
U = Matrix.RotateZ3D(IERS.GetGmstRad(Mjd_UT));
// Acceleration
a = Force.AccelerOfHarmonicGraviFiled(r, U, OrbitConsts.GM_Earth, Force.Grav.R_ref, Force.Grav.CS, n, m);
// Gradient
for (i = 0; i <= 2; i++)
{
// Set offset in i-th component of the position vector
//dr = 0.0;
dr = new Geo.Algorithm.Vector(3);
dr[i] = d;
// Acceleration difference
da = Force.AccelerOfHarmonicGraviFiled(r + dr / 2, U, OrbitConsts.GM_Earth, Force.Grav.R_ref, Force.Grav.CS, n, m) -
Force.AccelerOfHarmonicGraviFiled(r - dr / 2, U, OrbitConsts.GM_Earth, Force.Grav.R_ref, Force.Grav.CS, n, m);
// da = AccelHarmonic ( r+dr,U, SatConst.GM_Earth, Grav.R_ref,Grav.CS, n,m ) - a;
// Derivative with respect to i-th axis
G.SetCol(i, da / d);
}
return(G);
}
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 Sta = new GeoCoord(lon_Sta, lat_Sta, alt_h); // Geodetic coordinates
// Fixed media satData at ground site
const double T0 = 273.2; // Temperature at 0 deg C [K]
const double pa = 1024.0; // Partial pressure of dry air [mb]
const double fh = 0.7; // Relative humidity
// Spacecraft orbit
double Mjd_Epoch = DateUtil.DateToMjd(1997, 01, 01); // Epoch
const double a = 42164.0e3; // Semimajor axis [m]
const double e = 0.000296; // Eccentricity
const double i = 0.05 * OrbitConsts.RadPerDeg; // Inclination [rad]
const double Omega = 150.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 Kep = new Vector(a, e, i, Omega, omega, M0); // Keplerian elements
// Variables
int Hour;
double Mjd_UTC, dt;
double Azim = 0, Elev = 0, Elev0 = 0, Dist;
double Ns, eh, T, TC;
Vector dElev = new Vector(2);
Vector R_Sta = new Vector(3);
Vector r = new Vector(3), s = new Vector(3);
Matrix U = new Matrix(3, 3), E = new Matrix(3, 3);
double[] Tv = { 303.0, 283.0 }; // Temperature [K]
// Station
R_Sta = Sta.ToXyzVector(OrbitConsts.RadiusOfEarth, OrbitConsts.FlatteningOfEarth); // Geocentric position vector
E = Sta.ToLocalNez_Matrix(); // Transformation to
// local tangent coordinates
// Header
var endl = "\r\n";
var info = "Exercise 6-4: Tropospheric Refraction" + endl + endl;
Console.Write(info);
// Orbit
for (Hour = 0; Hour <= 8; Hour++)
{
Mjd_UTC = Mjd_Epoch + 3.0 * Hour / 24.0; // Modified Julian Date [UTC]
dt = (Mjd_UTC - Mjd_Epoch) * 86400.0; // Time since epoch [s]
r = Kepler.State(OrbitConsts.GM_Earth, Kep, dt).Slice(0, 2); // Inertial position vector
U = Matrix.RotateZ3D(IERS.GetGmstRad(Mjd_UTC)); // Earth rotation
s = E * (U * r - R_Sta); // Topocentric position vector
GeoCoord.LocalEnzToPolar(s, out Azim, out Elev, out Dist); // Azimuth, Elevation
if (Hour == 0)
{
Elev0 = Elev; // Store initial elevation
info = "E0 [deg] " + String.Format("{0, 10:F3}", Elev0 * OrbitConsts.DegPerRad) + endl + endl;
Console.Write(info);
info = " Date UTC E-E0 dE_t1 dE_t2 " + endl
+ "yyyy/mm/dd hh:mm:ss.sss [deg] [deg] [deg]" + endl;
Console.Write(info);
}
;
for (int Ti = 0; Ti <= 1; Ti++)
{ // Evaluate at 2 temperatures
T = Tv[Ti]; // Map to scalar
TC = T - T0; // Temperature [C]
eh = 6.10 * fh * Math.Exp(17.15 * TC / (234.7 + TC)); // Partial water pressure
Ns = 77.64 * pa / T + 3.734e5 * eh / (T * T); // Refractivity
dElev[Ti] = Ns * 1.0e-6 / Math.Tan(Elev); // Tropospheric refraction
}
;
info = DateUtil.MjdToDateTimeString(Mjd_UTC)
+ String.Format("{0, 10:F3}", (Elev - Elev0) * OrbitConsts.DegPerRad)
+ String.Format("{0, 10:F3}", dElev[0] * OrbitConsts.DegPerRad) + String.Format("{0, 10:F3}", dElev[1] * OrbitConsts.DegPerRad) + endl;
Console.Write(info);
}
;
Console.ReadKey();
}
static void Main0(string[] args)
{
// Ground station
const int N_obs = 6;
const double Step = 1200.0;
Vector Null3D = new Vector(0.0, 0.0, 0.0);
const double sigma_range = 10.0; // [m]
const double sigma_angle = 0.01 * OrbitConsts.RadPerDeg; // [rad] (=36")
string[] Label = { "x [m] ", "y [m] ", "z [m] ",
"vx [m/s]", "vy [m/s]", "vz [m/s]" };
// Variables
int i, iterat;
double Mjd0, t, MjdUTC, Theta;
double Azim = 0, Elev = 0, Dist = 0;
Vector Y0_ref = new Vector(6), Y0_apr = new Vector(6), Y0 = new Vector(6), Y = new Vector(6), r = new Vector(3), R = new Vector(3), s = new Vector(3);
Vector dAds = new Vector(3), dEds = new Vector(3), dDds = new Vector(3);
Vector dAdY0 = new Vector(6), dEdY0 = new Vector(6), dDdY0 = new Vector(6);
Matrix dYdY0 = new Matrix(6, 6), U = new Matrix(3, 3), E = new Matrix(3, 3);
LsqEstimater OrbEst = new LsqEstimater(6);
Vector dY0 = new Vector(6), SigY0 = new Vector(6);
ObsType[] Obs = new ObsType[N_obs];
// Ground station
R = new Vector(+1344.0e3, +6069.0e3, 1429.0e3); // [m]
E = CoordTransformer.XyzToGeoCoord(new XYZ(R.OneDimArray)).ToLocalNez_Matrix();
// Header
var endl = "\r\n";
var info = "Exercise 8-2: Least-squares orbit determination" + endl + endl;
Console.Write(info);
// Generation of artificial observations from given epoch state
Mjd0 = DateUtil.DateToMjd(1995, 03, 30, 00, 00, 00.0); // Epoch (UTC)
Y0_ref = new Vector(-6345.000e3, -3723.000e3, -580.000e3, // [m]
+2.169000e3, -9.266000e3, -1.079000e3); // [m/s]
Y0 = Y0_ref;
info = "Measurements" + endl + endl
+ " Date UTC Az[deg] El[deg] Range[km]" + endl;
Console.Write(info);
for (i = 0; i < N_obs; i++)
{
// Time increment and propagation
t = (i + 1) * Step; // Time since epoch [s]
MjdUTC = Mjd0 + t / 86400.0; // Modified Julian Date
Kepler.TwoBody(OrbitConsts.GM_Earth, Y0_ref, t, ref Y, ref dYdY0); // State vector
// Topocentric coordinates
Theta = IERS.GetGmstRad(MjdUTC); // Earth rotation
U = Matrix.RotateZ3D(Theta);
r = Y.Slice(0, 2);
s = E * (U * r - R); // Topocentric position [m]
GeoCoord.LocalEnzToPolar(s, out Azim, out Elev, out Dist); // Azimuth, Elevation, Range
// Observation record
Obs[i].Mjd_UTC = MjdUTC;
Obs[i].Azim = Azim;
Obs[i].Elev = Elev;
Obs[i].Dist = Dist;
// Output
info = " " + DateUtil.MjdToDateTimeString(MjdUTC) + String.Format("{0, 10:F3}{1, 10:F3}{2, 12:F3}", +OrbitConsts.DegPerRad * Azim, OrbitConsts.DegPerRad * Elev, Dist / 1000.0) + endl;
Console.Write(info);
}
;
Console.WriteLine();
//
// Orbit determination
//
Mjd0 = DateUtil.DateToMjd(1995, 03, 30, 00, 00, 00.0); // Epoch (UTC)
Y0_apr = Y0_ref + new Vector(+10.0e3, -5.0e3, +1.0e3, -1.0, +3.0, -0.5);
Y0 = Y0_apr;
// Iteration
for (iterat = 1; iterat <= 3; iterat++)
{
OrbEst.Init();
info = "Iteration Nr. " + iterat + endl + endl
+ " Residuals:" + endl + endl
+ " Date UTC Az[deg] El[deg] Range[m]" + endl;
Console.Write(info);
for (i = 0; i < N_obs; i++)
{
// Time increment and propagation
MjdUTC = Obs[i].Mjd_UTC; // Modified Julian Date
t = (MjdUTC - Mjd0) * 86400.0; // Time since epoch [s]
Kepler.TwoBody(OrbitConsts.GM_Earth, Y0, t, ref Y, ref dYdY0); // State vector
// Topocentric coordinates
Theta = IERS.GetGmstRad(MjdUTC); // Earth rotation
U = Matrix.RotateZ3D(Theta);
r = Y.Slice(0, 2);
s = E * (U * r - R); // Topocentric position [m]
// Observations and partials
GeoCoord.LocalEnzToPolar(s, out Azim, out Elev, out dAds, out dEds); // Azimuth, Elevation
Dist = s.Norm(); dDds = s / Dist; // Range
dAdY0 = (dAds * E * U).Stack(Null3D) * dYdY0;
dEdY0 = (dEds * E * U).Stack(Null3D) * dYdY0;
dDdY0 = (dDds * E * U).Stack(Null3D) * dYdY0;
// Accumulate least-squares system
OrbEst.Accumulate(dAdY0, (Obs[i].Azim - Azim), sigma_angle / Math.Cos(Elev));
OrbEst.Accumulate(dEdY0, (Obs[i].Elev - Elev), sigma_angle);
OrbEst.Accumulate(dDdY0, (Obs[i].Dist - Dist), sigma_range);
// Output
info = " " + DateUtil.MjdToDateTimeString(MjdUTC) + String.Format("{0, 10:F3}{1, 10:F3}{2, 10:F3}", OrbitConsts.DegPerRad * (Obs[i].Azim - Azim), OrbitConsts.DegPerRad * (Obs[i].Elev - Elev)
, Obs[i].Dist - Dist) + endl;
Console.Write(info);
}
;
// Solve least-squares system
OrbEst.Solve(dY0);
SigY0 = OrbEst.StdDev();
info = endl + " Correction:" + endl + endl
+ " Pos" + dY0.Slice(0, 2)
+ " m " + endl
+ " Vel" + dY0.Slice(3, 5)
+ " m/s" + endl + endl;
Console.Write(info);
// Correct epoch state
Y0 = Y0 + dY0;
}
;
// Summary
info = "Summary:" + endl
+ " a priori correction final sigma"
+ endl;
Console.Write(info);
for (i = 0; i < 6; i++)
{
info = " " + String.Format("{0, 10:S}", Label[i])
+ String.Format("{0, 12:F3}{1, 11:F3}{2, 14:F3}{3, 11:F3}", Y0_apr[i], Y0[i] - Y0_apr[i], Y0[i], SigY0[i])
+ endl;
Console.Write(info);
}
Console.ReadKey();
}
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 Sta = new GeoCoord(lon_Sta, lat_Sta, alt_h); // Geodetic coordinates
// Spacecraft orbit
double Mjd_Epoch = DateUtil.DateToMjd(1997, 01, 01); // 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 Kep = new Vector(a, e, i, Omega, omega, M0); // Keplerian elements
// Light time iteration
const int I_max = 2; // Maxim. number of iterations
// Variables
int Iteration, Step; // Loop counters
double Mjd_UTC, t; // Time
double rho, range; // Range 1-way/2-way
double tau_up, tau_down; // Upleg/downleg light time
Vector R_Sta = new Vector(3); // Earth-fixed station position
Vector r_Sta = new Vector(3); // Inertial station position
Vector r = new Vector(3); // Inertial satellite position
Vector rho_up = new Vector(I_max + 1), rho_down = new Vector(I_max + 1); // Upleg/downleg range
Matrix U = new Matrix(3, 3); // Earth rotation matrix
// Station
R_Sta = Sta.ToXyzVector(OrbitConsts.RadiusOfEarth, OrbitConsts.FlatteningOfEarth); // Geocentric position vector
// Header
var endl = "\r\n";
var info = "Exercise 6-1: Light time iteration" + endl + endl
+ " Date UTC Distance " +
"Down It 1 It 2 Up It 1 Range" + endl
+ "yyyy/mm/dd hh:mm:ss.sss [m] " +
" [m] [mm] [m] [m] " + endl;
Console.WriteLine(info);
// Orbit
for (Step = 0; Step <= 6; Step++)
{
// Ground-received time
t = 360.0 + 180.0 * Step; // Time since epoch [s]
Mjd_UTC = Mjd_Epoch + t / 86400.0; // Modified Julian Date [UTC]
U = Matrix.RotateZ3D(IERS.GetGmstRad(Mjd_UTC)); // Earth rotation matrix
r_Sta = U.Transpose() * R_Sta; // Inertial station position
// Light time iteration for downleg satellite -> station
tau_down = 0.0;
for (Iteration = 0; Iteration <= I_max; Iteration++)
{
r = Kepler.State(OrbitConsts.GM_Earth, Kep, t - tau_down).Slice(0, 2); // Spacecraft position
rho = (r - r_Sta).Norm(); // Downleg range
tau_down = rho / OrbitConsts.SpeedOfLight; // Downleg light time
rho_down[Iteration] = rho;
}
;
// Light time iteration for upleg station -> satellite
tau_up = 0.0;
for (Iteration = 0; Iteration <= I_max; Iteration++)
{
U = Matrix.RotateZ3D(IERS.GetGmstRad(Mjd_UTC - (tau_down + tau_up) / 86400.0));
r_Sta = U.Transpose() * R_Sta; // Inertial station pos.
rho = (r - r_Sta).Norm(); // at ground transmit time
tau_up = rho / OrbitConsts.SpeedOfLight; // Upleg light time
rho_up[Iteration] = rho;
}
;
// Two-way range
range = 0.5 * (rho_down[I_max] + rho_up[I_max]);
info = DateUtil.MjdToDateTimeString(Mjd_UTC)
+ String.Format("{0,15:F3}{1,9:F3}{2,9:F3}{3,8:F3}{4,12:F3}", rho_down[0], rho_down[1] - rho_down[0], (rho_down[2] - rho_down[1]) * 1000.0, rho_up[1] - rho_up[0], range);
Console.WriteLine(info);
}
Console.ReadKey();
}
/// <summary>
/// 构造函数。
/// </summary>
/// <param name="option"></param>
/// <param name="IERS"></param>
public AccelerationCalculator(ForceModelOption option, IERS IERS)
{
this.IERS = IERS;
this.Option = option;
}
static void Main0(string[] args)
{
// Variables
int i; // Loop counter
double MJD_GPS, MJD_TT; // Modified Julian Date (GPS,TT)
double MJD_UTC, MJD_UT1; // Modified Julian Date (UTC,UT1)
Matrix P = new Matrix(3, 3), N = new Matrix(3, 3); // Precession/nutation matrix
Matrix Theta = new Matrix(3, 3); // Sidereal Time matrix
Matrix S = new Matrix(3, 3), dTheta = new Matrix(3, 3); // and derivative
Matrix Pi = new Matrix(3, 3); // Polar motion matrix
Matrix U = new Matrix(3, 3), dU = new Matrix(3, 3); // ICRS to ITRS transformation and derivative
Vector r_WGS = new Vector(3), v_WGS = new Vector(3); // Position/velocity in the Earth-fixed frame
Vector r = new Vector(3), v = new Vector(3); // Position/velocity in the ICRS
Vector y = new Vector(6), Kep = new Vector(6); // Satte vector and Keplerian elements
// Header
var endl = "\r\n";
var info = "Exercise 5-2: Velocity in the Earth-fixed frame"
+ endl + endl;
Console.WriteLine(info);
// Earth Orientation Parameters (UT1-UTC[s],UTC-TAI[s], x["], y["])
// (from IERS Bulletin B #135 and C #16; valid for 1999/03/04 0:00 UTC)
IERS IERS = new IERS(0.6492332, -32.0, 0.06740, 0.24173);
// Date
MJD_GPS = DateUtil.DateToMjd(1999, 03, 04, 0, 0, 0.0);
MJD_UTC = MJD_GPS - IERS.GetGPS_UTC(MJD_GPS) / 86400.0;
MJD_UT1 = MJD_UTC + IERS.GetUT1_UTC(MJD_UTC) / 86400.0;
MJD_TT = MJD_UTC + IERS.GetTT_UTC(MJD_UTC) / 86400.0;
// Earth-fixed state vector of GPS satellite #PRN15
// (from NIMA ephemeris nim09994.eph; WGS84(G873) system)
r_WGS = new Vector(19440.953805e+3, 16881.609273e+3, -6777.115092e+3); // [m]
v_WGS = new Vector(-8111.827456e-1, -2573.799137e-1, -30689.508125e-1); // [m/s]
// ICRS to ITRS transformation matrix and derivative
P = IERS.PrecessionMatrix(OrbitConsts.MJD_J2000, MJD_TT); // IAU 1976 Precession
N = IERS.NutMatrix(MJD_TT); // IAU 1980 Nutation
Theta = IERS.GreenwichHourAngleMatrix(MJD_UT1); // Earth rotation
Pi = IERS.PoleMatrix(MJD_UTC); // Polar motion
S[0, 1] = 1.0; S[1, 0] = -1.0; // Derivative of Earth rotation
dTheta = OrbitConsts.RotationSpeedOfEarth_Rad * S * Theta; // matrix [1/s]
U = Pi * Theta * N * P; // ICRS to ITRS transformation
dU = Pi * dTheta * N * P; // Derivative [1/s]
// Transformation from WGS to ICRS
r = U.Transpose() * r_WGS;
v = U.Transpose() * v_WGS + dU.Transpose() * r_WGS;
// Orbital elements
y = r.Stack(v);
Kep = Kepler.Elements(OrbitConsts.GM_Earth, y);
// Output
info = "Date" + endl + endl
+ " " + DateUtil.MjdToDateTimeString(MJD_GPS) + " GPS" + endl
+ " " + DateUtil.MjdToDateTimeString(MJD_UTC) + " UTC" + endl
+ " " + DateUtil.MjdToDateTimeString(MJD_UT1) + " UT1" + endl
+ " " + DateUtil.MjdToDateTimeString(MJD_TT) + " TT " + endl + endl;
Console.WriteLine(info);
info = "WGS84 (G873) State vector:" + endl + endl;
info += " Position ";
for (i = 0; i < 3; i++)
{
info += String.Format("{0, 10:F6}", r_WGS[i] / 1000.0);
}
;
info += " [km]";
Console.WriteLine(info);
info = " Velocity ";
for (i = 0; i < 3; i++)
{
info += String.Format("{0, 10:F6}", v_WGS[i] / 1000.0);
}
;
info += " [km/s]" + endl + endl;
Console.WriteLine(info);
info = "ICRS-ITRS transformation" + endl + endl
+ String.Format("{0, 10:F6}", U) + endl;
info += "Derivative of ICRS-ITRS transformation [10^(-4)/s]" + endl + endl
+ String.Format("{0, 10:F6}", dU * 1.0e4) + endl;
info += "ICRS State vector:" + endl;
Console.WriteLine(info);
info = " Position ";
for (i = 0; i < 3; i++)
{
info += String.Format("{0, 14:F6}", r[i] / 1000.0);
}
;
info += " [km]";
Console.WriteLine(info);
info = " Velocity ";
for (i = 0; i < 3; i++)
{
info += String.Format("{0, 14:F6}", v[i] / 1000.0);
}
;
info += " [km/s]" + endl + endl;
Console.WriteLine(info);
info = "Orbital elements:" + endl + endl
+ " Semimajor axis " + String.Format("{0, 10:F3}", Kep[0] / 1000.0) + " km" + endl
+ " Eccentricity " + String.Format("{0, 10:F7}", Kep[1]) + endl
+ " Inclination " + String.Format("{0, 10:F3}", Kep[2] * OrbitConsts.DegPerRad) + " deg" + endl
+ " RA ascend. node " + String.Format("{0, 10:F3}", Kep[3] * OrbitConsts.DegPerRad) + " deg" + endl
+ " Arg. of perigee " + String.Format("{0, 10:F3}", Kep[4] * OrbitConsts.DegPerRad) + " deg" + endl
+ " Mean anomaly " + String.Format("{0, 10:F3}", Kep[5] * OrbitConsts.DegPerRad) + " deg" + endl;
Console.WriteLine(info);
Console.ReadKey();
}
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 Sta = new GeoCoord(lon_Sta, lat_Sta, alt_h); // Geodetic coordinates
// Earth rotation
Vector omega_vec = new Vector(0.0, 0.0, OrbitConsts.RotationSpeedOfEarth_Rad); // Earth rotation vector
// Spacecraft orbit
double Mjd_Epoch = DateUtil.DateToMjd(1997, 01, 01); // 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 Kep = new Vector(a, e, i, Omega, omega, M0); // Keplerian elements
// Radar Modelling
const int I_max = 3; // Maximum light time iterations
const double Count = 1.0; // Doppler count time [s]
// Variables
int Iteration, Step; // Loop counters
double Mjd_UTC, t; // Time
double rho; // Range 1-way
double range1, range0; // Range 2-way at end, begin of count
double range_rate; // Range rate
double Doppler; // Instantaneous Doppler
double tau_up, tau_down; // Upleg/downleg light time
double rho_up = 0, rho_down = 0; // Upleg/downleg range
Vector R_Sta = new Vector(3); // Earth-fixed station position
Vector r_Sta = new Vector(3); // Inertial station position
Vector r = new Vector(3); // Inertial satellite position
Vector x = new Vector(3), v = new Vector(3); // Earth-fixed satellite position, velocity
Vector u = new Vector(3); // Unit vector satellite station
Matrix U = new Matrix(3, 3); // Earth rotation matrix
// Station
R_Sta = Sta.ToXyzVector(OrbitConsts.RadiusOfEarth, OrbitConsts.FlatteningOfEarth); // Geocentric position vector
// Header
var endl = "\r\n";
var info = "Exercise 6-2: Range Rate Modelling" + endl + endl
+ " Date UTC Range Rate Doppler Difference" + endl
+ "yyyy/mm/dd hh:mm:ss.sss [m/s] [m/s] [m/s] " + endl;
Console.WriteLine(info);
// Orbit
for (Step = 0; Step <= 6; Step++)
{
// Ground-received time
t = 360.0 + 180.0 * Step; // Time since epoch [s]
Mjd_UTC = Mjd_Epoch + t / 86400.0; // Modified Julian Date [UTC]
U = Matrix.RotateZ3D(IERS.GetGmstRad(Mjd_UTC)); // Earth rotation matrix
r_Sta = U.Transpose() * R_Sta; // Inertial station position
// Light time iteration at count interval end for downleg satellite -> station
tau_down = 0.0;
for (Iteration = 0; Iteration <= I_max; Iteration++)
{
r = Kepler.State(OrbitConsts.GM_Earth, Kep, t - tau_down).Slice(0, 2); // Spacecraft position
rho = (r - r_Sta).Norm(); // Downleg range
tau_down = rho / OrbitConsts.SpeedOfLight; // Downleg light time
rho_down = rho;
}
;
// Light time iteration at count interval end for upleg station -> satellite
tau_up = 0.0;
for (Iteration = 0; Iteration <= I_max; Iteration++)
{
U = Matrix.RotateZ3D(IERS.GetGmstRad(Mjd_UTC - (tau_down + tau_up) / 86400.0));
r_Sta = U.Transpose() * R_Sta; // Inertial station pos.
rho = (r - r_Sta).Norm(); // at ground transmit time
tau_up = rho / OrbitConsts.SpeedOfLight; // Upleg light time
rho_up = rho;
}
;
// Two-way range at end of count interval
range1 = 0.5 * (rho_down + rho_up);
// Station position at begin of count interval
U = Matrix.RotateZ3D(IERS.GetGmstRad(Mjd_UTC - Count / 86400.0)); // Earth rotation matrix
r_Sta = U.Transpose() * R_Sta; // Inertial station position
// Light time iteration at count interval begin for downleg satellite -> station
tau_down = 0.0;
for (Iteration = 0; Iteration <= I_max; Iteration++)
{
r = Kepler.State(OrbitConsts.GM_Earth, Kep, t - tau_down - Count).Slice(0, 2); // Spacecraft position
rho = (r - r_Sta).Norm(); // Downleg range
tau_down = rho / OrbitConsts.SpeedOfLight; // Downleg light time
rho_down = rho;
}
;
// Light time iteration at count interval begin for upleg station -> satellite
tau_up = 0.0;
for (Iteration = 0; Iteration <= I_max; Iteration++)
{
U = Matrix.RotateZ3D(IERS.GetGmstRad(Mjd_UTC - (tau_down + tau_up + Count) / 86400.0));
r_Sta = U.Transpose() * R_Sta; // Inertial station pos.
rho = (r - r_Sta).Norm(); // at ground transmit time
tau_up = rho / OrbitConsts.SpeedOfLight; // Upleg light time
rho_up = rho;
}
;
// Two-way range at begin of count interval
range0 = 0.5 * (rho_down + rho_up);
// Two-way average range rate
range_rate = (range1 - range0) / Count;
// Instantaneous Doppler modelling at mid of count interval
U = Matrix.RotateZ3D(IERS.GetGmstRad(Mjd_UTC - (Count / 2.0) / 86400)); // Earth rotation matrix
x = U * Kepler.State(OrbitConsts.GM_Earth, Kep, t - (Count / 2.0)).Slice(0, 2); // Spacecraft position
v = U * Kepler.State(OrbitConsts.GM_Earth, Kep, t - (Count / 2.0)).Slice(3, 5) // Spacecraft velocity
- omega_vec.Cross3D(x);
u = (x - R_Sta) / (x - R_Sta).Norm(); // Unit vector s/c-station
Doppler = v.Dot(u); // Instantaneous Doppler
// Output
info = DateUtil.MjdToDateTimeString(Mjd_UTC) + String.Format("{0, 12:F3}{1, 12:F3}{2, 12:F3}", range_rate, Doppler, range_rate - Doppler);
Console.WriteLine(info);
}
Console.ReadKey();
}
static void Main0(string[] args)
{
// Constants
const int N_Step = 720; // Maximum number of steps
// Variables
int i;
int N_Step1;
int N_Step2;
double Step;
double Mjd0_UTC;
Geo.Algorithm.Vector Y0 = new Geo.Algorithm.Vector(6);
Geo.Algorithm.Vector Kep = new Geo.Algorithm.Vector(6);
ForceModelOption Aux_ref = new ForceModelOption(), Aux = new ForceModelOption(); // Auxiliary parameters
double Max_J20, Max_J22, Max_J44, Max_J1010;
double Max_Sun, Max_Moon, Max_SRad, Max_Drag;
Geo.Algorithm.Vector[] Eph_Ref = new Geo.Algorithm.Vector[N_Step + 1];
Geo.Algorithm.Vector[] Eph_J20 = new Geo.Algorithm.Vector[N_Step + 1];
Geo.Algorithm.Vector[] Eph_J22 = new Geo.Algorithm.Vector[N_Step + 1];
Geo.Algorithm.Vector[] Eph_J44 = new Geo.Algorithm.Vector[N_Step + 1];
Geo.Algorithm.Vector[] Eph_J1010 = new Geo.Algorithm.Vector[N_Step + 1];
Geo.Algorithm.Vector[] Eph_Sun = new Geo.Algorithm.Vector[N_Step + 1];
Geo.Algorithm.Vector[] Eph_Moon = new Geo.Algorithm.Vector[N_Step + 1];
Geo.Algorithm.Vector[] Eph_SRad = new Geo.Algorithm.Vector[N_Step + 1];
Geo.Algorithm.Vector[] Eph_Drag = new Geo.Algorithm.Vector[N_Step + 1];
// Initialize UT1-UTC and UTC-TAI time difference
IERS = new IERS(-0.05, -30.00, 0.0, 0.0);
// Epoch state (remote sensing satellite)
Mjd0_UTC = DateUtil.DateToMjd(1999, 03, 01, 00, 00, 0.0);
Kep = new Vector(7178.0e3, 0.0010, 98.57 * OrbitConsts.RadPerDeg, 0.0, 0.0, 0.0);
Y0 = Kepler.State(OrbitConsts.GM_Earth, Kep, 0.0);
// Model parameters
Aux_ref.Mjd0_TT = Mjd0_UTC - IERS.GetTT_UTC(Mjd0_UTC) / 86400.0;
Aux_ref.Area = 5.0; // [m^2] Remote sensing satellite
Aux_ref.Mass = 1000.0; // [kg]
Aux_ref.CoefOfRadiation = 1.3;
Aux_ref.CoefOfDrag = 2.3;
Aux_ref.MaxDegree = 20;
Aux_ref.MaxOrder = 20;
Aux_ref.EnableSun = true;
Aux_ref.EnableMoon = true;
Aux_ref.EnableSolarRadiation = true;
Aux_ref.EnableDrag = true;
// Reference orbit
Step = 120.0; // [s]
N_Step1 = 50; // 100 mins
N_Step2 = 720; // 1 day
Aux = Aux_ref;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_Ref);
// J2,0 perturbations
Aux.MaxDegree = 2; Aux.MaxOrder = 0;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_J20);
// J2,2 perturbations
Aux.MaxDegree = 2; Aux.MaxOrder = 2;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_J22);
// J4,4 perturbations
Aux.MaxDegree = 4; Aux.MaxOrder = 4;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_J44);
// J10,10 perturbations
Aux.MaxDegree = 10; Aux.MaxOrder = 10;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_J1010);
Aux.MaxDegree = 20; Aux.MaxOrder = 20;
// Solar perturbations
Aux.EnableSun = false;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_Sun);
Aux.EnableSun = true;
// Lunar perturbations
Aux.EnableMoon = false;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_Moon);
Aux.EnableMoon = true;
// Solar radiation pressure perturbations
Aux.EnableSolarRadiation = false;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_SRad);
Aux.EnableSolarRadiation = true;
// Drag perturbations
Aux.EnableDrag = false;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_Drag);
Aux.EnableDrag = true;
// Find maximum over N_Step1 steps
Max_J20 = Max_J22 = Max_J44 = Max_J1010 = Max_Sun = Max_Moon = Max_SRad = Max_Drag = 0.0;
for (i = 0; i <= N_Step1; i++)
{
var refEph = Eph_Ref[i].Slice(0, 2);
Max_J20 = max(Norm(Eph_J20[i].Slice(0, 2) - refEph), Max_J20);
Max_J22 = max(Norm(Eph_J22[i].Slice(0, 2) - refEph), Max_J22);
Max_J44 = max(Norm(Eph_J44[i].Slice(0, 2) - refEph), Max_J44);
Max_J1010 = max(Norm(Eph_J1010[i].Slice(0, 2) - refEph), Max_J1010);
Max_Sun = max(Norm(Eph_Sun[i].Slice(0, 2) - refEph), Max_Sun);
Max_Moon = max(Norm(Eph_Moon[i].Slice(0, 2) - refEph), Max_Moon);
Max_SRad = max(Norm(Eph_SRad[i].Slice(0, 2) - refEph), Max_SRad);
Max_Drag = max(Norm(Eph_Drag[i].Slice(0, 2) - refEph), Max_Drag);
}
;
// Output
var info = "Exercise 3-4: Orbit Perturbations " + "\r\n" + "\r\n";
info += "Remote sensing satellite: " + "\r\n" + "\r\n";
info += " Maximum position errors within ";
Console.Write(info);
info = String.Format("{0, 8:F}", N_Step1 * Step / 60.0);
info += " min propagation interval " + "\r\n";
info += " J2,0 J2,2 J4,4 J10,10" + " Sun Moon SolRad Drag" + "\r\n";
info += " [m] [m] [m] [m]" + " [m] [m] [m] [m]";
Console.WriteLine(info);
info = String.Format("{0, 8:F}{1, 8:F}{2, 8:F}{3, 8:F}{4, 8:F}{5, 8:F}{6, 8:F}{7, 8:F}", Max_J20, Max_J22, Max_J44, Max_J1010, Max_Sun, Max_Moon, Max_SRad, Max_Drag);
Console.WriteLine(info);
Console.WriteLine();
// Find maximum over N_Step2 steps
for (i = N_Step1 + 1; i <= N_Step2; i++)
{
var refEph = Eph_Ref[i].Slice(0, 2);
Max_J20 = max(Norm(Eph_J20[i].Slice(0, 2) - refEph), Max_J20);
Max_J22 = max(Norm(Eph_J22[i].Slice(0, 2) - refEph), Max_J22);
Max_J44 = max(Norm(Eph_J44[i].Slice(0, 2) - refEph), Max_J44);
Max_J1010 = max(Norm(Eph_J1010[i].Slice(0, 2) - refEph), Max_J1010);
Max_Sun = max(Norm(Eph_Sun[i].Slice(0, 2) - refEph), Max_Sun);
Max_Moon = max(Norm(Eph_Moon[i].Slice(0, 2) - refEph), Max_Moon);
Max_SRad = max(Norm(Eph_SRad[i].Slice(0, 2) - refEph), Max_SRad);
Max_Drag = max(Norm(Eph_Drag[i].Slice(0, 2) - refEph), Max_Drag);
}
;
// Output
info = " Maximum position errors within ";
Console.Write(info);
info = String.Format("{0, 8:F}", N_Step2 * Step / 60.0);
info += " min propagation interval " + "\r\n";
info += " J2,0 J2,2 J4,4 J10,10" + " Sun Moon SolRad Drag" + "\r\n";
info += " [m] [m] [m] [m]" + " [m] [m] [m] [m]";
Console.WriteLine(info);
info = String.Format("{0, 8:F}{1, 8:F}{2, 8:F}{3, 8:F}{4, 8:F}{5, 8:F}{6, 8:F}{7, 8:F}", Max_J20, Max_J22, Max_J44, Max_J1010, Max_Sun, Max_Moon, Max_SRad, Max_Drag);
Console.WriteLine(info);
Console.WriteLine();
// Epoch state (geostationary satellite)
Kep = new Vector(42166.0e3, 0.0004, 0.02 * OrbitConsts.RadPerDeg, 0.0, 0.0, 0.0);
Y0 = Kepler.State(OrbitConsts.GM_Earth, Kep, 0.0);
// Model parameters
Aux_ref.Area = 10.0; // [m^2] Geostationary satellite
// Reference orbit
Step = 1200.0; // [s]
N_Step1 = 72; // 1 day
N_Step2 = 144; // 2 days
Aux = Aux_ref;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_Ref);
// J2,0 perturbations
Aux.MaxDegree = 2; Aux.MaxOrder = 0;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_J20);
// J2,2 perturbations
Aux.MaxDegree = 2; Aux.MaxOrder = 2;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_J22);
// J4,4 perturbations
Aux.MaxDegree = 4; Aux.MaxOrder = 4;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_J44);
// J10,10 perturbations
Aux.MaxDegree = 10; Aux.MaxOrder = 10;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_J1010);
Aux.MaxDegree = 20; Aux.MaxOrder = 20;
// Solar perturbations
Aux.EnableSun = false;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_Sun);
Aux.EnableSun = true;
// Lunar perturbations
Aux.EnableMoon = false;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_Moon);
Aux.EnableMoon = true;
// Solar radiation pressure perturbations
Aux.EnableSolarRadiation = false;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_SRad);
Aux.EnableSolarRadiation = true;
// Drag perturbations
Aux.EnableDrag = false;
Ephemeris(Y0, N_Step2, Step, Aux, Eph_Drag);
Aux.EnableDrag = true;
// Find maximum over N_Step1 steps
Max_J20 = Max_J22 = Max_J44 = Max_J1010 = Max_Sun = Max_Moon = Max_SRad = Max_Drag = 0.0;
for (i = 0; i <= N_Step1; i++)
{
var refEph = Eph_Ref[i].Slice(0, 2);
Max_J20 = max(Norm(Eph_J20[i].Slice(0, 2) - refEph), Max_J20);
Max_J22 = max(Norm(Eph_J22[i].Slice(0, 2) - refEph), Max_J22);
Max_J44 = max(Norm(Eph_J44[i].Slice(0, 2) - refEph), Max_J44);
Max_J1010 = max(Norm(Eph_J1010[i].Slice(0, 2) - refEph), Max_J1010);
Max_Sun = max(Norm(Eph_Sun[i].Slice(0, 2) - refEph), Max_Sun);
Max_Moon = max(Norm(Eph_Moon[i].Slice(0, 2) - refEph), Max_Moon);
Max_SRad = max(Norm(Eph_SRad[i].Slice(0, 2) - refEph), Max_SRad);
Max_Drag = max(Norm(Eph_Drag[i].Slice(0, 2) - refEph), Max_Drag);
}
;
// Output
// Output
info = "Geostationary satellite: " + "\r\n";
info += " Maximum position errors within ";
Console.Write(info);
info = String.Format("{0, 8:F}", N_Step1 * Step / 60.0);
info += " min propagation interval " + "\r\n";
info += " J2,0 J2,2 J4,4 J10,10" + " Sun Moon SolRad Drag" + "\r\n";
info += " [m] [m] [m] [m]" + " [m] [m] [m] [m]";
Console.WriteLine(info);
info = String.Format("{0, 8:F}{1, 8:F}{2, 8:F}{3, 8:F}{4, 8:F}{5, 8:F}{6, 8:F}{7, 8:F}", Max_J20, Max_J22, Max_J44, Max_J1010, Max_Sun, Max_Moon, Max_SRad, Max_Drag);
Console.WriteLine(info);
Console.WriteLine();
// Find maximum over N_Step2 steps
for (i = N_Step1 + 1; i <= N_Step2; i++)
{
var refEph = Eph_Ref[i].Slice(0, 2);
Max_J20 = max(Norm(Eph_J20[i].Slice(0, 2) - refEph), Max_J20);
Max_J22 = max(Norm(Eph_J22[i].Slice(0, 2) - refEph), Max_J22);
Max_J44 = max(Norm(Eph_J44[i].Slice(0, 2) - refEph), Max_J44);
Max_J1010 = max(Norm(Eph_J1010[i].Slice(0, 2) - refEph), Max_J1010);
Max_Sun = max(Norm(Eph_Sun[i].Slice(0, 2) - refEph), Max_Sun);
Max_Moon = max(Norm(Eph_Moon[i].Slice(0, 2) - refEph), Max_Moon);
Max_SRad = max(Norm(Eph_SRad[i].Slice(0, 2) - refEph), Max_SRad);
Max_Drag = max(Norm(Eph_Drag[i].Slice(0, 2) - refEph), Max_Drag);
}
;
// Output
info = " Maximum position errors within ";
Console.Write(info);
info = String.Format("{0, 8:F}", N_Step2 * Step / 60.0);
info += " min propagation interval " + "\r\n";
info += " J2,0 J2,2 J4,4 J10,10" + " Sun Moon SolRad Drag" + "\r\n";
info += " [m] [m] [m] [m]" + " [m] [m] [m] [m]";
Console.WriteLine(info);
info = String.Format("{0, 8:F}{1, 8:F}{2, 8:F}{3, 8:F}{4, 8:F}{5, 8:F}{6, 8:F}{7, 8:F}", Max_J20, Max_J22, Max_J44, Max_J1010, Max_Sun, Max_Moon, Max_SRad, Max_Drag);
Console.WriteLine(info);
Console.WriteLine();
Console.ReadKey();
}
static void Main0(string[] args)
{
// Variables
double MJD_UTC; // Modified Julian Date (UTC)
double MJD_UT1; // Modified Julian Date (UTC)
double MJD_TT; // Modified Julian Date (TT)
Matrix P = new Matrix(3, 3); // Precession matrix (ICRS -> mean-of-date)
Matrix N = new Matrix(3, 3); // Nutation matrix (mean-of-date -> true-of-date)
Matrix Theta = new Matrix(3, 3); // Sidereal Time matrix (tod -> pseudo-Earth-fixed)
Matrix Pi = new Matrix(3, 3); // Polar motion matrix (pseudo-Earth-fixed -> ITRS)
// Header
var info = "Exercise 5-1: Transformation from celestial "
+ "to terrestrial reference system" + "\r\n";
Console.WriteLine(info);
// Earth Orientation Parameters (UT1-UTC[s],UTC-TAI[s], x["], y["])
// (from IERS Bulletin B #135 and C #16; valid for 1999/03/04 0:00 UTC)
IERS IERS = new IERS(0.6492332, -32.0, 0.06740, 0.24173);
// Date
MJD_UTC = DateUtil.DateToMjd(1999, 03, 04, 0, 0, 0.0);
MJD_UT1 = MJD_UTC + IERS.GetUT1_UTC(MJD_UTC) / 86400.0;
MJD_TT = MJD_UTC + IERS.GetTT_UTC(MJD_UTC) / 86400.0;
// IAU 1976 Precession
// (ICRF to mean equator and equinox of date)
P = IERS.PrecessionMatrix(OrbitConsts.MJD_J2000, MJD_TT);
// IAU 1980 Nutation
// (Transformation to the true equator and equinox)
N = IERS.NutMatrix(MJD_TT);
// Apparent Sidereal Time
// Rotation about the Celestial Ephemeris Pole
Theta = IERS.GreenwichHourAngleMatrix(MJD_UT1); // Note: here we evaluate the equation of the
// equinoxes with the MJD_UT1 time argument
// (instead of MJD_TT)
// Polar motion
// (Transformation from the CEP to the IRP of the ITRS)
Pi = IERS.PoleMatrix(MJD_UTC); // Note: the time argument of polar motion series
// is not rigorously defined, but any differences
// are negligible
// Output
var endl = "\r\n";
info = "Date" + "\r\n"
+ " " + DateUtil.MjdToDateTimeString(MJD_UTC) + " UTC" + endl
+ " " + DateUtil.MjdToDateTimeString(MJD_UT1) + " UT1" + endl
+ " " + DateUtil.MjdToDateTimeString(MJD_TT) + " TT " + endl + endl + endl;
Console.WriteLine(info);
info = "IAU 1976 Precession matrix (ICRS to tod)" + endl
+ P.ToString();
Console.WriteLine(info);
info = "IAU 1980 Nutation matrix (tod to mod)" + endl
+ N.ToString();
Console.WriteLine(info);
info = "Earth Rotation matrix" + endl
+ Theta.ToString();
Console.WriteLine(info);
info = "Polar motion matrix" + endl
+ Pi.ToString();
Console.WriteLine(info);
info = "ICRS-ITRS transformation" + endl
+ (Pi * Theta * N * P).ToString();
Console.WriteLine(info);
Console.ReadKey();
}
static void Main0(string[] args)
{
// Ground station
const int N_obs = 6;
const double Step = 1200.0;
Vector Null3D = new Vector(0.0, 0.0, 0.0);
const double sigma_range = 10.0; // [m]
const double sigma_angle = 0.01 * OrbitConsts.RadPerDeg; // [rad] (0.01 deg = 36")
string[] Label = { "x [m] ", "y [m] ", "z [m] ",
"vx [m/s]", "vy [m/s]", "vz [m/s]" };
// Variables
int i;
double Mjd0, t, t_old, MjdUTC, Theta;
double Azim = 0, Elev = 0, Dist = 0;
Vector Y0_true = new Vector(6), Y_true = new Vector(6), Y = new Vector(6), Y_old = new Vector(6);
Vector ErrY = new Vector(6), SigY = new Vector(6);
Vector r = new Vector(3), R = new Vector(3), s = new Vector(3);
Vector dAds = new Vector(3), dEds = new Vector(3), dDds = new Vector(3);
Vector dAdY = new Vector(6), dEdY = new Vector(6), dDdY = new Vector(6);
Matrix U = new Matrix(3, 3), E = new Matrix(3, 3);
Matrix Phi = new Matrix(6, 6), Phi_true = new Matrix(6, 6), P = new Matrix(6, 6);
ExtendedKalmanFilter Filter = new ExtendedKalmanFilter(6);
ObsType[] Obs = new ObsType[N_obs];
// Ground station
R = new Vector(+1344.0e3, +6069.0e3, 1429.0e3); // [m] Bangalore
E = CoordTransformer.XyzToGeoCoord(new XYZ(R.OneDimArray)).ToLocalNez_Matrix();
// Header
var endl = "\r\n";
var info = "Exercise 8-3: Sequential orbit determination" + endl + endl;
Console.Write(info);
// Generation of artificial observations from given epoch state
Mjd0 = DateUtil.DateToMjd(1995, 03, 30, 00, 00, 00.0); // Epoch (UTC)
Y0_true = new Vector(-6345.000e3, -3723.000e3, -580.000e3, // [m]
+2.169000e3, -9.266000e3, -1.079000e3); // [m/s]
info = "Measurements" + endl + endl
+ " Date UTC Az[deg] El[deg] Range[km]" + endl;
Console.Write(info);
for (i = 0; i < N_obs; i++)
{
// Time increment and propagation
t = (i + 1) * Step; // Time since epoch [s]
MjdUTC = Mjd0 + t / 86400.0; // Modified Julian Date
Kepler.TwoBody(OrbitConsts.GM_Earth, Y0_true, t, ref Y, ref Phi); // State vector
// Topocentric coordinates
Theta = IERS.GetGmstRad(MjdUTC); // Earth rotation
U = Matrix.RotateZ3D(Theta);
r = Y.Slice(0, 2);
s = E * (U * r - R); // Topocentric position [m]
GeoCoord.LocalEnzToPolar(s, out Azim, out Elev, out Dist); // Azimuth, Elevation, Range
// Observation record
Obs[i].Mjd_UTC = MjdUTC;
Obs[i].Azim = Azim;
Obs[i].Elev = Elev;
Obs[i].Dist = Dist;
// Output
info = " " + DateUtil.MjdToDateTimeString(MjdUTC) + String.Format(" {0, 10:F3}{1, 10:F3}{2, 10:F3}", +OrbitConsts.DegPerRad * Azim, OrbitConsts.DegPerRad * Elev, Dist / 1000.0) + endl;
Console.Write(info);
}
;
//
// Orbit determination
//
info = "State errors" + endl + endl
+ " Pos[m] Vel[m/s] "
+ endl
+ " Date UTC Upd. Error Sigma Error Sigma"
+ endl;
Console.Write(info);
// Initialization
Mjd0 = DateUtil.DateToMjd(1995, 03, 30, 00, 00, 00.0); // Epoch (UTC)
t = 0.0;
Y = Y0_true + new Vector(+10.0e3, -5.0e3, +1.0e3, -1.0, +3.0, -0.5);
//P = 0.0;
for (i = 0; i < 3; i++)
{
P[i, i] = 1.0e8;
}
for (i = 3; i < 6; i++)
{
P[i, i] = 1.0e2;
}
Filter.Init(t, Y, P);
// Measurement loop
for (i = 0; i < N_obs; i++)
{
// Previous step
t_old = Filter.Time();
Y_old = Filter.State();
// Propagation to measurement epoch
MjdUTC = Obs[i].Mjd_UTC; // Modified Julian Date
t = (MjdUTC - Mjd0) * 86400.0; // Time since epoch [s]
Kepler.TwoBody(OrbitConsts.GM_Earth, Y_old, t - t_old, ref Y, ref Phi); // State vector
Theta = IERS.GetGmstRad(MjdUTC); // Earth rotation
U = Matrix.RotateZ3D(Theta);
// Time update
Filter.TimeUpdate(t, Y, Phi);
// Truth orbit
Kepler.TwoBody(OrbitConsts.GM_Earth, Y0_true, t, ref Y_true, ref Phi_true);
// State error and standard deviation
ErrY = Filter.State() - Y_true;
SigY = Filter.StdDev();
info = DateUtil.MjdToDateTimeString(MjdUTC) + " t "
+ String.Format("{0, 10:F3}{1, 10:F3}{2, 10:F3}{3, 10:F3}", ErrY.Slice(0, 2).Norm(), SigY.Slice(0, 2).Norm(), ErrY.Slice(3, 5).Norm(), SigY.Slice(3, 5).Norm()) + endl;
Console.Write(info);
// Azimuth and partials
r = Filter.State().Slice(0, 2);
s = E * (U * r - R); // Topocentric position [m]
GeoCoord.LocalEnzToPolar(s, out Azim, out Elev, out dAds, out dEds); // Azimuth, Elevation
dAdY = (dAds * E * U).Stack(Null3D);
// Measurement update
Filter.MeasUpdate(Obs[i].Azim, Azim, sigma_angle / Math.Cos(Elev), dAdY);
ErrY = Filter.State() - Y_true;
SigY = Filter.StdDev();
info = " Az "
+ String.Format("{0, 10:F3}{1, 10:F3}{2, 10:F3}{3, 10:F3}", ErrY.Slice(0, 2).Norm(), SigY.Slice(0, 2).Norm(), ErrY.Slice(3, 5).Norm(), SigY.Slice(3, 5).Norm()) + endl;
Console.Write(info);
// Elevation and partials
r = Filter.State().Slice(0, 2);
s = E * (U * r - R); // Topocentric position [m]
GeoCoord.LocalEnzToPolar(s, out Azim, out Elev, out dAds, out dEds); // Azimuth, Elevation
dEdY = (dEds * E * U).Stack(Null3D);
// Measurement update
Filter.MeasUpdate(Obs[i].Elev, Elev, sigma_angle, dEdY);
ErrY = Filter.State() - Y_true;
SigY = Filter.StdDev();
info = " El "
+ String.Format("{0, 10:F3}{1, 10:F3}{2, 10:F3}{3, 10:F3}", ErrY.Slice(0, 2).Norm(), SigY.Slice(0, 2).Norm(), ErrY.Slice(3, 5).Norm(), SigY.Slice(3, 5).Norm()) + endl;
Console.Write(info);
// Range and partials
r = Filter.State().Slice(0, 2);
s = E * (U * r - R); // Topocentric position [m]
Dist = s.Norm(); dDds = s / Dist; // Range
dDdY = (dDds * E * U).Stack(Null3D);
// Measurement update
Filter.MeasUpdate(Obs[i].Dist, Dist, sigma_range, dDdY);
ErrY = Filter.State() - Y_true;
SigY = Filter.StdDev();
info = " rho"
+ String.Format("{0, 10:F3}{1, 10:F3}{2, 10:F3}{3, 10:F3}", ErrY.Slice(0, 2).Norm(), SigY.Slice(0, 2).Norm(), ErrY.Slice(3, 5).Norm(), SigY.Slice(3, 5).Norm()) + endl;
Console.Write(info);
}
Console.ReadKey();
}
static void Main0(string[] args)
{
string endl = "\r\n";
// Ground station
Vector R_Sta = new Geo.Algorithm.Vector(+1344.143e3, +6068.601e3, +1429.311e3); // Position vector
GeoCoord Sta = CoordTransformer.XyzToGeoCoord(new XYZ(R_Sta.OneDimArray)); //new GeoCoord(R_Sta, OrbitConsts.RadiusOfEarth, OrbitConsts.FlatteningOfEarth);// Geodetic coordinates
// Observations
ObsType[] Obs = new ObsType[] {
new ObsType(DateUtil.DateToMjd(1999, 04, 02, 00, 30, 00.0), 132.67 * OrbitConsts.RadPerDeg, 32.44 * OrbitConsts.RadPerDeg, 16945.450e3),
new ObsType(DateUtil.DateToMjd(1999, 04, 02, 03, 00, 00.0), 123.08 * OrbitConsts.RadPerDeg, 50.06 * OrbitConsts.RadPerDeg, 37350.340e3)
};
// Variables
int i, j;
double Az, El, d;
Geo.Algorithm.Vector s = new Geo.Algorithm.Vector(3);
Geo.Algorithm.Vector[] r = new Geo.Algorithm.Vector[2];
// Transformation to local tangent coordinates
Matrix E = Sta.ToLocalNez_Matrix();
// Convert observations
for (i = 0; i < 2; i++)
{
// Earth rotation
Matrix U = Matrix.RotateZ3D(IERS.GetGmstRad(Obs[i].Mjd_UTC));
// Topocentric position vector
Az = Obs[i].Azimuth; El = Obs[i].Elevation; d = Obs[i].Range;
s = d * Geo.Algorithm.Vector.VecPolar(OrbitConsts.PI / 2 - Az, El);
// Inertial position vector
r[i] = U.Transpose() * (E.Transpose() * s + R_Sta);
}
// Orbital elements
Geo.Algorithm.Vector Kep = Kepler.Elements(OrbitConsts.GM_Earth, Obs[0].Mjd_UTC, Obs[1].Mjd_UTC, r[0], r[1]);
// Output
var info = "Exercise 2-6: Initial orbit determination" + "\r\n";
info += "Inertial positions:" + "\r\n";
info += " ";
info += "[km]" + " [km]" + " [km]";
Console.WriteLine(info);
for (i = 0; i < 2; i++)
{
info = " " + DateUtil.MjdToDateTimeString(Obs[i].Mjd_UTC);
for (j = 0; j < 3; j++)
{
info += " " + String.Format("{0, 12:F3}", r[i][j] / 1000.0);
}
;
Console.WriteLine(info);
}
Console.WriteLine();
info = "Orbital elements:" + "\r\n"
+ " Epoch (1st obs.) " + DateUtil.MjdToDateTimeString(Obs[0].Mjd_UTC) + endl
+ " Semimajor axis " + String.Format("{0, 10:F3}", Kep[0] / 1000.0) + " km" + endl
+ " Eccentricity " + String.Format("{0, 10:F3}", Kep[1]) + endl
+ " Inclination " + String.Format("{0, 10:F3}", Kep[2] * OrbitConsts.DegPerRad) + " deg" + endl
+ " RA ascend. node " + String.Format("{0, 10:F3}", Kep[3] * OrbitConsts.DegPerRad) + " deg" + endl
+ " Arg. of perigee " + String.Format("{0, 10:F3}", Kep[4] * OrbitConsts.DegPerRad) + " deg" + endl
+ " Mean anomaly " + String.Format("{0, 10:F3}", Kep[5] * OrbitConsts.DegPerRad) + " deg" + endl;
Console.WriteLine(info);
Console.ReadKey();
}
