예제 #1
0
        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();
        }
        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();
        }
예제 #3
0
        // 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();
        }
예제 #4
0
        static void Main0(string[] args)
        {
            // Variables

            double[] MeanAnom = { 4.0, 50.0 };   // [deg]
            int      i;
            double   M, E, E_ref, e;


            for (int iCase = 0; iCase <= 1; iCase++)
            {
                // Test Case
                M     = MeanAnom[iCase] * OrbitConsts.RadPerDeg;
                e     = 0.72;
                E_ref = Kepler.EccAnom(M, e);

                // Header

                String str = "Exercise 2-2: Solution of Kepler's equation" + "\r\n" + "\r\n";
                str += "  M =" + M.ToString("F11") + "\r\n";
                str += "  e =" + e.ToString("F11") + "\r\n";
                str += "  E =" + E_ref.ToString("F11") + "\r\n";;
                Console.WriteLine(str);
                // Newton's iteration
                string info = "  a) Newton's iteration" + "\r\n" + "\r\n"
                              + "  i         E         Accuracy  sin/cos" + "\r\n";
                Console.WriteLine(info);
                E = M;
                i = 0;
                do
                {
                    i++;
                    E    = E - (E - e * Math.Sin(E) - M) / (1.0 - e * Math.Cos(E));
                    info = " " + i + "   " + E.ToString("F11") + "  " + Math.Abs(E - E_ref).ToString("E2") + "  " + 2 * i;
                    Console.WriteLine(info);
                } while (Math.Abs(E - E_ref) > 1.0e-10);

                // Fixed point iteration
                Console.WriteLine();
                info = "  b) Fixed point iteration" + "\r\n" + "  i         E         Accuracy  sin/cos";
                Console.WriteLine(info);
                E = M;
                i = 0;
                do
                {
                    i++;
                    E    = M + e * Math.Sin(E);
                    info = " " + i + "   " + E.ToString("F11") + "  " + Math.Abs(E - E_ref).ToString("E2") + "  " + i;
                    Console.WriteLine(info);
                } while (Math.Abs(E - E_ref) > 1.0e-10);
            }
            ;
            Console.ReadKey();
        }
예제 #5
0
        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 = { 100, 300, 600, 1000, 1500, 2000, 3000, 4000 };

            // Variables
            // Function call count
            int    iCase;
            double t, h;                                                                                 // Time and step aboutSize

            GaussJackson4OrderPredictor Orbit = new GaussJackson4OrderPredictor(f_Kep3D, 3, nCallsPlus); // Object for integrating the
            // 2nd order diff. equation
            // defined by f_Kep3D using the 4th-order GJ predictor

            // Header

            var info = "Exercise 4-2: Gauss-Jackson 4th-order predictor" + "\r\n";

            info += "  Problem D1 (e=0.1)" + "\r\n";
            info += "  N_fnc   Accuracy   Digits " + "\r\n";
            Console.WriteLine(info);
            // Loop over test cases

            for (iCase = 0; iCase < 8; iCase++)
            {
                // Step aboutSize
                var step = Steps[iCase];
                h = t_end / step;
                // Initial values
                nCalls = 0;
                t      = 0.0;
                var y = Orbit.GetState(e, t, h, step);

                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();
        }
예제 #6
0
        static void Main0(string[] args)
        {
            // Constants

            const double GM    = 1.0;                                                          // Gravitational coefficient
            const double e     = 0.9;                                                          // 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

            // Variables

            double t;                                                        // Time
            double relerr, abserr;                                           // Accuracy requirements

            Geo.Algorithm.Vector y     = new Geo.Algorithm.Vector(6);        // State vector
            AuxDataRecord        Aux   = new AuxDataRecord();                // Auxiliary satData
            DeIntegrator         Orbit = new DeIntegrator(f_Kep6D_, 6, Aux); // Object for integrating the
            // differential equation
            // defined by f_Kep6D_
            // Header

            var info = "Exercise 4-3: Step size control of DE multistep method" + "\r\n";

            info += "  Step       t           h          r " + "\r\n";
            Console.WriteLine(info);
            // 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);

            Aux.n_step = 0;
            Aux.t      = 0;

            relerr = 0.0;
            abserr = 1.0e-8;

            // Integration from t=t to t=t_end
            Orbit.Init(t, relerr, abserr);
            Orbit.Integ(t_end, y);

            Console.ReadKey();
        }
예제 #7
0
        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();
        }
예제 #8
0
        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 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();
        }
예제 #11
0
        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)
        {
            // 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();
        }