public override void Correct(EpochSatellite epochSatellite) { Time gpsTime = epochSatellite.RecevingTime; IEphemeris sat = epochSatellite.Ephemeris; SatelliteNumber prn = epochSatellite.Prn; //计算太阳位置方法 //XYZ sunPosition = epochSatellite.EpochInfo.DataSouceProvider.UniverseObjectProvider.GetSunPosition(gpsTime); //新的计算太阳位置方法 Time tutc = gpsTime.GpstToUtc(); //查找地球自转信息 Gnsser.Data.ErpItem erpv = null; if (DataSouceProvider.ErpDataService != null) { erpv = DataSouceProvider.ErpDataService.Get(tutc); } if (erpv == null) { erpv = ErpItem.Zero; } XYZ sunPosition = new XYZ(); DataSouceProvider.UniverseObjectProvider.GetSunPosition(gpsTime, erpv, ref sunPosition); //use L1 value IAntenna antenna = DataSouceProvider.AntennaDataSource.Get(prn.ToString(), gpsTime); if (antenna == null) { return; } string AntennaType = antenna.AntennaType; XYZ svPos = sat.XYZ; XYZ receiverPosition = epochSatellite.SiteInfo.EstimatedXyz; if (receiverPosition.Equals(XYZ.Zero)) { return; } bool cycleSlip = epochSatellite.IsUnstable; if (cycleSlip || !PhaseManager.Contains(prn)) //a cycle slip happend { PhaseManager[prn] = new SatVectorPhase(); } double windUpCorrection = GetSatPhaseWindUpCorectValue(prn, gpsTime, svPos, receiverPosition, sunPosition, AntennaType); //double windUpCorrection2 = GetSatPhaseWindUpCorectValue(satelliteType, gpsTime, svPos, receiverPosition, epochSatellite, sunPosition); this.Correction = (windUpCorrection); }
/// <summary> /// 计算天线缠绕改正,单位:弧度。 /// </summary> /// <param name="prn">卫星编号</param> /// <param name="time">时间</param> /// <param name="satPos">卫星位置</param> /// <param name="receiverPos">接收机位置</param> /// <param name="sunPos">太阳位置</param> /// <returns></returns> private double GetSatPhaseWindUpCorectValue(SatelliteNumber prn, Time time, XYZ satPos, XYZ receiverPos, XYZ sunPos, string AntennaType) { //Vector from SV to Sun center of mass XYZ gps_sun = sunPos - satPos; //Unitary vector from satellite to Earth mass center XYZ rk = (-1.0) * satPos.UnitVector(); //rj=rk * gps_sun, then make sure it is unitary XYZ rj = (rk.Cross(gps_sun)).UnitVector(); //Define ri: ri= rj * rk, then make sure it is unitary //Now, ri, rj, rk form a base in the satellite body reference frame, expressed in the ECEF reference frame XYZ ri = (rj.Cross(rk)).UnitVector(); // Get satellite rotation angle // Get vector from Earth mass center to receiver XYZ rxPos = new XYZ(receiverPos.X, receiverPos.Y, receiverPos.Z); // Compute unitary vector vector from satellite to RECEIVER XYZ rrho = (rxPos - satPos).UnitVector(); // Projection of "rk" vector to line of sight vector (rrho) double zk = rrho.Dot(rk); // Get a vector without components on rk (time.e., belonging // to ri, rj plane) XYZ dpp = (rrho - zk * rk); // Compute dpp components in ri, rj plane double xk = (dpp.Dot(ri)); double yk = (dpp.Dot(rj)); //Compute satellite rotation angle, in radians double alpha1 = Math.Atan2(yk, xk); // Get receiver rotation angle // Redefine rk: Unitary vector from Receiver to Earth mass center rk = (-1.0) * (rxPos.UnitVector()); // Let's define a NORTH unitary vector in the Up, East, North // (UEN) topocentric reference frame XYZ delta = new XYZ(0.0, 0.0, 1.0); // Rotate delta to XYZ reference frame GeoCoord nomNEU = Geo.Coordinates.CoordTransformer.XyzToGeoCoord(receiverPos); delta = XYZ.RotateY(delta, nomNEU.Lat); delta = XYZ.RotateZ(delta, -nomNEU.Lon); // Computation of reference trame unitary vectors for receiver // rj = rk x delta, and make it unitary rj = (rk.Cross(delta)).UnitVector(); // ri = rj x rk, and make it unitary ri = (rj.Cross(rk)).UnitVector(); // Projection of "rk" vector to line of sight vector (rrho) zk = rrho.Dot(rk); // Get a vector without components on rk (time.e., belonging // to ri, rj plane) dpp = rrho - zk * rk; // Compute dpp components in ri, rj plane xk = dpp.Dot(ri); yk = dpp.Dot(rj); // Compute receiver rotation angle, in radians double alpha2 = Math.Atan2(yk, xk); double wind_up = 0.0; // Find out if satellite belongs to block "IIR", because // satellites of block IIR have a 180 phase shift if (SatInfoService.GetBlock(prn, time) == "IIR") { wind_up = Math.PI;// 3.1415926535898;//PI } //if (AntennaType.Contains("IIR") && SatInfoService.GetBlock(prn, time) != "IIR") //{ // wind_up += 0; //} alpha1 = alpha1 + wind_up; SatVectorPhase satVecPhase = PhaseManager[prn]; double da1 = alpha1 - satVecPhase.PhaseOfSatellite; double da2 = (alpha2 - satVecPhase.PhaseOfReceiver); //double da1 = alpha1 - phase_satellite[satid].PreviousPhase; //double da2 = (alpha2 - phase_station[satid].PreviousPhase); // Let's avoid problems when passing from 359 to 0 degrees. double tmp1 = satVecPhase.PhaseOfSatellite; tmp1 += Math.Atan2(Math.Sin(da1), Math.Cos(da1)); satVecPhase.PhaseOfSatellite = tmp1; double tmp2 = satVecPhase.PhaseOfReceiver; tmp2 += Math.Atan2(Math.Sin(da2), Math.Cos(da2)); satVecPhase.PhaseOfReceiver = tmp2; // Compute wind up effect in radians wind_up = satVecPhase.PhaseOfSatellite - satVecPhase.PhaseOfReceiver; // Let's avoid problems when passing from 359 to 0 degrees. //PhaseData tmp1 = phase_satellite[satid]; //tmp1.PreviousPhase += Math.Atan2(Math.Sin(da1), Math.Cos(da1)); //phase_satellite[satid] = tmp1; //PhaseData tmp2 = phase_station[satid]; //tmp2.PreviousPhase += Math.Atan2(Math.Sin(da2), Math.Cos(da2)); //phase_station[satid] = tmp2; //// Compute wind up effect in radians //wind_up = phase_satellite[satid].PreviousPhase - // phase_station[satid].PreviousPhase; return(wind_up); }