/// <summary> /// 获取权值 /// </summary> /// <param name="satellite">测站卫星向量对象</param> /// <returns></returns> public double GetWeight(EpochSatellite satellite) { //避免低高度角卫星的权太小,czs,2018.09.27, hmx double sinE = 0.5 + 0.5 * Math.Sin(satellite.Polar.Elevation * AngularConvert.DegToRadMultiplier); //距离始终为1,错误:2015.05.07,cy,发现。待修改,先替换为 satellite.ApproxXyzVector.Length //2015.05.10,czs, 已经找到原因,是因为后续验证错误,而源代码无误,已修改 double range = satellite.EstmatedVector.Length;// satellite.Polar.Range; if (range == 0) { range = 1; } GnssSystem sys = GnssSystem.GetGnssSystem(satellite.Prn.SatelliteType); double factor = range / sys.ApproxRadius; double Weight = Math.Pow(sinE / factor, 2.0); //防止测站坐标为 0 的情况 if (Geo.Utils.DoubleUtil.IsValid(Weight)) { return(Weight); } return(1); }
// Compute satellite relativity correction (sec) at the given time // throw Invalid Request if the required satData has not been stored. double svRelativity(double t) { Geo.Referencing.IEllipsoid ell = GnssSystem.GetGnssSystem(prn.SatelliteType).Ellipsoid; double twoPI = 2.0 * PI; double sqrtgm = SQRT(ell.GM); double elapte = t - ctToe; // Compute A at time of interest double Ak = A + Adot * elapte; // LNAV: Adot==0 //double dnA = dn + 0.5*dndot*elapte; // LNAV: dndot==0 double Ahalf = SQRT(A); double amm = (sqrtgm / (A * Ahalf)) + dn; // Eqn specifies A0 not Ak double meana, F, G, delea; meana = M0 + elapte * amm; meana = fmod(meana, twoPI); double ea = meana + ecc * sin(meana); int loop_cnt = 1; do { F = meana - (ea - ecc * sin(ea)); G = 1.0 - ecc * cos(ea); delea = F / G; ea = ea + delea; loop_cnt++; } while ((ABS(delea) > 1.0e-11) && (loop_cnt <= 20)); return(REL_CONST * ecc * SQRT(Ak) * sin(ea)); }
/// <summary> /// 获取已知的频率带宽。 /// </summary> /// <param name="satNumber">卫星编号, GLONASS 频分多址</param> /// <param name="type">类型</param> /// <param name="rinexNum">编号,1、2、3 分别代表各系统的频率如GPS的L1、L2、L5</param> /// <param name="time">时间,历元,GLONASS或频分多址需要</param> /// <returns></returns> public static Frequence GetFrequenceBand(GnssType type, int rinexNum = 1, int satNumber = -1, Geo.Times.Time time = default(Geo.Times.Time)) { var satType = GnssSystem.GetSatelliteType(type); var freqType = GetFrequenceType(satType, rinexNum); return(GetFrequenceBand(type, freqType, satNumber, time)); }
/// <summary> /// 观测值精度 /// </summary> /// <param name="satellite"></param> /// <returns></returns> public double GetInverseWeightOfRange(EpochSatellite satellite) { var prn = satellite.Prn; // double sinE = 0.5 + 0.5 * Math.Sin(satellite.GeoElevation * AngularConvert.DegToRadMultiplier); double sinE = 0.01 + 0.99 * Math.Sin(satellite.GeoElevation * AngularConvert.DegToRadMultiplier); //防止测站坐标为 0 的情况 if (!Geo.Utils.DoubleUtil.IsValid(sinE)) { sinE = 1.0; } if (IsSpeedTopModel) { return(sinE); } //不同系统的权值 double stdDevFactorOfSystem = 1;// 默认 1m 精度 if (this.SystemStdDevFactors.ContainsKey(prn.SatelliteType)) { stdDevFactorOfSystem = this.SystemStdDevFactors[prn.SatelliteType]; } //不同卫星的权值 double stdDevFactorOfSat = 1;// 默认 1m 精度 if (this.SatelliteStdDevs.ContainsKey(prn)) { stdDevFactorOfSat = this.SatelliteStdDevs[prn]; } else { GnssSystem sys = GnssSystem.GetGnssSystem(satellite.Prn.SatelliteType); double factor = 1.0 * satellite.AvailablePseudoRange.Value / sys.ApproxRadius; //如果是静止轨道卫星,此值偏大 stdDevFactorOfSat = factor; } double stdDev = stdDevFactorOfSat * stdDevFactorOfSystem / sinE; //标准差 double inverseWeight = stdDev * stdDev; //方差 return(inverseWeight); }
/// <summary> /// 获取频率带宽。 /// </summary> /// <param name="type"></param> /// <param name="rinexNum"></param> /// <param name="satNumber"></param> /// <param name="time">时间,历元</param> /// <returns></returns> public static Frequence GetFrequence(SatelliteType type, int rinexNum, int satNumber = -1, Geo.Times.Time time = default(Geo.Times.Time)) { return(ObsCodeConvert.GetFrequenceBand(GnssSystem.GetGnssType(type), rinexNum, satNumber, time)); }
/// <summary> /// 获取第3频率,推荐方法 /// </summary> /// <param name="prn">卫星编号</param> /// <param name="time">时间,GLONASS需要</param> /// <returns></returns> public static Frequence GetFrequenceC(SatelliteNumber prn, Geo.Times.Time time = default(Geo.Times.Time)) { return(ObsCodeConvert.GetFrequenceBand(GnssSystem.GetGnssType(prn.SatelliteType), FrequenceType.C, prn.PRN, time)); }
/// <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; }
// Compute satellite position at the given time. public Xvt svXvt(double t) { // if(!dataLoadedFlag) // GPSTK_THROW(InvalidRequest("Data not loaded")); Xvt sv = new Xvt(); double E; // eccentric anomaly // double delea; // delta eccentric anomaly during iteration double tk; // elapsed time since Toe //double elaptc; // elapsed time since Toc //double dtc,dtr; double q, sinE, cosE; double GSTA, GCTA; double en; double M; // mean anomaly // double F; double G; // temporary real variables double u, u_u, cos2u, sin2u, du, dr, di, U, R, f, eyek; double omgk, cosu, sinu, xip, yip, cosOmgk, sinOmgk, cosEyek, sinEyek; double xef, yef, zef, dek, dlk, div, domk, duv, drv; double dxp, dyp, vxef, vyef, vzef; Geo.Referencing.IEllipsoid ell = GnssSystem.GetGnssSystem(prn.SatelliteType).Ellipsoid; double sqrtGM = SQRT(ell.GM); double twoPI = 2.0e0 * PI; double e; // eccentricity double eyeDot; // dt inclination double sqrtA = SQRT(A); // A is semi-major axis of orbit double ToeSOW = this.ctToe; // GPSWeekSecond(ctToe).sow; // SOW is time-system-independent e = ecc; eyeDot = idot; // Compute time since ephemeris & clock epochs tk = Time.GetDifferSecondOfWeek(t, ctToe);// t - ctToe; // Compute A at time of interest (LNAV: Adot==0) double Ak = A + Adot * tk; // Compute mean motion (LNAV: dndot==0) double dnA = dn + 0.5 * dndot * tk; en = (sqrtGM / (A * sqrtA)) + dnA; // Eqn specifies A0, not Ak // In-plane angles // meana - Mean anomaly // ea - Eccentric anomaly // truea - True anomaly M = M0 + tk * en; M = fmod(M, twoPI); E = OrbitUtil.KeplerEqForEccAnomaly(M, e); //E = M + e * sin(M); //int loop_cnt = 1; //do //{ // F = M - (E - e * sin(E)); // G = 1.0 - e * cos(E); // delea = F / G; // E = E + delea; // loop_cnt++; //} while ((fabs(delea) > 1.0e-11) && (loop_cnt <= 20)); // Compute clock corrections sv.relcorr = svRelativity(t); sv.clkbias = svClockBias(t); sv.clkdrift = svClockDrift(t); // sv.frame = ReferenceFrame.WGS84; // Compute true anomaly sinE = sin(E); cosE = cos(E); q = SQRT(1.0 - e * e); G = 1.0 - e * cosE; // G*SIN(TA) AND G*COS(TA) GSTA = q * sinE; GCTA = cosE - e; // True anomaly f = atan2(GSTA, GCTA); // Argument of lat and correction terms (2nd harmonic) u = f + Omega; u_u = 2.0 * u; cos2u = cos(u_u); sin2u = sin(u_u); du = cos2u * Cuc + sin2u * Cus; dr = cos2u * Crc + sin2u * Crs; di = cos2u * Cic + sin2u * Cis; // U = updated argument of lat, R = radius, AINC = inclination U = u + du; R = Ak * G + dr; eyek = eye0 + eyeDot * tk + di; // Longitude of ascending node (ANLON) omgk = OMEGA0 + (OMEGAdot - ell.AngleVelocity) * tk - ell.AngleVelocity * ToeSOW; // In plane location cosu = cos(U); sinu = sin(U); xip = R * cosu; yip = R * sinu; // Angles for rotation to earth fixed cosOmgk = cos(omgk); sinOmgk = sin(omgk); cosEyek = cos(eyek); sinEyek = sin(eyek); // Earth fixed coordinates in meters xef = xip * cosOmgk - yip * cosEyek * sinOmgk; yef = xip * sinOmgk + yip * cosEyek * cosOmgk; zef = yip * sinEyek; sv.x[0] = xef; sv.x[1] = yef; sv.x[2] = zef; // Compute velocity of rotation coordinates dek = en * Ak / R; dlk = sqrtA * q * sqrtGM / (R * R); div = eyeDot - 2.0e0 * dlk * (Cic * sin2u - Cis * cos2u); domk = OMEGAdot - ell.AngleVelocity; duv = dlk * (1.0 + 2.0 * (Cus * cos2u - Cuc * sin2u)); drv = Ak * e * dek * sinE - 2.0 * dlk * (Crc * sin2u - Crs * cos2u); dxp = drv * cosu - R * sinu * duv; dyp = drv * sinu + R * cosu * duv; // Calculate velocities vxef = dxp * cosOmgk - xip * sinOmgk * domk - dyp * cosEyek * sinOmgk + yip * (sinEyek * sinOmgk * div - cosEyek * cosOmgk * domk); vyef = dxp * sinOmgk + xip * cosOmgk * domk + dyp * cosEyek * cosOmgk - yip * (sinEyek * cosOmgk * div + cosEyek * sinOmgk * domk); vzef = dyp * sinEyek + yip * cosEyek * div; // Move results into output variables sv.v[0] = vxef; sv.v[1] = vyef; sv.v[2] = vzef; return(sv); }