Ejemplo n.º 1
0
        public GpstkOrbitUtil(EphemerisParam eph)
        {
            this.af0       = eph.ClockBias;
            this.af1       = eph.ClockDrift;
            this.af2       = eph.DriftRate;
            this.ctToc     = eph.Toe;//
            this.Cic       = eph.Cic;
            this.Crc       = eph.Crc;
            this.Crs       = eph.Crs;
            this.OMEGAdot  = eph.OmegaDot;
            this.e0        = eph.Eccentricity;//
            this.Cuc       = eph.Cuc;
            this.Cis       = eph.Cis;
            this.Cus       = eph.Cus;
            this.ctToe     = eph.Toe;
            this.Adot      = 0;//??
            this.A         = eph.SqrtA * eph.SqrtA;
            this.dn        = eph.DeltaN;
            this.M0        = eph.MeanAnomaly;
            this.ecc       = eph.Eccentricity;
            this.REL_CONST = eph.Eccentricity;
            this.PI        = CoordConsts.PI;
            this.eye0      = eph.Inclination;
            this.Omega     = eph.ArgumentOfPerigee;
            this.OMEGA0    = eph.LongOfAscension;
            this.dndot     = 0;//??
            this.idot      = eph.EyeDot;

            this.prn = eph.Prn;
        }
Ejemplo n.º 2
0
 public eph_t(EphemerisParam eph)
 {
     this.A   = eph.SqrtA * eph.SqrtA;
     this.toe = eph.Toe;
     // this.sat = eph.sat;
     this.M0   = eph.MeanAnomaly;
     this.deln = eph.DeltaN;
     this.e    = eph.Eccentricity;
     this.omg  = eph.ArgumentOfPerigee;
     this.cuc  = eph.Cuc;
     this.crs  = eph.Crs;
     this.cis  = eph.Cis;
     this.cus  = eph.Cus;
     this.cic  = eph.Cis;
     this.crc  = eph.Crc;
     this.OMG0 = eph.LongOfAscension;
     this.OMGd = eph.OmegaDot;
     this.toes = eph.Toe;
     this.toc  = eph.Toe;//??;
     this.f0   = eph.ClockBias;
     this.f1   = eph.ClockDrift;
     this.f2   = eph.DriftRate;
     this.sva  = eph.SVAccuracy;
     this.i0   = eph.Inclination;
     this.idot = eph.EyeDot;
 }
Ejemplo n.º 3
0
        public static double STD_BRDCCLK     = 30.0;            /* error of broadcast clock (m) */

        public static XYZ GetPos(double secOfWeek, EphemerisParam param)
        {
            double  [] rs = new double [3];
            double     dts = 0, var = 0;

            eph2pos(secOfWeek, new eph_t(param), rs, ref dts, ref var, param.Prn);
            return(XYZ.Parse(rs));
        }
Ejemplo n.º 4
0
        public static RmsedXYZ GetPosXYZ(EphemerisParam eph, double secOfWeek)
        {
            GpstkOrbitUtil o   = new GpstkOrbitUtil(eph);
            Xvt            xvt = o.svXvt(secOfWeek);

            XYZ xyzGpstk  = XYZ.Parse(xvt.x);
            XYZ velercity = XYZ.Parse(xvt.v);

            return(new RmsedXYZ(xyzGpstk, velercity));
        }
Ejemplo n.º 5
0
        public override bool Equals(object obj)
        {
            EphemerisParam p = obj as EphemerisParam;

            if (p == null)
            {
                return(false);
            }

            return(this.Prn.Equals(p.Prn) && this.Time.Equals(p.Time));
        }
Ejemplo n.º 6
0
        private static double KeplerEqForEccAnomaly1(EphemerisParam eph, double M)
        {
            int    n;
            double Ek, E1;

            for (n = 0, E1 = M, Ek = 0.0; Math.Abs(E1 - Ek) > 1e-14; n++)
            {
                Ek  = E1;
                E1 -= (E1 - eph.Eccentricity * Math.Sin(E1) - M) / (1.0 - eph.Eccentricity * Math.Cos(E1));
            }
            return(E1);
        }
Ejemplo n.º 7
0
        /// <summary>
        /// 获取卫星钟差
        /// </summary>
        /// <param name="record">星历参数</param>
        /// <param name="gpsTime">时间</param>
        /// <returns></returns>
        public static double GetClockOffset(EphemerisParam record, Time gpsTime)
        {
            double toc        = record.Time.SecondsOfWeek;
            double t          = gpsTime.SecondsOfWeek;
            double differTime = t - record.Toe;

            //GPS卫星以钟为周秒,考虑到一个星期的开始或结束
            if (differTime > 302400)
            {
                differTime -= 604800;
            }
            else if (differTime < -302400)
            {
                differTime += 604800;
            }

            return(record.GetOffset(differTime));
        }
Ejemplo n.º 8
0
        /// <summary>
        /// 根据轨道根数计算卫星位置。
        /// 核心计算程序。
        /// </summary>
        /// <param name="secOfWeek">GPS周秒</param>
        /// <param name="isGeosynchronous">是否是地球同步轨道卫星</param>
        /// <returns></returns>
        public static Geo.Coordinates.XYZ GetSatXyz(EphemerisParam eph, double gpsSecOfWeek, SatelliteType satelliteType = SatelliteType.G, bool isGeosynchronous = false)
        {
            double e = eph.Eccentricity;
            double A, en0, tk, en, M, E, sinE, cosE, f, u, cos2u, sin2u, duk, drk, dik;
            double uk, rk, eyek, cosEyek, xpk, ypk, omgk, cosOmgk, sinOmgk, xk, yk, zk;
            double secOfWeek = gpsSecOfWeek;

            if (satelliteType == SatelliteType.C)//如果是北斗,需要转换为北斗时间来计算。
            {
                secOfWeek = gpsSecOfWeek - 14;
            }

            //*** time since orbit reference epoch
            tk = Time.GetDifferSecondOfWeek(secOfWeek, eph.Toe);
            Geo.Referencing.IEllipsoid ellipsoid = GnssSystem.GetGnssSystem(satelliteType).Ellipsoid;

            double GM   = ellipsoid.GM;            // MU_GPS;
            double omge = ellipsoid.AngleVelocity; //  OMGE;

            //平均角速度
            A   = eph.SqrtA * eph.SqrtA;
            en0 = Math.Sqrt(GM) / (A * eph.SqrtA);

            //改正平均角度
            en = en0 + eph.DeltaN;

            //*** mean anomaly, M
            M = eph.MeanAnomaly + en * tk;

            //*** solve kepler's equation for eccentric anomaly, E
            E = KeplerEqForEccAnomaly(M, e);
            //var E0 = KeplerEqForEccAnomalyOld(M, e);
            //var E1 = KeplerEqForEccAnomaly1(M, e);
            //  double E1 = KeplerEqForEccAnomaly1(eph, M);

            sinE = Math.Sin(E);
            cosE = Math.Cos(E);

            //*** true anomaly, L
            f = Math.Atan2(Math.Sqrt(1.0 - e * e) * sinE, cosE - e);

            //*** argument of latitude, rightHandSide
            u = f + eph.ArgumentOfPerigee;

            cos2u = Math.Cos(u + u);
            sin2u = Math.Sin(u + u);

            //*** corrections to the arg. of lat., radius, and inclination

            duk = eph.Cuc * cos2u + eph.Cus * sin2u;
            drk = eph.Crc * cos2u + eph.Crs * sin2u;
            dik = eph.Cic * cos2u + eph.Cis * sin2u;

            //*** correct the arg. of lat., radius, and inclination

            uk   = u + duk;
            rk   = A * (1.0 - e * cosE) + drk;
            eyek = eph.Inclination + eph.EyeDot * tk + dik;

            //*** position in the orbital plane

            xpk = rk * Math.Cos(uk);
            ypk = rk * Math.Sin(uk);

            cosEyek = Math.Cos(eyek);
            //*** correct the longitude of the ascending node
            XYZ xyz = new XYZ();

            //同步轨道卫星,如北斗
            if (isGeosynchronous)
            {
                omgk    = eph.LongOfAscension + eph.OmegaDot * tk - omge * eph.Toe;
                cosOmgk = Math.Cos(omgk);
                sinOmgk = Math.Sin(omgk);
                //*** compute ecbf coordinates
                double xg = xpk * cosOmgk - ypk * sinOmgk * cosEyek;
                double yg = xpk * sinOmgk + ypk * cosOmgk * cosEyek;
                double zg = ypk * Math.Sin(eyek);

                double sino = Math.Sin(omge * tk);
                double coso = Math.Cos(omge * tk);

                double x = xg * coso + yg * sino * COS_5 + zg * sino * SIN_5;
                double y = -xg * sino + yg * coso * COS_5 + zg * coso * SIN_5;
                double z = -yg * SIN_5 + zg * COS_5;
                xyz = new XYZ(x, y, z);
            }
            else
            {
                omgk    = eph.LongOfAscension + (eph.OmegaDot - omge) * tk - omge * eph.Toe;
                cosOmgk = Math.Cos(omgk);
                sinOmgk = Math.Sin(omgk);
                //*** compute ecbf coordinates
                xk  = xpk * cosOmgk - ypk * sinOmgk * cosEyek;
                yk  = xpk * sinOmgk + ypk * cosOmgk * cosEyek;
                zk  = ypk * Math.Sin(eyek);
                xyz = new XYZ(xk, yk, zk);
            }

            return(xyz);

            //以下为测试比较
            XYZ xyz2 = RtkLibOrbitUtil.GetPos(secOfWeek, eph);
            //XYZ xyzGpstk = GetPosXYZ(eph, secOfWeek);
            // return xyzGpstk;
        }
Ejemplo n.º 9
0
        /// <summary>
        /// 计算位置。
        /// </summary>
        /// <param name="record"></param>
        /// <param name="gpstime">周秒</param>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        /// <param name="Z"></param>
        public static void GetSatPos(EphemerisParam record, double gpstime, out double X, out double Y, out double Z)
        {
            double mu = 3.986005e14;
            double n0, n;

            n0 = Math.Sqrt(mu) / (record.SqrtA * record.SqrtA * record.SqrtA);
            n  = n0 + record.DeltaN;

            //double t = GetGPSTime(clk.Year + 2000,
            //    clk.Month, clk.Day, clk.Hour, clk.Minute, clk.Second);
            double t = gpstime;// -0.075;

            double tk = t - record.Toe;

            if (tk > 302400)
            {
                tk -= 604800;
            }
            else if (tk < -302400)
            {
                tk += 604800;
            }

            double Mk = record.MeanAnomaly + n * tk;

            double Ek0, Ek;

            Ek0 = Mk;
            while (true)
            {
                Ek = Mk + record.Eccentricity * Math.Sin(Ek0);
                if (Math.Abs(Ek - Ek0) < 1e-8)
                {
                    break;
                }
                Ek0 = Ek;
            }

            //double fk = Math.Atan(Math.Sqrt(1 - clk.east * clk.east) * Math.Sin(Ek) / (Math.Cos(Ek) - clk.east));
            double fk = 2 * Math.Atan(Math.Sqrt(1 + record.Eccentricity) / Math.Sqrt(1 - record.Eccentricity) * Math.Tan(Ek / 2));

            double fik = fk + record.ArgumentOfPerigee;

            double deltau = record.Cuc * Math.Cos(2 * fik) + record.Cus * Math.Sin(2 * fik);
            double deltar = record.Crc * Math.Cos(2 * fik) + record.Crs * Math.Sin(2 * fik);
            double deltai = record.Cic * Math.Cos(2 * fik) + record.Cis * Math.Sin(2 * fik);

            double uk = fik + deltau;
            double rk = record.SqrtA * record.SqrtA * (1 - record.Eccentricity * Math.Cos(Ek)) + deltar;
            double ik = record.Inclination + deltai + record.EyeDot * tk;

            double xk = rk * Math.Cos(uk);
            double yk = rk * Math.Sin(uk);

            double omegae = 7.2921151467e-5;
            double omegak = record.LongOfAscension + (record.OmegaDot - omegae) * tk - omegae * record.Toe;

            //卫星在地心地固坐标系中的位置。
            X = xk * Math.Cos(omegak) - yk * Math.Cos(ik) * Math.Sin(omegak);
            Y = xk * Math.Sin(omegak) + yk * Math.Cos(ik) * Math.Cos(omegak);
            Z = yk * Math.Sin(ik);
        }
Ejemplo n.º 10
0
        static double RTOL_KEPLER = 1E-14;               /* relative tolerance for Kepler equation */

        public static Geo.Coordinates.XYZ GetSatXyz(EphemerisParam eph, double secOfWeek)
        {
            bool isGeosynchronous = eph.Prn.PRN <= 5 && eph.Prn.SatelliteType == SatelliteType.C;

            return(GetSatXyz(eph, secOfWeek, eph.Prn.SatelliteType, isGeosynchronous));
        }