/// <summary>
/// 获取某颗卫星的位置
/// </summary>
/// <param name="t0"></param>
/// <param name="prn"></param>
/// <returns></returns>
public double[] GetSatPos(GPST t0, string prn)
{
double[] p = { 0, 0, 0 };
// 00:00:00之前，无法插值
if (t0 - StartTime + 1e-13 < 0)
{
return(p);
}
// 00:00:00之后，无法插值
if (t0 - StartTime > 24 * 3600)
{
return(p);
}
// 10阶插值
double[] t = new double[10];
double[] x = new double[10];
double[] y = new double[10];
double[] z = new double[10];
GPST ts = StartTime;
int index = (int)System.Math.Floor((t0 - ts) / Interval);
// Console.WriteLine("index:{0}", index);
// 刚好落在采样点上
if (Math.Abs(ts - t0 + Interval * index) < 1e-13)
{
p[0] = AllEpoch[index][prn].X;
p[1] = AllEpoch[index][prn].Y;
p[2] = AllEpoch[index][prn].Z;
return(p);
}
else if (Math.Abs(ts - t0 + Interval * index + Interval) < 1e-13)
{
p[0] = AllEpoch[index + 1][prn].X;
p[1] = AllEpoch[index + 1][prn].Y;
p[2] = AllEpoch[index + 1][prn].Z;
return(p);
}
// 在开始的几个历元内
if (index < 4)
{
for (int i = 0; i < 10; i++)
{
x[i] = AllEpoch[i][prn].X;
y[i] = AllEpoch[i][prn].Y;
z[i] = AllEpoch[i][prn].Z;
t[i] = AllEpoch[i].Epoch.TotalSeconds;
}
}
// 在结束的几个历元内
else if (EpochNum - index < 6)
{
for (int i = 0; i < 10; i++)
{
x[i] = AllEpoch[EpochNum - 10 + i][prn].X;
y[i] = AllEpoch[EpochNum - 10 + i][prn].Y;
z[i] = AllEpoch[EpochNum - 10 + i][prn].Z;
t[i] = AllEpoch[EpochNum - 10 + i].Epoch.TotalSeconds;
}
}
// 在中间
else
{
for (int i = 0; i < 10; i++)
{
x[i] = AllEpoch[index - 4 + i][prn].X;
y[i] = AllEpoch[index - 4 + i][prn].Y;
z[i] = AllEpoch[index - 4 + i][prn].Z;
t[i] = AllEpoch[index - 4 + i].Epoch.TotalSeconds;
}
}
p[0] = Interp.GetValueLagrange(10, t, x, t0.TotalSeconds);
p[1] = Interp.GetValueLagrange(10, t, y, t0.TotalSeconds);
p[2] = Interp.GetValueLagrange(10, t, z, t0.TotalSeconds);
return(p);
}