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