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