示例#1
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();
        }
示例#2
0
        static void Main0(string[] args)
        {
            // Constants
            double[] t = { 0.04, 0.32, 0.51, 0.73, 1.03, 1.42, 1.60 };
            double[] z = { 2.63, 1.18, 1.16, 1.54, 2.65, 5.41, 7.67 };

            // Variables
            int    i;
            double b;

            Geo.Algorithm.Vector a = new Geo.Algorithm.Vector(3), c = new Geo.Algorithm.Vector(3), d = new Geo.Algorithm.Vector(3);
            Matrix       R       = new Matrix(3, 3);
            LsqEstimater PolyFit = new LsqEstimater(3);


            // Header
            var endl = "\r\n";
            var info = "Exercise 8-1: Least-squares fit using Givens rotation"
                       + endl + endl;

            Console.Write(info);

            // Accumulation of satData equations
            for (i = 0; i < 7; i++)
            {
                // Data equation
                a[0] = 1.0;
                a[1] = t[i];
                a[2] = t[i] * t[i];
                b    = z[i];

                info = "Observation " + i + endl + endl
                       + String.Format("  a = {0, 8:F4}", a) + "    "
                       + String.Format("  b = {0, 8:F4}", b) + endl + endl;
                Console.Write(info);
                // Process satData equation

                PolyFit.Accumulate(a, b);

                // Square-root information matrix and transformed satData

                R = PolyFit.SRIM();
                d = PolyFit.Data();

                info = "      " + R.Row(0) + "    "
                       + "      " + String.Format("{0, 8:F3}", d[0]) + endl;
                Console.Write(info);
                info = "  R = " + R.Row(1) + "    "
                       + "  d = " + String.Format("{0, 8:F3}", d[1]) + endl;
                Console.Write(info);
                info = "      " + R.Row(2) + "    "
                       + "      " + String.Format("{0, 8:F3}", d[2]) + endl + endl;
                Console.Write(info);
            }

            // Solution of least squares system
            PolyFit.Solve(c);

            info = endl + "Adjusted polynomial coefficients" + endl + endl;
            for (i = 0; i < 3; i++)
            {
                info += "  c(" + i + ") = "
                        + String.Format("{0, 8:F3}", c[i]) + endl;
            }

            Console.WriteLine(info);
            Console.ReadKey();
        }