Exemple #1
0
        /// <summary>
        /// 计算绕地航天器主要的加速度。
        ///   Computes the acceleration of an Earth orbiting satellite due to
        ///    - the Earth's harmonic gravity field,
        ///    - the gravitational perturbations of the Sun and Moon
        ///    - the solar radiation pressure and
        ///    - the atmospheric drag
        /// </summary>
        /// <param name="Mjd_TT"> Mjd_TT  Terrestrial Time (Modified Julian Date)</param>
        /// <param name="satXyzIcrf"> r 卫星在国际天球参考框架的位置  Satellite position vector in the ICRF/EME2000 system</param>
        /// <param name="satVelocityIcrf"> v 卫星在国际天球参考框架的速度 Satellite velocity vector in the ICRF/EME2000 system</param>
        /// <param name="Area">  Area    Cross-section </param>
        /// <param name="mass">  mass 质量  Spacecraft mass</param>
        /// <param name="solarRadiPresCoeff"> coefOfRadiation  光压系数  Radiation pressure coefficient</param>
        /// <param name="atmDragCoefficient"> coefOfDrag  大气阻力系数  Drag coefficient</param>
        /// <returns> Acceleration (a=d^2r/dt^2) in the ICRF/EME2000 system</returns>
        public static Geo.Algorithm.Vector AccelerOfMainForces(double Mjd_TT, Geo.Algorithm.Vector satXyzIcrf, Geo.Algorithm.Vector satVelocityIcrf, double Area, double mass, double solarRadiPresCoeff, double atmDragCoefficient)
        {
            double Mjd_UT1;

            Geo.Algorithm.Vector a = new Geo.Algorithm.Vector(3), r_Sun = new Geo.Algorithm.Vector(3), r_Moon = new Geo.Algorithm.Vector(3);
            Matrix T = new Matrix(3, 3), E = new Matrix(3, 3);

            // Acceleration due to harmonic gravity field
            Mjd_UT1 = Mjd_TT;

            T = IERS.NutMatrix(Mjd_TT) * IERS.PrecessionMatrix(MJD_J2000, Mjd_TT);
            E = IERS.GreenwichHourAngleMatrix(Mjd_UT1) * T;

            a = AccelerOfHarmonicGraviFiled(satXyzIcrf, E, Grav.GM, Grav.R_ref, Grav.CS, Grav.n_max, Grav.m_max);

            // Luni-solar perturbations
            r_Sun  = CelestialUtil.Sun(Mjd_TT);
            r_Moon = CelestialUtil.Moon(Mjd_TT);

            a += AccelerOfPointMass(satXyzIcrf, r_Sun, OrbitConsts.GM_Sun);
            a += AccelerOfPointMass(satXyzIcrf, r_Moon, OrbitConsts.GM_Moon);

            // Solar radiation pressure
            a += CelestialUtil.Illumination(satXyzIcrf, r_Sun) * AccelerOfSolarRadiPressure(satXyzIcrf, r_Sun, Area, mass, solarRadiPresCoeff, OrbitConsts.PressureOfSolarRadiationPerAU, AU);

            // Atmospheric drag
            a += AccelerDragOfAtmos(Mjd_TT, satXyzIcrf, satVelocityIcrf, T, Area, mass, atmDragCoefficient);

            // Acceleration
            return(a);
        }
Exemple #2
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();
        }
        //------------------------------------------------------------------------------
        //
        // 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);
        }
Exemple #5
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();
        }
Exemple #6
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();
        }
Exemple #8
0
 /// <summary>
 /// 构造函数。
 /// </summary>
 /// <param name="option"></param>
 /// <param name="IERS"></param>
 public AccelerationCalculator(ForceModelOption option, IERS IERS)
 {
     this.IERS   = IERS;
     this.Option = option;
 }
Exemple #9
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)
        {
            // Variables

            double MJD_UTC;                  // Modified Julian Date (UTC)
            double MJD_UT1;                  // Modified Julian Date (UTC)
            double MJD_TT;                   // Modified Julian Date (TT)
            Matrix P     = new Matrix(3, 3); // Precession matrix (ICRS -> mean-of-date)
            Matrix N     = new Matrix(3, 3); // Nutation matrix (mean-of-date -> true-of-date)
            Matrix Theta = new Matrix(3, 3); // Sidereal Time matrix (tod -> pseudo-Earth-fixed)
            Matrix Pi    = new Matrix(3, 3); // Polar motion matrix (pseudo-Earth-fixed -> ITRS)


            // Header

            var info = "Exercise 5-1: Transformation from celestial "
                       + "to terrestrial reference system" + "\r\n";

            Console.WriteLine(info);


            // Earth Orientation Parameters (UT1-UTC[s],UTC-TAI[s], x["], y["])
            // (from IERS Bulletin B #135 and C #16; valid for 1999/03/04 0:00 UTC)

            IERS IERS = new IERS(0.6492332, -32.0, 0.06740, 0.24173);


            // Date

            MJD_UTC = DateUtil.DateToMjd(1999, 03, 04, 0, 0, 0.0);
            MJD_UT1 = MJD_UTC + IERS.GetUT1_UTC(MJD_UTC) / 86400.0;
            MJD_TT  = MJD_UTC + IERS.GetTT_UTC(MJD_UTC) / 86400.0;

            // IAU 1976 Precession
            // (ICRF to mean equator and equinox of date)

            P = IERS.PrecessionMatrix(OrbitConsts.MJD_J2000, MJD_TT);

            // IAU 1980 Nutation
            // (Transformation to the true equator and equinox)

            N = IERS.NutMatrix(MJD_TT);

            // Apparent Sidereal Time
            // Rotation about the Celestial Ephemeris Pole

            Theta = IERS.GreenwichHourAngleMatrix(MJD_UT1);   // Note: here we evaluate the equation of the
            // equinoxes with the MJD_UT1 time argument
            // (instead of MJD_TT)

            // Polar motion
            // (Transformation from the CEP to the IRP of the ITRS)

            Pi = IERS.PoleMatrix(MJD_UTC);     // Note: the time argument of polar motion series
            // is not rigorously defined, but any differences
            // are negligible

            // Output
            var endl = "\r\n";

            info = "Date" + "\r\n"
                   + " " + DateUtil.MjdToDateTimeString(MJD_UTC) + " UTC" + endl
                   + " " + DateUtil.MjdToDateTimeString(MJD_UT1) + " UT1" + endl
                   + " " + DateUtil.MjdToDateTimeString(MJD_TT) + " TT " + endl + endl + endl;
            Console.WriteLine(info);
            info = "IAU 1976 Precession matrix (ICRS to tod)" + endl
                   + P.ToString();
            Console.WriteLine(info);
            info = "IAU 1980 Nutation matrix (tod to mod)" + endl
                   + N.ToString();

            Console.WriteLine(info);
            info = "Earth Rotation matrix" + endl
                   + Theta.ToString();

            Console.WriteLine(info);
            info = "Polar motion matrix" + endl
                   + Pi.ToString();
            Console.WriteLine(info);
            info = "ICRS-ITRS transformation" + endl
                   + (Pi * Theta * N * P).ToString();
            Console.WriteLine(info);

            Console.ReadKey();
        }
        static void Main0(string[] args)
        {
            // Ground station



            const int    N_obs = 6;
            const double Step  = 1200.0;

            Vector Null3D = new Vector(0.0, 0.0, 0.0);

            const double sigma_range = 10.0;                         // [m]
            const double sigma_angle = 0.01 * OrbitConsts.RadPerDeg; // [rad] (0.01 deg = 36")

            string[] Label = { "x  [m]  ", "y  [m]  ", "z  [m]  ",
                               "vx [m/s]", "vy [m/s]", "vz [m/s]" };

            // Variables

            int    i;
            double Mjd0, t, t_old, MjdUTC, Theta;
            double Azim = 0, Elev = 0, Dist = 0;
            Vector Y0_true = new Vector(6), Y_true = new Vector(6), Y = new Vector(6), Y_old = new Vector(6);
            Vector ErrY = new Vector(6), SigY = new Vector(6);
            Vector r = new Vector(3), R = new Vector(3), s = new Vector(3);
            Vector dAds = new Vector(3), dEds = new Vector(3), dDds = new Vector(3);
            Vector dAdY = new Vector(6), dEdY = new Vector(6), dDdY = new Vector(6);
            Matrix U = new Matrix(3, 3), E = new Matrix(3, 3);
            Matrix Phi = new Matrix(6, 6), Phi_true = new Matrix(6, 6), P = new Matrix(6, 6);
            ExtendedKalmanFilter Filter = new ExtendedKalmanFilter(6);

            ObsType[] Obs = new ObsType[N_obs];


            // Ground station

            R = new Vector(+1344.0e3, +6069.0e3, 1429.0e3);    // [m] Bangalore
            E = CoordTransformer.XyzToGeoCoord(new XYZ(R.OneDimArray)).ToLocalNez_Matrix();

            // Header
            var endl = "\r\n";
            var info = "Exercise 8-3: Sequential orbit determination" + endl + endl;

            Console.Write(info);

            // Generation of artificial observations from given epoch state

            Mjd0 = DateUtil.DateToMjd(1995, 03, 30, 00, 00, 00.0);       // Epoch (UTC)

            Y0_true = new Vector(-6345.000e3, -3723.000e3, -580.000e3,   // [m]
                                 +2.169000e3, -9.266000e3, -1.079000e3); // [m/s]

            info = "Measurements" + endl + endl
                   + "     Date          UTC       Az[deg]   El[deg]   Range[km]" + endl;
            Console.Write(info);
            for (i = 0; i < N_obs; i++)
            {
                // Time increment and propagation
                t      = (i + 1) * Step;                                          // Time since epoch [s]
                MjdUTC = Mjd0 + t / 86400.0;                                      // Modified Julian Date
                Kepler.TwoBody(OrbitConsts.GM_Earth, Y0_true, t, ref Y, ref Phi); // State vector

                // Topocentric coordinates
                Theta = IERS.GetGmstRad(MjdUTC);                  // Earth rotation
                U     = Matrix.RotateZ3D(Theta);
                r     = Y.Slice(0, 2);
                s     = E * (U * r - R);                                   // Topocentric position [m]
                GeoCoord.LocalEnzToPolar(s, out Azim, out Elev, out Dist); // Azimuth, Elevation, Range

                // Observation record
                Obs[i].Mjd_UTC = MjdUTC;
                Obs[i].Azim    = Azim;
                Obs[i].Elev    = Elev;
                Obs[i].Dist    = Dist;

                // Output
                info = "  " + DateUtil.MjdToDateTimeString(MjdUTC) + String.Format(" {0, 10:F3}{1, 10:F3}{2, 10:F3}", +OrbitConsts.DegPerRad * Azim, OrbitConsts.DegPerRad * Elev, Dist / 1000.0) + endl;
                Console.Write(info);
            }
            ;


            //
            // Orbit determination
            //

            info = "State errors" + endl + endl
                   + "                                     Pos[m]             Vel[m/s]    "
                   + endl
                   + "     Date          UTC    Upd.   Error     Sigma     Error     Sigma"
                   + endl;
            Console.Write(info);

            // Initialization

            Mjd0 = DateUtil.DateToMjd(1995, 03, 30, 00, 00, 00.0);        // Epoch (UTC)

            t = 0.0;

            Y = Y0_true + new Vector(+10.0e3, -5.0e3, +1.0e3, -1.0, +3.0, -0.5);

            //P = 0.0;
            for (i = 0; i < 3; i++)
            {
                P[i, i] = 1.0e8;
            }
            for (i = 3; i < 6; i++)
            {
                P[i, i] = 1.0e2;
            }

            Filter.Init(t, Y, P);

            // Measurement loop

            for (i = 0; i < N_obs; i++)
            {
                // Previous step
                t_old = Filter.Time();
                Y_old = Filter.State();

                // Propagation to measurement epoch
                MjdUTC = Obs[i].Mjd_UTC;                 // Modified Julian Date
                t      = (MjdUTC - Mjd0) * 86400.0;      // Time since epoch [s]


                Kepler.TwoBody(OrbitConsts.GM_Earth, Y_old, t - t_old, ref Y, ref Phi); // State vector
                Theta = IERS.GetGmstRad(MjdUTC);                                        // Earth rotation
                U     = Matrix.RotateZ3D(Theta);

                // Time update
                Filter.TimeUpdate(t, Y, Phi);

                // Truth orbit
                Kepler.TwoBody(OrbitConsts.GM_Earth, Y0_true, t, ref Y_true, ref Phi_true);

                // State error and standard deviation
                ErrY = Filter.State() - Y_true;
                SigY = Filter.StdDev();
                info = DateUtil.MjdToDateTimeString(MjdUTC) + "  t  "
                       + String.Format("{0, 10:F3}{1, 10:F3}{2, 10:F3}{3, 10:F3}", ErrY.Slice(0, 2).Norm(), SigY.Slice(0, 2).Norm(), ErrY.Slice(3, 5).Norm(), SigY.Slice(3, 5).Norm()) + endl;

                Console.Write(info);
                // Azimuth and partials
                r = Filter.State().Slice(0, 2);
                s = E * (U * r - R);                                                 // Topocentric position [m]
                GeoCoord.LocalEnzToPolar(s, out Azim, out Elev, out dAds, out dEds); // Azimuth, Elevation
                dAdY = (dAds * E * U).Stack(Null3D);

                // Measurement update
                Filter.MeasUpdate(Obs[i].Azim, Azim, sigma_angle / Math.Cos(Elev), dAdY);
                ErrY = Filter.State() - Y_true;
                SigY = Filter.StdDev();
                info = "                         Az "
                       + String.Format("{0, 10:F3}{1, 10:F3}{2, 10:F3}{3, 10:F3}", ErrY.Slice(0, 2).Norm(), SigY.Slice(0, 2).Norm(), ErrY.Slice(3, 5).Norm(), SigY.Slice(3, 5).Norm()) + endl;
                Console.Write(info);

                // Elevation and partials
                r = Filter.State().Slice(0, 2);
                s = E * (U * r - R);                                                 // Topocentric position [m]
                GeoCoord.LocalEnzToPolar(s, out Azim, out Elev, out dAds, out dEds); // Azimuth, Elevation
                dEdY = (dEds * E * U).Stack(Null3D);

                // Measurement update
                Filter.MeasUpdate(Obs[i].Elev, Elev, sigma_angle, dEdY);
                ErrY = Filter.State() - Y_true;
                SigY = Filter.StdDev();
                info = "                         El "
                       + String.Format("{0, 10:F3}{1, 10:F3}{2, 10:F3}{3, 10:F3}", ErrY.Slice(0, 2).Norm(), SigY.Slice(0, 2).Norm(), ErrY.Slice(3, 5).Norm(), SigY.Slice(3, 5).Norm()) + endl;
                Console.Write(info);

                // Range and partials
                r    = Filter.State().Slice(0, 2);
                s    = E * (U * r - R);                         // Topocentric position [m]
                Dist = s.Norm(); dDds = s / Dist;               // Range
                dDdY = (dDds * E * U).Stack(Null3D);

                // Measurement update
                Filter.MeasUpdate(Obs[i].Dist, Dist, sigma_range, dDdY);
                ErrY = Filter.State() - Y_true;
                SigY = Filter.StdDev();
                info = "                         rho"
                       + String.Format("{0, 10:F3}{1, 10:F3}{2, 10:F3}{3, 10:F3}", ErrY.Slice(0, 2).Norm(), SigY.Slice(0, 2).Norm(), ErrY.Slice(3, 5).Norm(), SigY.Slice(3, 5).Norm()) + endl;
                Console.Write(info);
            }

            Console.ReadKey();
        }
        static void Main0(string[] args)
        {
            string endl = "\r\n";

            // Ground station

            Vector   R_Sta = new Geo.Algorithm.Vector(+1344.143e3, +6068.601e3, +1429.311e3); // Position vector
            GeoCoord Sta   = CoordTransformer.XyzToGeoCoord(new XYZ(R_Sta.OneDimArray));      //new GeoCoord(R_Sta, OrbitConsts.RadiusOfEarth, OrbitConsts.FlatteningOfEarth);// Geodetic coordinates

            // Observations
            ObsType[] Obs = new ObsType[]  {
                new ObsType(DateUtil.DateToMjd(1999, 04, 02, 00, 30, 00.0), 132.67 * OrbitConsts.RadPerDeg, 32.44 * OrbitConsts.RadPerDeg, 16945.450e3),
                new ObsType(DateUtil.DateToMjd(1999, 04, 02, 03, 00, 00.0), 123.08 * OrbitConsts.RadPerDeg, 50.06 * OrbitConsts.RadPerDeg, 37350.340e3)
            };

            // Variables

            int    i, j;
            double Az, El, d;

            Geo.Algorithm.Vector   s = new Geo.Algorithm.Vector(3);
            Geo.Algorithm.Vector[] r = new Geo.Algorithm.Vector[2];

            // Transformation to local tangent coordinates
            Matrix E = Sta.ToLocalNez_Matrix();

            // Convert observations
            for (i = 0; i < 2; i++)
            {
                // Earth rotation
                Matrix U = Matrix.RotateZ3D(IERS.GetGmstRad(Obs[i].Mjd_UTC));
                // Topocentric position vector
                Az = Obs[i].Azimuth; El = Obs[i].Elevation; d = Obs[i].Range;
                s  = d * Geo.Algorithm.Vector.VecPolar(OrbitConsts.PI / 2 - Az, El);
                // Inertial position vector
                r[i] = U.Transpose() * (E.Transpose() * s + R_Sta);
            }

            // Orbital elements
            Geo.Algorithm.Vector Kep = Kepler.Elements(OrbitConsts.GM_Earth, Obs[0].Mjd_UTC, Obs[1].Mjd_UTC, r[0], r[1]);

            // Output
            var info = "Exercise 2-6: Initial orbit determination" + "\r\n";

            info += "Inertial positions:" + "\r\n";
            info += "                             ";
            info += "[km]" + "          [km]" + "          [km]";
            Console.WriteLine(info);
            for (i = 0; i < 2; i++)
            {
                info = "  " + DateUtil.MjdToDateTimeString(Obs[i].Mjd_UTC);

                for (j = 0; j < 3; j++)
                {
                    info += " " + String.Format("{0, 12:F3}", r[i][j] / 1000.0);
                }
                ;
                Console.WriteLine(info);
            }
            Console.WriteLine();

            info = "Orbital elements:" + "\r\n"
                   + "  Epoch (1st obs.)  " + DateUtil.MjdToDateTimeString(Obs[0].Mjd_UTC) + endl
                   + "  Semimajor axis   " + String.Format("{0, 10:F3}", Kep[0] / 1000.0) + " km" + endl
                   + "  Eccentricity     " + String.Format("{0, 10:F3}", Kep[1]) + endl
                   + "  Inclination      " + String.Format("{0, 10:F3}", Kep[2] * OrbitConsts.DegPerRad) + " deg" + endl
                   + "  RA ascend. node  " + String.Format("{0, 10:F3}", Kep[3] * OrbitConsts.DegPerRad) + " deg" + endl
                   + "  Arg. of perigee  " + String.Format("{0, 10:F3}", Kep[4] * OrbitConsts.DegPerRad) + " deg" + endl
                   + "  Mean anomaly     " + String.Format("{0, 10:F3}", Kep[5] * OrbitConsts.DegPerRad) + " deg" + endl;
            Console.WriteLine(info);
            Console.ReadKey();
        }