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);
}
