/// <summary>
/// 默认的卫星天线相位改正。
/// </summary>
/// <param name="satelliteType"></param>
/// <param name="time"></param>
/// <param name="satPos"></param>
/// <param name="sunPosition"></param>
/// <returns></returns>
public static double DefautSatPhaseCorrector(SatelliteNumber prn, Time time, XYZ satPos, XYZ ReceiverPosition, XYZ sunPosition, SatInfoFile satData)
{
// Unitary vector from satellite to Earth mass center (ECEF)
XYZ satToEarthUnit = (-1.0) * satPos.UnitVector();
// Unitary vector from Earth mass center to Sun (ECEF)
XYZ earthToSunUnit = sunPosition.UnitVector();
// rj = rk x ri: Rotation axis of solar panels (ECEF)
XYZ rj = satToEarthUnit.Cross(earthToSunUnit);
// Redefine ri: ri = rj x rk (ECEF)
earthToSunUnit = rj.Cross(satToEarthUnit);
// Let's funcKeyToDouble ri to an unitary vector. (ECEF)
earthToSunUnit = earthToSunUnit.UnitVector();
// Get vector from Earth mass center to receiver
XYZ receiverPos = ReceiverPosition;
// Compute unitary vector vector from satellite to RECEIVER
XYZ satToReceverUnit = (receiverPos - satPos).UnitVector();
// When not using Antex information, if satellite belongs to block
// "IIR" its correction is 0.0, else it will depend on satellite model.
// This variable that will hold the correction, 0.0 by default
double svPCcorr = 0.0;
// If no Antex information is given, or if phase center information
// uses a relative model, then use a simpler, older approach
// Please note that in this case all GLONASS satellite are
// considered as having phase center at (0.0, 0.0, 0.0). The former
// is not true for 'GLONASS-M' satellites (-0.545, 0.0, 0.0 ), but
// currently there is no simple way to take this into account.
// For satellites II and IIA:
if ((satData.GetBlock(prn, time) == "II") || (satData.GetBlock(prn, time) == "IIA"))
{
//First, build satellite antenna vector for models II/IIA
XYZ svAntenna = 0.279 * earthToSunUnit + 1.023 * satToEarthUnit;
// Projection of "svAntenna" vector to line of sight vector rrho
svPCcorr = (satToReceverUnit.Dot(svAntenna));
}
else
{
// For satellites belonging to block "I"
if ((satData.GetBlock(prn, time) == "I"))
{
// First, build satellite antenna vector for model I
XYZ svAntenna = (0.210 * earthToSunUnit + 0.854 * satToEarthUnit);
// Projection of "svAntenna" to line of sight vector (rrho)
svPCcorr = (satToReceverUnit.Dot(svAntenna));
}
}
return(svPCcorr);
}
/// <summary>
/// 通过两个时刻的位置，求轨道参数。
/// </summary>
/// <param name="posA"></param>
/// <param name="posB"></param>
/// <returns></returns>
public static KeplerEphemerisParam GetKeplerEphemerisParamFromTwoPos(XYZ posA, XYZ posB, double deltaTime)
{
throw new NotImplementedException("通过两个时刻的位置，求轨道参数，算法未实现！2016.06.06.16");
//2.6.1.1 计算轨道倾角i和升交点赤经Ω
XYZ hXyz = posA.Cross(posB);
double hLen = hXyz.Length;
double r1 = posA.Length;
double r2 = posB.Length;
double vv = posB.Dot(posB);
//计算轨道倾角
double ABLen = Math.Sqrt(hXyz.X * hXyz.X + hXyz.Y * hXyz.Y);
double i = Math.Atan2(ABLen, hXyz.Z);
// double i = Math.Acos(hXyz.Z / hLen); //表示在Z轴上的投影，与上结果计算一致
//double includedAngle = hXyz.GetIncludedAngle(new XYZ(0, 0, 1));//轨道倾角为两个平面法向量的夹角
//计算升交点赤经
double Ω = Math.Atan2(hXyz.X, -hXyz.Y);
//用面积比法求半通径
double sinf2minusf1 = hXyz.Z / (r1 * r2 * Math.Cos(i));
return(null);
//KeplerEphemerisParam param = new KeplerEphemerisParam
//{
// ArgumentOfPerigee = ww,
// Eccentricity = e,
// Inclination = i,
// LongOfAscension = Ω,
// MeanAnomaly = M,
// SqrtA = sqrtA
//};
//return param;
}
public void Dot()
{
var p1 = new XYZ(1, 2, 3);
Assert.AreEqual(14, p1.Dot(new XYZ(1, 2, 3)));
Assert.AreEqual(0, p1.DotCross(new XYZ(4, 5, 6), new XYZ(4, 5, 6)));
}
public OrbitParam CaculateOrbitParam(XYZ pos, XYZ dot, double time)
{
double u = 3.986005e14;
//double sigma = 1e-6;
XYZ cross = pos.Cross(dot);
double A = cross.X;
double B = cross.Y;
double C = cross.Z;
double sqrtAB = Math.Sqrt(A * A + B * B);
//
//角动量公式,可推出轨道倾角i
double i = Math.Atan2(sqrtAB, C); //轨道倾角
double bigOmiga = Math.Atan2(-A, B); //升点赤经 的值
double r0 = pos.Length;
double v0_2 = dot.Length * dot.Length;
//由能量方程,可推出半长轴a的值
double a = 1.0 / (2 / r0 - v0_2 / u); //轨道长半径
double n = Math.Sqrt(u / a / a / a); //周期
double sqrta = Math.Sqrt(a);
double sqrtu = Math.Sqrt(u);
double eSinE = pos.Dot(dot) / sqrta / sqrtu;
double eCosE = 1 - r0 / a;
double e = Math.Sqrt(Math.Pow(eSinE, 2.0) + Math.Pow(eCosE, 2.0));//偏心率
double E = Math.Atan2(eSinE, eCosE);
double t = time;
//平近点角M与偏近点角E
//迭代
double M = E - eSinE;//平近点角
double tao = t - M / n;
//求出 E 么，偏近点角。
double tan2f = Math.Sqrt((1 + e) / (1 - e)) * Math.Tan(E / 2.0);
double f = Math.Atan(tan2f) * 2;
double smallOmiga = Math.Atan2(pos.Z / Math.Sin(i), pos.X * Math.Cos(bigOmiga) + pos.Y * Math.Sin(bigOmiga)) - f;
return(new OrbitParam()
{
Eccentricity = e,
LongOfAscension = bigOmiga,
SemiMajor = a,
EpochOfPerigee = tao,
Inclination = i,
MeanAnomaly = M,
ArgumentOfPerigee = smallOmiga
});
}
/// <summary>
/// 计算开普勒根数
/// </summary>
/// <param name="pos"></param>
/// <param name="velocity"></param>
/// <returns></returns>
public static KeplerEphemerisParam GetKeplerEphemerisParam(XYZ pos, XYZ velocity)
{
//2.6.1.1 计算轨道倾角i和升交点赤经Ω
XYZ hXyz = pos.Cross(velocity);
double hLen = hXyz.Length;
double r = pos.Length;
double vv = velocity.Dot(velocity);
//计算轨道倾角
//double ABLen = Math.Sqrt(hXyz.X * hXyz.X + hXyz.Y * hXyz.Y);
//double i = Math.Atan2(ABLen, hXyz.Z);
double i = Math.Acos(hXyz.Z / hLen); //表示在Z轴上的投影，与上结果计算一致
//double includedAngle = hXyz.GetIncludedAngle(new XYZ(0, 0, 1));//轨道倾角为两个平面法向量的夹角
//计算升交点赤经
double Ω = Math.Atan2(hXyz.X, -hXyz.Y);
//2.6.1.2 计算轨道长半轴a，
double a = 1 / (2 / r - vv / GM); //由活力公式计算
double sqrtA = Math.Sqrt(a);
double n = Math.Sqrt(GM) / (sqrtA * sqrtA * sqrtA); //平运动角速度n
//离心率e和平近点交M
double rdotv = pos.Dot(velocity);
double eCosE = 1 - r / a;
double eSinE = rdotv / Math.Sqrt(GM * a);
double e = Math.Sqrt(eSinE * eSinE + eCosE * eCosE); //离心率e
double E = Math.Atan2(eSinE, eCosE); //偏近点角E
double M = E - eSinE; //平近点角
//2.6.1.3 计算近升角距ω
XYZ eXyz = velocity.Cross(hXyz) / GM - pos.UnitVector();
double ww = Math.Atan2(eXyz.Z, (eXyz.Y * Math.Sin(Ω) + eXyz.X * Math.Cos(Ω)) * Math.Sin(i));
double u = Math.Atan2(pos.Z, (pos.Y * Math.Sin(Ω) + pos.X * Math.Cos(Ω)) * Math.Sin(i));
double ff = u - ww;//真近点角
KeplerEphemerisParam param = new KeplerEphemerisParam
{
ArgumentOfPerigee = ww,
Eccentricity = e,
Inclination = i,
LongOfAscension = Ω,
MeanAnomaly = M,
SqrtA = sqrtA
};
return(param);
}
/// <summary>
/// 改正
/// </summary>
/// <param name="epochSatellite"></param>
public override void Correct(EpochSatellite epochSatellite)
{
Dictionary <RinexSatFrequency, double> correction = new Dictionary <RinexSatFrequency, double>();
IEphemeris sat = epochSatellite.Ephemeris;
XYZ satPos = sat.XYZ;
XYZ receiverPosition = epochSatellite.SiteInfo.EstimatedXyz;
XYZ ray = satPos - receiverPosition;
GeoCoord rcvGeoCoord = epochSatellite.SiteInfo.ApproxGeoCoord;
int i = 0;
List <RinexSatFrequency> frequences = epochSatellite.RinexSatFrequences;
foreach (var item in frequences)
{
XYZ dant1 = GetSatAntOff(epochSatellite.Prn, item, epochSatellite.Ephemeris, epochSatellite.Ephemeris.Time);
//Compute vector station-satellite, in ECEF
//Rotate vector ray to UEN reference frame
//此处修改为 NEU 坐标系。
//NEU rayNeu = CoordTransformer.XyzToNeu(ray, rcvGeoCoord, AngleUnit.Degree);
//double rangeCorretion = CoordUtil.GetDirectionLength(dant1, epochSatellite.Polar);
//ray = XYZ.RotateZ(ray, lon);
//ray = XYZ.RotateY(ray, -lat);
//Convert ray to an unitary vector
// XYZ xyzNeu = new XYZ(rayNeu.N, rayNeu.E, rayNeu.U);
//NEU unit = rayNeu.UnitNeuVector();
//计算沿着射线方向的改正数。Compute corrections = displacement vectors components along ray direction.
XYZ unit2 = ray.UnitVector();
double range2 = dant1.Dot(unit2);
//double correctForL = dant1.Dot(unit);
//double rang = dant1.Dot(unit);
if (range2 != 0)
{
correction.Add(item, range2);
}
i++;
}
this.Correction = (correction);
}
public void ValueType_XYZ()
{
var p1 = new XYZ(1, 2, 3);
var p2 = new XYZ(4, 5, 6);
Assert.AreEqual(14, p1.SquareModulus());
Assert.AreEqual(Math.Sqrt(14), p1.Modulus());
Assert.IsTrue(p1.IsEqual(p2, 3));
Assert.IsFalse(p1.IsEqual(p2, 2.99));
p2 = p1;
p2.Add(new XYZ(1, 2, 3));
Assert.AreEqual(new XYZ(2, 4, 6), p2);
Assert.AreEqual(new XYZ(2, 4, 6), p1.Added(new XYZ(1, 2, 3)));
p2 += new XYZ(1, 2, 3);
Assert.AreEqual(new XYZ(3, 6, 9), p2);
p2 = new XYZ(1, 2, 3);
p2.Cross(new XYZ(3, 2, 1));
Assert.AreEqual(new XYZ(-4, 8, -4), p2);
Assert.AreEqual(new XYZ(-4, 8, -4), p1.Crossed(new XYZ(3, 2, 1)));
Assert.AreEqual(Math.Sqrt(96), p1.CrossMagnitude(new XYZ(3, 2, 1)));
Assert.AreEqual(96, p1.CrossSquareMagnitude(new XYZ(3, 2, 1)));
p2 = new XYZ(1, 2, 3);
p2.CrossCross(new XYZ(1, 2, 3), new XYZ(4, 5, 6));
Assert.AreEqual(new XYZ(-24, -6, 12), p2);
Assert.AreEqual(new XYZ(-24, -6, 12), p1.CrossCrossed(new XYZ(1, 2, 3), new XYZ(4, 5, 6)));
p2 = new XYZ(1, 2, 3);
p2.Divide(2);
Assert.AreEqual(new XYZ(0.5, 1, 1.5), p2);
Assert.AreEqual(new XYZ(0.5, 1, 1.5), p1.Divided(2));
Assert.AreEqual(14, p1.Dot(new XYZ(1, 2, 3)));
Assert.AreEqual(0, p1.DotCross(new XYZ(4, 5, 6), new XYZ(4, 5, 6)));
p2 = new XYZ(1, 2, 3);
p2.Multiply(2);
Assert.AreEqual(new XYZ(2, 4, 6), p2);
Assert.AreEqual(new XYZ(2, 4, 6), p1.Multiplied(2));
Assert.AreEqual(new XYZ(2, 4, 6), p1 * 2);
p2 = new XYZ(1, 2, 3);
p2.Multiply(new XYZ(1, 2, 3));
Assert.AreEqual(new XYZ(1, 4, 9), p2);
Assert.AreEqual(new XYZ(1, 4, 9), p1.Multiplied(new XYZ(1, 2, 3)));
Assert.AreEqual(new XYZ(1, 4, 9), p1 * new XYZ(1, 2, 3));
Mat m1 = new Mat();
m1.SetRotation(Dir.DZ.Coord, Math.PI / 2);
p2 = new XYZ(4, 5, 6);
Assert.AreEqual("-5,4,6", p2.Multiplied(m1).ToString());
Assert.AreEqual("-5,4,6", (p2 * m1).ToString());
p2.Multiply(m1);
Assert.AreEqual("-5,4,6", p2.ToString());
p2 = new XYZ(1, 2, 3);
p2.Normalize();
Assert.IsTrue(p2.IsEqual(new XYZ(0.26726, 0.53452, 0.80178), 0.00001));
Assert.IsTrue(p1.Normalized().IsEqual(new XYZ(0.26726, 0.53452, 0.80178), 0.00001));
p2 = new XYZ(1, 2, 3);
p2.Reverse();
Assert.AreEqual(new XYZ(-1, -2, -3), p2);
Assert.AreEqual(new XYZ(-1, -2, -3), p1.Reversed());
p2 = new XYZ(1, 2, 3);
p2.Subtract(new XYZ(3, 2, 1));
Assert.AreEqual(new XYZ(-2, 0, 2), p2);
Assert.AreEqual(new XYZ(-2, 0, 2), p1.Subtracted(new XYZ(3, 2, 1)));
Assert.AreEqual(new XYZ(-2, 0, 2), p1 - new XYZ(3, 2, 1));
p2.SetLinearForm(new XYZ(1, 2, 3), new XYZ(4, 5, 6));
Assert.AreEqual(new XYZ(5, 7, 9), p2);
p2.SetLinearForm(2, new XYZ(1, 2, 3), new XYZ(4, 5, 6));
Assert.AreEqual(new XYZ(6, 9, 12), p2);
p2.SetLinearForm(2, new XYZ(1, 2, 3), 3, new XYZ(4, 5, 6));
Assert.AreEqual(new XYZ(14, 19, 24), p2);
p2.SetLinearForm(2, new XYZ(1, 2, 3), 3, new XYZ(4, 5, 6), new XYZ(7, 8, 9));
Assert.AreEqual(new XYZ(21, 27, 33), p2);
p2.SetLinearForm(2, new XYZ(1, 2, 3), 3, new XYZ(4, 5, 6), 4, new XYZ(7, 8, 9));
Assert.AreEqual(new XYZ(42, 51, 60), p2);
p2.SetLinearForm(2, new XYZ(1, 2, 3), 3, new XYZ(4, 5, 6), 4, new XYZ(7, 8, 9), new XYZ(10, 11, 12));
Assert.AreEqual(new XYZ(52, 62, 72), p2);
//TestContext.WriteLine(string.Format(CultureInfo.InvariantCulture, "{0},{1},{2}", gp2.x, gp2.y, gp2.z));
}
/// <summary>
/// 根据卫星位置和测站位置计算其在站星坐标系中的位置。
/// Returns the topo-centric (azimuth, elevation, etc.) coordinates for
/// a target object described by the given ECI coordinates.
/// </summary>
/// <param name="satPos">The ECI coordinates of the target object.</param>
/// <param name="siteCoord">The ECI coordinates of the 观测站</param>
/// <returns>The look angle to the target object.</returns>
public static TopoCoord GetSatTopoCoord(TimedMotionState satPos, GeoCoord siteCoord)
{
// Calculate the ECI coordinates for this Site object at the time of interest.
Julian date = satPos.Date;
MotionState sitePos = OrbitUtils.GetMovingState(siteCoord, date);
XYZ vecV = satPos.Velocity - sitePos.Velocity;
XYZ vecP = satPos.Position - sitePos.Position;
// The site's Local Mean Sidereal Time at the time of interest.
double thetaRad = date.GetLocalMeanSiderealTime(siteCoord.Lon, false);
double sinLat = Math.Sin(siteCoord.Lat);
double cosLat = Math.Cos(siteCoord.Lat);
double sinTheta = Math.Sin(thetaRad);
double cosTheta = Math.Cos(thetaRad);
double top_s = sinLat * cosTheta * vecP.X +
sinLat * sinTheta * vecP.Y -
cosLat * vecP.Z;
double top_e = -sinTheta * vecP.X +
cosTheta * vecP.Y;
double top_z = cosLat * cosTheta * vecP.X +
cosLat * sinTheta * vecP.Y +
sinLat * vecP.Z;
double az = Math.Atan(-top_e / top_s);
if (top_s > 0.0)
{
az += OrbitConsts.PI;
}
if (az < 0.0)
{
az += 2.0 * OrbitConsts.PI;
}
double el = Math.Asin(top_z / vecP.Radius());
double rate = vecP.Dot(vecV) / vecP.Radius();
TopoCoord topo = new TopoCoord(
az, // azimuth, radians
el, // elevation, radians
vecP.Radius(), // range
rate // rate, per sec
);
#if WANT_ATMOSPHERIC_CORRECTION
// Elevation correction for atmospheric refraction.
// Reference: Astronomical Algorithms by Jean Meeus, pp. 101-104
// Note: Correction is meaningless when apparent elevation is below horizon
topo.Elevation += AngularConvert.DegToRad((1.02 /
Math.Tan(AngularConvert.DegToRad(AngularConvert.RadToDeg(el) + 10.3 /
(AngularConvert.RadToDeg(el) + 5.11)))) / 60.0);
if (topo.Elevation < 0.0)
{
topo.Elevation = el; // Reset to true elevation
}
if (topo.Elevation > (Consts.PI / 2.0))
{
topo.Elevation = (Consts.PI / 2.0);
}
#endif
return(topo);
}
/// <summary>
/// 卫星天线相位中心改正，参照GPSTK模块
/// </summary>
/// <param name="satelliteType"></param>
/// <param name="satPos"></param>
/// <param name="ReceiverPosition"></param>
/// <param name="sunPosition"></param>
/// <param name="svPCcorr"></param>
/// <param name="antenna"></param>
/// <returns></returns>
private static double GetPhaseCorrection(SatelliteNumber prn, XYZ satPos, XYZ ReceiverPosition, XYZ sunPosition, double svPCcorr, IAntenna antenna)
{
// Unitary vector from satellite to Earth mass center (ECEF)
XYZ satToEarthUnit = (-1.0) * satPos.UnitVector();
// Unitary vector from Earth mass center to Sun (ECEF)
XYZ earthToSunUnit = sunPosition.UnitVector();
// rj = rk x ri: Rotation axis of solar panels (ECEF)
XYZ rj = satToEarthUnit.Cross(earthToSunUnit);
// Redefine ri: ri = rj x rk (ECEF)
earthToSunUnit = rj.Cross(satToEarthUnit);
// Let's funcKeyToDouble ri to an unitary vector. (ECEF)
earthToSunUnit = earthToSunUnit.UnitVector();
// Get vector from Earth mass center to receiver
XYZ receiverPos = ReceiverPosition;
// Compute unitary vector vector from satellite to RECEIVER
XYZ satToReceverUnit = (receiverPos - satPos).UnitVector();
// When not using Antex information, if satellite belongs to block
// "IIR" its correction is 0.0, else it will depend on satellite model.
// We will need the elevation, in degrees. It is found using
// dot product and the corresponding unitary angles
double cosa = satToReceverUnit.Dot(satToEarthUnit);
cosa = cosa < -1.0 ? -1.0 : (cosa > 1.0 ? 1.0 : cosa);
double nadir = Math.Acos(cosa) * CoordConsts.RadToDegMultiplier;
if (!DoubleUtil.IsValid(nadir))
{
return(0);
}
// The nadir angle should always smaller than 14.0 deg,
// but some times it's a bit bigger than 14.0 deg, we
// force it to 14.0 deg to stop throwing an exception.
// The Reference is available at:
// http://igscb.jpl.nasa.gov/igscb/resource/pubs/02_ott/session_8.pdf
nadir = (nadir > 14) ? 14.0 : nadir;
double elev = 90.0 - nadir;
// Get antenna eccentricity for frequency "G01" (L1), in
// satellite reference system.
// NOTE: It is NOT in ECEF, it is in UEN!!!
RinexSatFrequency freq = new RinexSatFrequency(prn, 1);
// NEU satAnt = antenna.GetAntennaEccentricity(AntennaFrequency.G01);
NEU satAnt = antenna.GetPcoValue(freq);
if (satAnt.Equals(NEU.Zero))
{
return(0);
}
//Now, get the phase center variation.
NEU var = new NEU(0, 0, antenna.GetPcvValue(freq, elev));
// We must substract them
satAnt = satAnt - var;
// Change to ECEF
// 原 satAnt t is in UEN!!!,本satAnt为NEU，分量相对应
// satAnt[0] = U
// Triple svAntenna( satAnt[2]*ri + satAnt[1]*rj + satAnt[0]*rk );
// XYZ svAntenna = satAnt.N * earthToSunUnit + satAnt.E * rj + satAnt.U * satToEarthUnit;
XYZ svAntenna = -(var.N * earthToSunUnit + var.E * rj + var.U * satToEarthUnit);
// Projection of "svAntenna" vector to line of sight vector rrho
svPCcorr = (satToReceverUnit.Dot(svAntenna));
return(svPCcorr);
}
/// <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);
}
/// <summary>
/// 天线缠绕改正 方法2 参考RTKLIB
/// 李林阳添加 2015.01.01
/// </summary>
/// <param name="prn"></param>
/// <param name="time"></param>
/// <param name="satPos"></param>
/// <param name="receiverPos"></param>
/// <param name="epochSatellite"></param>
/// <param name="sunPos"></param>
/// <returns></returns>
private double GetSatPhaseWindUpCorectValue(SatelliteNumber prn, Time time, XYZ satPos, XYZ receiverPos, EpochSatellite epochSatellite, XYZ sunPos)
{
#region
XYZ rxPos = new XYZ(receiverPos.X, receiverPos.Y, receiverPos.Z);
//李林阳加天线相位缠绕
/* unit vector satellite to receiver */
XYZ ek = (rxPos - satPos).UnitVector();; //接收机位置-卫星位置,单位化
// for (time=0;time<3;time++) r[time]=rr[time]-rs[time];
//if (!normv3(r, ek)) return; //单位化
/* unit vectors of satellite antenna */
//for (time = 0; time < 3; time++) r[time] = -rs[time];
//if (!normv3(r, ezs)) return;
XYZ ezs = (-1.0 * satPos).UnitVector();//卫星位置的单位向量
//for (time = 0; time < 3; time++) r[time] = rsun[time] - rs[time];
//if (!normv3(r, ess)) return;
XYZ ess = (sunPos - satPos).UnitVector();
//cross3(ezs, ess, r);
//if (!normv3(r, eys)) return;
XYZ eys = ezs.Cross(ess);
eys = eys.UnitVector();
//cross3(eys, ezs, exs);
XYZ exs = eys.Cross(ezs);
/* unit vectors of receiver antenna */
GeoCoord geoCoord = epochSatellite.SiteInfo.EstimatedGeoCoord;
//ecef2pos(rr, pos);
//xyz2enu(pos, E);
//exr[0] = E[1]; exr[1] = E[4]; exr[2] = E[7]; /* x = north */
//eyr[0] = -E[0]; eyr[1] = -E[3]; eyr[2] = -E[6]; /* y = west */
double Lat = geoCoord.Lat * CoordConsts.DegToRadMultiplier;
double Lon = geoCoord.Lon * CoordConsts.DegToRadMultiplier;
//geoCoord.Lat = geoCoord.Lat / CoordConsts.RadToDegMultiplier;
//geoCoord.Lon = geoCoord.Lon / CoordConsts.RadToDegMultiplier;
double[] exr = new double[3];
double[] eyr = new double[3];
double cosB = Math.Cos(Lat);
double sinB = Math.Sin(Lat);
double cosL = Math.Cos(Lon);
double sinL = Math.Sin(Lon);
exr[0] = -sinB * cosL; exr[1] = -sinB * sinL; exr[2] = cosB; /* x = north */
eyr[0] = sinL; eyr[1] = -cosL; eyr[2] = 0; /* y = west */
XYZ exr_ = new XYZ();
exr_.X = exr[0]; exr_.Y = exr[1]; exr_.Z = exr[2];
XYZ eyr_ = new XYZ();
eyr_.X = eyr[0]; eyr_.Y = eyr[1]; eyr_.Z = eyr[2];
/* phase windup effect */
//cross3(ek, eys, eks);
//cross3(ek, eyr, ekr);
XYZ eks = ek.Cross(eys);
XYZ ekr = ek.Cross(eyr_);
double[] ds = new double[3];
double[] dr = new double[3];
for (int i = 0; i < 3; i++)
{
//ds[time] = exs[time] - ek[time] * dot(ek, exs, 3) - eks[time];
//dr[time] = exr[time] - ek[time] * dot(ek, exr, 3) + ekr[time];
ds[i] = exs[i] - ek[i] * ek.Dot(exs) - eks[i];
dr[i] = exr[i] - ek[i] * ek.Dot(exr_) + ekr[i];
}
XYZ ds_ = new XYZ(ds);
XYZ dr_ = new XYZ(dr);
//cosp = dot(ds, dr, 3) / norm(ds, 3) / norm(dr, 3);
double cosp = ds_.Dot(dr_) / ds_.Length / dr_.Length;
if (cosp < -1.0)
{
cosp = -1.0;
}
else if (cosp > 1.0)
{
cosp = 1.0;
}
//ph = acos(cosp) / 2.0 / PI;
double ph = Math.Acos(cosp) / 2.0 / Math.PI;
//cross3(ds, dr, drs);
XYZ drs = ds_.Cross(dr_);
//if (dot(ek, drs, 3) < 0.0) ph = -ph;
if (ek.Dot(drs) < 0.0)
{
ph = -ph;
}
//*phw = ph + floor(*phw - ph + 0.5); /* in cycle */
//double wind_up = ph + Math.Floor(wind_up - ph + 0.5);
PhaseManager[prn].CorrectionValue = ph + Math.Floor(PhaseManager[prn].CorrectionValue - ph + 0.5);
double wind_up = PhaseManager[prn].CorrectionValue * 2 * Math.PI;
//要处理异常IIR卫星，参见GPSTK，2015.02.08
//if (SatInfoService.GetBlock(satelliteType, time) == "IIR")
//{
// if (wind_up > 0)
// {
// wind_up -= Math.PI;
// }
// else
// {
// wind_up += Math.PI;
// }
//}
//已验证这种方法的精度，但对于IIR型卫星，如果处理，还未实现，因此该模型暂时不能用。
#endregion
return(wind_up);
}
/// <summary>
/// EclipsedSatFilter核心
/// </summary>
/// <param name="gData"></param>
/// <returns></returns>
public override bool Revise(ref EpochInformation gData)
{
// Time epoch = gData.CorrectedTime;
Time epoch = gData.ReceiverTime;
List <SatelliteNumber> satRejectedSet = new List <SatelliteNumber>();
// Set the threshold to declare that satellites are in eclipse
// threshold = cos(180 - coneAngle/2)
double threshold = Math.Cos((CoordConsts.PI - ConeAngle / 2.0 * CoordConsts.DegToRadMultiplier));
// Compute Sun position at this epoch, and store it in a Triple
SunPosition sunPosition = new SunPosition();
// Variables to hold Sun and Moon positions
XYZ sunPos = (sunPosition.GetPosition(epoch));
// Loop through all the satellites
foreach (var sat in gData.EnabledSats)
{
SatelliteNumber prn = sat.Prn;
//Define a XYZ that will hold satellite position, in ECEF
XYZ svPos = gData[prn].Ephemeris.XYZ; //.GetSatPostion(satelliteType);
XYZ rk = svPos.UnitVector(); // Unitary vector from Earth mass center to satellite
// Unitary vector from Earth mass center to Sun
XYZ ri = sunPos.UnitVector();
//Get dot product between unitary vectors = cosine(angle)
double cosAngle = ri.Dot(rk);
// Check if satellite is within shadow
if (cosAngle <= threshold)
{
// If satellite is eclipsed, then schedule it for removal
satRejectedSet.Add(prn);
if (!ShadowEpoch.ContainsKey(prn))
{
ShadowEpoch.Add(prn, epoch);
}
// Keep track of last known epoch the satellite was in eclipse
ShadowEpoch[prn] = epoch;
continue;
}
else
{
// Maybe the satellite is out fo shadow, but it was recently
// in eclipse. Check also that.
if (ShadowEpoch.ContainsKey(prn))
{
// // If satellite was recently in eclipse, check if elapsed
// time is less or equal than postShadowPeriod
double temp = (double)Math.Abs(epoch - ShadowEpoch[prn]);
if (temp <= PostShadowPeriod)
{
// Satellite left shadow, but too recently. Delete it
satRejectedSet.Add(prn);
}
else
{
// If satellite left shadow a long time ago, set it free
ShadowEpoch.Remove(prn);
}
}
}
}
//debug
if (satRejectedSet.Count > 0 && satRejectedSet.FindAll(m => !RemovedPrn.Contains(m)).Count > 0)
{
StringBuilder sb = new StringBuilder();
sb.Append(gData.Name + "," + gData.ReceiverTime + ",删除太阳阴影影响的卫星:");
sb.Append(String.Format(new EnumerableFormatProvider(), "{0}", satRejectedSet));
log.Debug(sb.ToString());
RemovedPrn.AddRange(satRejectedSet.FindAll(m => !RemovedPrn.Contains(m)));
}
//Remove satellites with missing satData
gData.Remove(satRejectedSet, true, "删除太阳阴影影响的卫星");
return(true);
}