/// <summary>
/// 非差轨道确定
/// </summary>
/// <param name="Material">历元信息</param>
/// <param name="ResultMatrix">平差信息</param>
/// <param name="ParamNameBuilder">钟差估计器</param>
/// <param name="previousResult">上一历元结果</param>
public RangeOrbitResult(
MultiSiteEpochInfo Material,
AdjustResultMatrix ResultMatrix,
RangeOrbitParamNameBuilder ParamNameBuilder,
RangeOrbitResult previousResult = null)
: base(Material, ResultMatrix, ParamNameBuilder)
{
//测站接收机钟差改正
foreach (var site in Material)
{
var clkName = ParamNameBuilder.GetReceiverClockParamName(site.SiteName);
var val = ResultMatrix.Estimated.Get(clkName);
site.Time.Correction = GetTimeCorrectionSeconds(val.Value);
}
//提取星历参数，可以用于改正卫星位置，进行迭代计算
EphemerisResults = new BaseDictionary <SatelliteNumber, EphemerisResult>();
foreach (var site in Material)
{
foreach (var sat in site)
{
var prn = sat.Prn;
Ephemeris estimated = new Ephemeris(sat.Ephemeris.Prn, sat.Ephemeris.Time)
{
XyzDotRms = new XYZ()
};
var clkName = ParamNameBuilder.GetSatClockParamName(prn);
var val = ResultMatrix.Estimated.Get(clkName);
estimated.ClockBias = GetTimeCorrectionSeconds(val.Value);
estimated.ClockBiasRms = val.Rms / GnssConst.LIGHT_SPEED;
var names = ParamNameBuilder.GetSatDxyz(prn);
foreach (var item in names)
{
val = ResultMatrix.Estimated.Get(item);
if (item.Contains(Gnsser.ParamNames.Dx))
{
estimated.XYZ.X = val.Value;
estimated.XyzDotRms.X = val.Rms;
}
if (item.Contains(Gnsser.ParamNames.Dy))
{
estimated.XYZ.Y = val.Value;
estimated.XyzDotRms.Y = val.Rms;
}
if (item.Contains(Gnsser.ParamNames.Dz))
{
estimated.XYZ.Z = val.Value;
estimated.XyzDotRms.Z = val.Rms;
}
}
EphemerisResults[prn] = new EphemerisResult((Ephemeris)sat.Ephemeris, estimated);
}
}
}
private void Init()
{
int count = entities.Count;
double[] x = new double[count];
double[] yX = new double[count];
double[] yY = new double[count];
double[] yZ = new double[count];
double[] yClock = new double[count];
int i = 0;
foreach (var item in entities)
{
Ephemeris record = item;
x[i] = (record.Time - entities.First.Time); //Y为GPS周秒。
yX[i] = record.XYZ.X;
yY[i] = record.XYZ.Y;
yZ[i] = record.XYZ.Z;
yClock[i] = record.ClockBias;
i++;
}
//for (int i = 0; i < count; i++)
//{
// Ephemeris record = sortedRecords[i];
// x[i] = (record.Time - sortedRecords[0].Time); //Y为GPS周秒。
// yX[i] = record.XYZ.X;
// yY[i] = record.XYZ.Y;
// yZ[i] = record.XYZ.Z;
// yClock[i] = record.ClockBias;
//}
int order = 10;
order = Math.Min(order, count);
//fitX = new PolyFit(x, yX);
//fitY = new PolyFit(x, yY);
//fitZ = new PolyFit(x, yZ);
//fitClock = new PolyFit(x, yClock);
fitX = new LagrangeInterplation(x, yX, order);
fitY = new LagrangeInterplation(x, yY, order);
fitZ = new LagrangeInterplation(x, yZ, order);
fitClock = new LagrangeInterplation(x, yClock, 2);
//fitClock = new LagrangeInterplation(x, yClock, 10);
//fitX = new ChebyshevPolyFit(x, yX, order);
//fitY = new ChebyshevPolyFit(x, yY, order);
//fitZ = new ChebyshevPolyFit(x, yZ, order);
//fitClock = new LagrangeInterplation(x, yClock, 2);
}
/// <summary>
/// 获取指定时刻的卫星位置，考虑了地球自转改正置。
/// </summary>
/// <param name="gpsTime"></param>
/// <param name="ephemeris"></param>
/// <returns></returns>
public static XYZ GetSatPosWithEarthRotateCorrection(Time gpsTime, Ephemeris ephemeris)
{
var dt = (ephemeris.Time - gpsTime); //相距参考时刻的秒数。
#region 方案1：顾及地球自转效应
//correction for earth rotation
double sinL = Math.Sin(GnssConst.RotateVelocityOfEarth * dt);
double cosL = Math.Cos(GnssConst.RotateVelocityOfEarth * dt);
double newX = cosL * ephemeris.XYZ.X - sinL * ephemeris.XYZ.Y;
double newY = sinL * ephemeris.XYZ.X + cosL * ephemeris.XYZ.Y;
double newZ = ephemeris.XYZ.Z;
XYZ newXyz = new XYZ(newX, newY, newZ);
#endregion
return(newXyz);
}
/// <summary>
/// 星历相减
/// </summary>
/// <param name="first"></param>
/// <param name="second"></param>
/// <returns></returns>
public static Ephemeris operator -(Ephemeris first, Ephemeris second)
{
if (first.Prn != second.Prn || first.Time != second.Time)
{
new Log(typeof(Ephemeris)).Error("不是同一卫星，不可相加，" + first + ", " + second);
return(first);
}
Ephemeris ephemeris = new Ephemeris(first.Prn, first.Time);
ephemeris.XYZ = first.XYZ - second.XYZ;
if (first.Rms != null && second.Rms != null)
{
ephemeris.Rms = XYZ.RmsPlus(first.Rms, second.Rms);
}
ephemeris.XyzDot = first.XyzDot - second.XyzDot;
ephemeris.ClockBias = first.ClockBias - second.ClockBias;
ephemeris.ClockBiasRms = Geo.Utils.DoubleUtil.RmsPlus(first.ClockBiasRms, first.ClockBiasRms);
ephemeris.ClockDrift = first.ClockDrift - second.ClockDrift;
return(ephemeris);
}
public override IEphemeris Get()
{
SatelliteNumber prn = EpochSat.Prn;
Ephemeris eph = null;
XYZ receiverPos = EpochSat.SiteInfo.EstimatedXyz;
Time receivingTime = EpochSat.RecevingTime;
//2.1 坐标迭代法求解
double transitTime = 0.075;//传输时间初始值，GEO卫星应该更长，但此处并不影响。
Time transmitTime = receivingTime - transitTime;
eph = EphemerisService.Get(prn, transmitTime);
if (eph == null)
{
throw new NullReferenceException("没有找到 " + prn + " " + transmitTime + " 的星历");
}
double differ = Double.MaxValue;//亚纳秒级别则退出
double DELTA = 1e-10;
double tmp = transitTime;
// int count = 0;//测试，统计计算了多少次
for (int i = 0; differ > DELTA && i < 5; i++) //一般循环一次，可满足精度
{
//由于第一次的时间求得的卫星坐标可能会很差（几十米即可带来较大时间误差），因此继续利用通过观测值求得的时间
tmp = transitTime;
transitTime = (eph.XYZ - receiverPos).Length / GnssConst.LIGHT_SPEED;
transmitTime = receivingTime - transitTime;
differ = Math.Abs(transitTime - tmp);
eph = EphemerisService.Get(prn, transmitTime);
// count++;
}
// System.IO.File.AppendAllText(@"D:\count.txt", count + "\t" + differ.ToString("E") + "\r\n");
return(eph);
}
private void Init()
{
int count = sortedRecords.Count;
double[] x = new double[count];
double[] yX = new double[count];
double[] yY = new double[count];
double[] yZ = new double[count];
double[] yClock = new double[count];
for (int i = 0; i < count; i++)
{
Ephemeris record = sortedRecords[i];
x[i] = (record.Time - sortedRecords[0].Time); //Y为GPS周秒。
yX[i] = record.XYZ.X;
yY[i] = record.XYZ.Y;
yZ[i] = record.XYZ.Z;
yClock[i] = record.ClockBias;
}
int order = 10;
order = Math.Min(order, count);
//fitX = new PolyFit(x, yX);
//fitY = new PolyFit(x, yY);
//fitZ = new PolyFit(x, yZ);
//fitClock = new PolyFit(x, yClock);
fitX = new LagrangeInterplation(x, yX, order);
fitY = new LagrangeInterplation(x, yY, order);
fitZ = new LagrangeInterplation(x, yZ, order);
// fitClock = new LagrangeInterplation(x, yClock, 2);
fitClock = new LagrangeInterplation(x, yClock, 10);
//fitX = new ChebyshevPolyFit(x, yX, order);
//fitY = new ChebyshevPolyFit(x, yY, order);
//fitZ = new ChebyshevPolyFit(x, yZ, order);
//fitClock = new LagrangeInterplation(x, yClock, 2);
}
public override IEphemeris Get()
{
//实质为计算发射时刻的系统时间, 只比较秒数
SatelliteNumber prn = EpochSat.Prn;
Ephemeris eph = null;
XYZ receiverPos = EpochSat.SiteInfo.EstimatedXyz;
Time receivingTime = EpochSat.RecevingTime;
//2.需要接收机钟差
if (EpochSat.Time.Correction != 0 && EpochSat.FrequenceA.DopplerShift != null)
{
//2.2 多普勒频移法
eph = EphemerisService.Get(prn, receivingTime);
double distance = (eph.XYZ - receiverPos).Length;
double deltaT = distance / GnssConst.LIGHT_SPEED;
//这里的距离差是接收时刻与发射时刻的位移差，所以应该减去。
distance -= EpochSat.AverageDopplorSpeed * deltaT;
deltaT = distance / GnssConst.LIGHT_SPEED;
Time transmitTime = receivingTime - deltaT;
eph = EphemerisService.Get(prn, transmitTime);
}
return(eph);
}
/// <summary>
/// 差值计算
/// </summary>
/// <param name="rTime"></param>
/// <param name="staXyz"></param>
private void SatPo(Time rTime, XYZ staXyz)
{
int count = sortedRecords.Count;
double[] TimeArr = new double[count];
double[] XArr = new double[count];
double[] YArr = new double[count];
double[] ZArr = new double[count];
double[] CLKsat = new double[count];
for (int i = 0; i < count; i++)
{
Ephemeris record = sortedRecords[i];
TimeArr[i] = record.Time.SecondsOfWeek; //Y为GPS周秒。
XArr[i] = record.XYZ.X;
YArr[i] = record.XYZ.Y;
ZArr[i] = record.XYZ.Z;
CLKsat[i] = record.ClockBias;
}
double Time = rTime.SecondsOfWeek;//卫星发射时刻
double startTime = TimeArr[0] - 5;
double lastTime = TimeArr[TimeArr.Length - 1] + 5;
double timeInterval;
int d = 9;
double NA = 99999;
double x1 = 0.0, y1 = 0.0, z1 = 0.0;
double[] x2 = new double[d]; double[] y2 = new double[d]; double[] z2 = new double[d];
double x3 = 0.0, y3 = 0.0, z3 = 0.0;
double x4 = 0.0, y4 = 0.0, z4 = 0.0;
double Xt, Yt, Zt;
double rho; double recieveTime;
if (Time >= startTime && Time <= lastTime)
{
timeInterval = TimeArr[2] - TimeArr[1];
//
XYZ(Time, TimeArr, XArr, d, NA, ref x1, ref x2, ref x3, ref x4);
if (x4 == 0)
{
satSpeed = new XYZ(0, 0, 0);
return;
}
//拉格朗日插值
Xt = LG(d, x1, x2, NA);
XYZ(Time, TimeArr, YArr, d, NA, ref y1, ref y2, ref y3, ref y4);
Yt = LG(d, y1, y2, NA);
XYZ(Time, TimeArr, ZArr, d, NA, ref z1, ref z2, ref z3, ref z4);
Zt = LG(d, z1, z2, NA);
//
satPos = new XYZ(Xt, Yt, Zt);
rho = (satPos - staXyz).Length;
recieveTime = rTime.SecondsOfWeek + rho / GnssConst.LIGHT_SPEED; //接受时刻
}
else
{
throw new Exception("rTime is out of range in TimeArr:SeeSatPo\n");
}
double t, stdx;
for (int i = 1; i <= 3; i++)
{
t = recieveTime - rho / GnssConst.LIGHT_SPEED;
stdx = ((Time - x3) / timeInterval) + 1;
Xt = LG(d, stdx, x2, NA);
Yt = LG(d, stdx, y2, NA);
Zt = LG(d, stdx, z2, NA);
satPos = new XYZ(Xt, Yt, Zt);
rho = (satPos - staXyz).Length;
}
stdx = Convert.ToInt32(Math.Round(x1));
double Dinterval;
if (Convert.ToInt32(stdx) == 1 || Convert.ToInt32(stdx) == d)
{
Dinterval = timeInterval;
}
else
{
Dinterval = 2 * timeInterval;
}
int B, L;
if (Convert.ToInt32(stdx) == 1)
{
B = 1;
}
else
{
B = Convert.ToInt32(stdx) - 1;
}
if (Convert.ToInt32(stdx) == d)
{
L = d;
}
else
{
L = Convert.ToInt32(stdx) + 1;
}
double Vx, Vy, Vz;
Vx = (x2[L - 1] - x2[B - 1]) / Dinterval;
Vy = (y2[L - 1] - y2[B - 1]) / Dinterval;
Vz = (z2[L - 1] - z2[B - 1]) / Dinterval;
satSpeed = new XYZ(Vx, Vy, Vz);
}
/// <summary>
/// 获取指定数组中与 X 最相近的数组编号。
/// </summary>
/// <param name="XArray">递增或递减数组</param>
/// <param name="xValue"></param>
/// <param name="count"></param>
/// <returns></returns>
public static List <int> GetNearstIndexes(List <Ephemeris> sortedRecords, double xValue, int order = 10)
{
int count = sortedRecords.Count;
double[] XArray = new double[count];
//double[] yX = new double[count];
//double[] yY = new double[count];
//double[] yZ = new double[count];
//double[] yClock = new double[count];
for (int i = 0; i < count; i++)
{
Ephemeris record = sortedRecords[i];
XArray[i] = (record.Time - sortedRecords[0].Time); //Y为GPS周秒。
//yX[time] = record.XYZ.X;
//yY[time] = record.XYZ.Y;
//yZ[time] = record.XYZ.Z;
//yClock[time] = record.Offset;
}
//int order = 10;
order = Math.Min(order, count);
List <int> indexes = new List <int>();
//如果数量大于数组数量，则返回全部
if (order >= XArray.Length)
{
for (int i = 0; i < order; i++)
{
indexes.Add(i);
}
return(indexes);
}
//找到离X最小的编号
int halfCount = order / 2;
int index = 0;
double min = double.MaxValue;
for (int i = 0; i < XArray.Length; i++)
{
double diff = Math.Abs(XArray[i] - xValue);
// if (diff == 0) return YArray[time];
if (diff < min)
{
min = diff;
index = i;
}
}
//在数组的头部
if (index - halfCount <= 0)
{
for (int i = 0; i < order; i++)
{
indexes.Add(i);
}
}
//尾部
else if (index + halfCount >= XArray.Length - 1)
{
for (int i = 0, j = XArray.Length - 1; i < order; i++, j--)
{
indexes.Insert(0, j);
}
}
//中间
else
{
for (int i = 0; i < order; i++)
{
indexes.Add(index - halfCount + i);
}
}
if (indexes[0] < 0)
{
throw new Exception("出错了");
}
return(indexes);
}
/// <summary>
/// 获取插值后的 EphemerisInfo。
/// 注意：不对钟差相对论误差进行改正，此处时间是卫星钟时间，没有相对信息。2014.09.08
/// 失败，如超过时间，则返回null
/// 由于一般没有卫星速度的数据，必须通过插值坐标计算速度。2015.03.
/// 如果有速度信息，以下的方法就不合适了。
/// </summary>
/// <param name="gpsTime">时间</param>
/// <returns></returns>
public Ephemeris GetEphemerisInfo(Time gpsTime)
{
SatelliteNumber PRN = Ephemeris.Prn;
Time min = Ephemeris.Time;
//Time max = sortedRecords[sortedRecords.Count - 1].Time;
//double span = (double)(max - min);
double gpsSecond = (gpsTime - min); //减去序列中第一个数
////老是在这里出问题，宽限一点呢？？16分钟
//double expand = 60 * 16;
//if (gpsSecond + expand < 0 || gpsSecond - expand > span)
//{
// //return null;
// throw new ArgumentException("历元在给定的时间段外，不可进行插值。");
//}
//计算位置
double x = 0, y = 0, z = 0; double dtime = 0;
double dx = 0, dy = 0, dz = 0; double ddtime = 0;
int count = entities.Count;
int order = 10;
order = Math.Min(order, count);
//czs
// List<int> indexes = GetNearstIndexes(sortedRecords, gpsSecond, order);
//cy
//List<int> indexes = GetNearstIndexes(sortedRecords, gpsTime, order);
double[] t = new double[order];
double[] tList = new double[order + 1];
double[] xList = new double[order + 1];
double[] yList = new double[order + 1];
double[] zList = new double[order + 1];
double[] clockList = new double[order + 1];
int i = 0;
foreach (var item in this.entities)
{
Ephemeris record = item;
t[i] = (record.Time - gpsTime); //秒。
#region 方案1：顾及地球自转效应
//correction for earth rotation
double sinl = Math.Sin(SunMoonPosition.OMGE * t[i]);
double cosl = Math.Cos(SunMoonPosition.OMGE * t[i]);
xList[i] = cosl * record.XYZ.X - sinl * record.XYZ.Y;
yList[i] = sinl * record.XYZ.X + cosl * record.XYZ.Y;
zList[i] = record.XYZ.Z;
#endregion
#region 方案2：不顾及地球自转效应
//xList[time] = record.XYZ.X;
//yList[time] = record.XYZ.Y;
//zList[time] = record.XYZ.Z;
#endregion
//tList[i] = (record.Time - sortedRecords[0].Time); //秒。
// clockList[i] = record.ClockBias;
i++;
}
//Lagrange(tList, xList, gpsSecond, ref x, ref dx);
//Lagrange(tList, yList, gpsSecond, ref y, ref dy);
//Lagrange(tList, zList, gpsSecond, ref z, ref dz);
//Lagrange(tList, clockList, gpsSecond, ref dtime, ref ddtime);
//int Nhalf = (int)(order / 2) - 1;
//int Nmatch = indexes[Nhalf];
x = fitX.GetY(gpsSecond);
y = fitY.GetY(gpsSecond);
z = fitZ.GetY(gpsSecond);
dtime = fitClock.GetY(gpsSecond);
XYZ SatXyz = new XYZ(x, y, z);
//精度赋值
//XYZ SatXyzSig = sortedRecords[Nmatch].XyzSdev;
//double clockSig = sortedRecords[Nmatch].ClockSdev;
//XYZ SatXyzDotSig = new Geo.Coordinates.XYZ(0);
//计算速度
//double nextSecond = gpsSecond + 0.001;
//XYZ xyzNext = new XYZ(fitX.GetY(nextSecond), fitY.GetY(nextSecond), fitZ.GetY(nextSecond));
//XYZ speed = (xyzNext-xyz) / 0.001;
XYZ SatSpeed = new XYZ(dx, dy, dz);
//Add relativity correction to dtime
//This only for consistency with GPSEphemerisInter
//dtr=-2*dot(R,V)/(C*C)
//dtime +=EphemerisUtil.GetRelativeCorrection(SatXyz, SatSpeed);
return(new Ephemeris()
{
Prn = PRN,
XYZ = SatXyz,
//XyzSdev=SatXyzSig,
// Clock = fitClock.GetY(gpsSecond),
ClockBias = dtime,
//ClockSdev = clockSig,
ClockDrift = ddtime,
Time = gpsTime,
XyzDot = SatSpeed,
//XyzDotSdev=SatXyzDotSig
});
}
public EphemerisResult(Ephemeris Original, Ephemeris Estimated)
{
this.Original = Original;
this.Estimated = Estimated;
this.Corrected = Original + Estimated;
}
