/// <summary>
/// 根据O_FileDataHead中的卫星PRN和TOC找出N_FileData中对应的星历序号
/// </summary>
/// <param name="sat_PRN">O_FileDataHead中的卫星PRN</param>
/// <param name="epoch">O_FileDataHead中的TOC</param>
/// <returns>N_FileData中对应的星历序号</returns>
public int FindBestEph(int sat_PRN, Time epoch)
{
GPSTime oTime = TimeToGPSTime(epoch);
for (int i = 0; i < N_FileSum.n_FileDataSum.Count; i++)
{
if (sat_PRN == N_FileSum.n_FileDataSum[i].PRN)
{
GPSTime nTime = TimeToGPSTime(N_FileSum.n_FileDataSum[i].TOC);
if (Math.Abs(oTime.GPSSecond - nTime.GPSSecond) < 3600.0)
{
return(i);
}
}
}
return(-1);
}
/// <summary>
/// 计算卫星在信号发射时的位置
/// </summary>
/// <param name="n_FileData">星历数据</param>
/// <param name="satSendTime">卫星信号发射的时间</param>
/// <returns>卫星在地心坐标系中的空间直角坐标</returns>
public Position SatellitePosition(N_FileData n_FileData, GPSTime satSendTime)
{
Position satPosition = new Position();
double A, n0, tk, n, Mk, Ek, E0, f, uu, omega_uk, omega_rk, omega_ik, uk, rk, ik, xk, yk, Lk;
A = Math.Pow(n_FileData.SqrtA, 2);//轨道长半轴
n0 = Math.Sqrt((Constant.GM / Math.Pow(A, 3)));
tk = satSendTime.GPSWeek * 604800 + satSendTime.GPSSecond - n_FileData.GPSWeek * 604800 - n_FileData.TOE;
if (tk > 302400)
{
tk = tk - 604800;
}
else if (tk < -302400)
{
tk = tk + 604800;
}
n = n0 + n_FileData.DetlaN;
Mk = n_FileData.M0 + n * tk;
E0 = Mk;
Ek = Mk + n_FileData.E * Math.Sin(E0);
while (Math.Abs(Ek - E0) > Math.Pow(10, -12))
{
E0 = Ek;
Ek = Mk + n_FileData.E * Math.Sin(E0);
}
f = Math.Atan2((Math.Sqrt(1 - Math.Pow(n_FileData.E, 2))) * Math.Sin(Ek), Math.Cos(Ek) - n_FileData.E);
uu = f + n_FileData.OmegaLow;
omega_uk = n_FileData.Cus * Math.Sin(2 * uu) + n_FileData.Cuc * Math.Cos(2 * uu);
omega_rk = n_FileData.Crs * Math.Sin(2 * uu) + n_FileData.Crc * Math.Cos(2 * uu);
omega_ik = n_FileData.Cis * Math.Sin(2 * uu) + n_FileData.Cic * Math.Cos(2 * uu);
uk = uu + omega_uk;
rk = A * (1 - n_FileData.E * Math.Cos(Ek)) + omega_rk;
ik = n_FileData.I0 + omega_ik + n_FileData.IDot * tk;
xk = rk * Math.Cos(uk);
yk = rk * Math.Sin(uk);
Lk = n_FileData.Omega + (n_FileData.OmegaDot - Constant.OmegaDotE) * tk - Constant.OmegaDotE * n_FileData.TOE;
satPosition.XX = xk * Math.Cos(Lk) - yk * Math.Cos(ik) * Math.Sin(Lk);
satPosition.YY = xk * Math.Sin(Lk) + yk * Math.Cos(ik) * Math.Cos(Lk);
satPosition.ZZ = yk * Math.Sin(ik);
return(satPosition);
}
/// <summary>
/// 通用时转换为GPS时
/// </summary>
/// <param name="time">通用时</param>
/// <returns>GPS时</returns>
public GPSTime TimeToGPSTime(Time time)
{
GPSTime gpsTime = new GPSTime();
double JD, UT;
int y, m;
UT = time.Hour + (time.Minute / 60.0) + (time.Second / 3600.0);
if (time.Month <= 2)
{
y = time.Year - 1;
m = time.Month + 12;
}
else
{
y = time.Year;
m = time.Month;
}
JD = (int)(365.25 * y) + (int)(30.6001 * (m + 1)) + time.Day + (UT / 24) + 1720981.5;
gpsTime.GPSWeek = (int)((JD - 2444244.5) / 7);
gpsTime.GPSSecond = (JD - 2444244.5) * 3600 * 24 - gpsTime.GPSWeek * 3600 * 24 * 7;
return(gpsTime);
}
public bool Positioning()
{
ReceiverPositionSum.receiverPositionSum.Clear();
OneEpochData oneEpochData = new OneEpochData();
List <OneEpochData> oneEpochDataSum = new List <OneEpochData>();
GPSTime TR = new GPSTime(); //历元时刻
double x, y, z, cdtr; //测站位置和钟差
double xx = 0;
double yy = 0;
double zz = 0; //用于储存每个历元的定位结果，用于赋值给下一历元的初值
double deltaT0Si; //卫星信号传播时间
double dtSi; //卫星钟差
double deltaT1Si;
double rho; //伪距
GPSTime TSi = new GPSTime(); //卫星信号发射概略时刻
Position XSi = new Position(); //卫星在TSi时刻的位置
Position XSiW = new Position(); //卫星自转改正后的位置
GPSTime T1Si = new GPSTime(); //卫星信号发射时刻
double RSi; //测站和卫星的几何距离
double b0Si, b1Si, b2Si, b3Si; //卫星方向余弦
double ISi; //卫星在观测方程中的余数项
double[,] A;
double[,] AT;
double[,] ATA = new double[4, 4];
double[,] revATA = new double[4, 4];
double[,] L;
double[,] ATL = new double[4, 1];
double[,] xi;
double sumX = 0;
double sumY = 0;
double sumZ = 0;
if (O_FlieSum.o_FileDataSum.Count == 0)
{
MessageBox.Show("没有打开观测数据文件", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
return(false);
}
if (N_FileSum.n_FileDataSum.Count == 0)
{
MessageBox.Show("没有打开导航电文文件", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
return(false);
}
for (int i = 0; i < O_FlieSum.o_FileDataSum.Count; i++)//循环全部历元
{
x = xx;
y = yy;
z = zz; //每个历元初值等于上个历元的定位结果，第一个历元为0
cdtr = 0; //第一次循环为0
xi = new double[4, 1];
do//解算一个历元
{
//************************有改动，X+
x = x + xi[0, 0];//第一次改正数为0，以后每次循环将改正数累加。
y = y + xi[1, 0];
z = z + xi[2, 0];
cdtr = cdtr + xi[3, 0];
for (int j = 0; j < O_FlieSum.o_FileDataSum[i].o_FileDataHead.Sat_Num; j++) //循环一个历元中的全部卫星
{
TR = TimeToGPSTime(O_FlieSum.o_FileDataSum[i].o_FileDataHead.Epoch); //O-FileDataHead中的历元时刻转换为GPS时
int prnNum = FindBestEph(O_FlieSum.o_FileDataSum[i].o_FileDataHead.Sat_PRN[j], O_FlieSum.o_FileDataSum[i].o_FileDataHead.Epoch);
if (prnNum == -1)
{
MessageBox.Show("卫星{0}没有对应的星历文件", O_FlieSum.o_FileDataSum[i].o_FileDataHead.Sat_PRN[j].ToString());
}
rho = O_FlieSum.o_FileDataSum[i].o_FileDataObs[j].C1;
//************有改动，-cdtr/Constant.SpeedOfLight
GPSTime gpsTimeTOC = TimeToGPSTime(N_FileSum.n_FileDataSum[prnNum].TOC);
dtSi = N_FileSum.n_FileDataSum[prnNum].ClkBias + N_FileSum.n_FileDataSum[prnNum].ClkDrift * (TR.GPSSecond - gpsTimeTOC.GPSSecond) + N_FileSum.n_FileDataSum[prnNum].ClkDriftRate * Math.Pow((TR.GPSSecond - gpsTimeTOC.GPSSecond), 2);
deltaT1Si = (rho / Constant.SpeedOfLight) + N_FileSum.n_FileDataSum[prnNum].ClkBias + N_FileSum.n_FileDataSum[prnNum].ClkDrift * (TR.GPSSecond - gpsTimeTOC.GPSSecond) + N_FileSum.n_FileDataSum[prnNum].ClkDriftRate * Math.Pow((TR.GPSSecond - gpsTimeTOC.GPSSecond), 2) - cdtr / Constant.SpeedOfLight;
do
{
deltaT0Si = deltaT1Si;
TSi.GPSSecond = TR.GPSSecond - deltaT0Si;
TSi.GPSWeek = TR.GPSWeek;
XSi = SatellitePosition(N_FileSum.n_FileDataSum[prnNum], TSi);
XSiW = EarthRotationCorrection(deltaT0Si, XSi);
//******************有改动，O_FlieSum.o_FileHeadSum.Approx_Position.XX.YY.ZZ XYZ
RSi = Math.Sqrt(Math.Pow(XSiW.XX - (O_FlieSum.o_FileHeadSum.Approx_Position.XX + x), 2) + Math.Pow(XSiW.YY - (O_FlieSum.o_FileHeadSum.Approx_Position.YY + y), 2) + Math.Pow(XSiW.ZZ - (O_FlieSum.o_FileHeadSum.Approx_Position.ZZ + z), 2));
deltaT1Si = RSi / Constant.SpeedOfLight;
} while (Math.Abs(deltaT1Si - deltaT0Si) > Math.Pow(10, -7));
T1Si.GPSSecond = TR.GPSSecond - deltaT1Si;
T1Si.GPSWeek = TR.GPSWeek;
//*********************有改动O_FlieSum.o_FileHeadSum.Approx_Position.XX + X，以及/RSi
b0Si = (O_FlieSum.o_FileHeadSum.Approx_Position.XX + x - XSiW.XX) / rho;
b1Si = (O_FlieSum.o_FileHeadSum.Approx_Position.YY + y - XSiW.YY) / rho;
b2Si = (O_FlieSum.o_FileHeadSum.Approx_Position.ZZ + z - XSiW.ZZ) / rho;
b3Si = 1.0;
//****************************有改动
ISi = rho - RSi + dtSi * Constant.SpeedOfLight - cdtr;
oneEpochData.b0S = b0Si;
oneEpochData.b1S = b1Si;
oneEpochData.b2S = b2Si;
oneEpochData.b3S = b3Si;
oneEpochData.IS = ISi;
oneEpochDataSum.Add(oneEpochData);
oneEpochData = new OneEpochData();
}//for j
A = new double[oneEpochDataSum.Count, 4];
L = new double[oneEpochDataSum.Count, 1];
AT = new double[4, oneEpochDataSum.Count];
for (int k = 0; k < oneEpochDataSum.Count; k++)
{
A[k, 0] = oneEpochDataSum[k].b0S;
A[k, 1] = oneEpochDataSum[k].b1S;
A[k, 2] = oneEpochDataSum[k].b2S;
A[k, 3] = oneEpochDataSum[k].b3S;
L[k, 0] = oneEpochDataSum[k].IS;
}
AT = Matrix.Transfer(A, oneEpochDataSum.Count, 4);
ATA = Matrix.Multiply(AT, A, 4, oneEpochDataSum.Count, 4);
revATA = Matrix.MatrixOpp(ATA);
ATL = Matrix.Multiply(AT, L, 4, oneEpochDataSum.Count, 1);
xi = Matrix.Multiply(revATA, ATL, 4, 4, 1);
//*************有改动，添加
oneEpochDataSum.Clear();
} while (Math.Abs(xi[0, 0]) > 0.001 && Math.Abs(xi[1, 0]) > 0.001 && Math.Abs(xi[2, 0]) > 0.001); //循环条件是否应该直接判断每次计算的坐标增量
xx = x + xi[0, 0]; //之前全部的改正加最后一次循环的改正数，这个历元的最终结果
yy = y + xi[1, 0];
zz = z + xi[2, 0];
receiverPosition.X = O_FlieSum.o_FileHeadSum.Approx_Position.XX + xx;
receiverPosition.Y = O_FlieSum.o_FileHeadSum.Approx_Position.YY + yy;
receiverPosition.Z = O_FlieSum.o_FileHeadSum.Approx_Position.ZZ + zz;
receiverPosition.Cdtr = (cdtr + xi[3, 0]) / Constant.SpeedOfLight;//是否是除以光速之后才能得出
receiverPosition.RPTime.GPSWeek = TR.GPSWeek;
receiverPosition.RPTime.GPSSecond = TR.GPSSecond;
ReceiverPositionSum.receiverPositionSum.Add(receiverPosition);
receiverPosition = new ReceiverPosition();
oneEpochDataSum.Clear();
}//for i
for (int i = 0; i < ReceiverPositionSum.receiverPositionSum.Count; i++)
{
sumX = sumX + ReceiverPositionSum.receiverPositionSum[i].X;
sumY = sumY + ReceiverPositionSum.receiverPositionSum[i].Y;
sumZ = sumZ + ReceiverPositionSum.receiverPositionSum[i].Z;
}
Result.X = sumX / ReceiverPositionSum.receiverPositionSum.Count;
Result.Y = sumY / ReceiverPositionSum.receiverPositionSum.Count;
Result.Z = sumZ / ReceiverPositionSum.receiverPositionSum.Count;
//Result.X = ReceiverPositionSum.receiverPositionSum[ReceiverPositionSum.receiverPositionSum.Count - 1].X;
//Result.Y = ReceiverPositionSum.receiverPositionSum[ReceiverPositionSum.receiverPositionSum.Count - 1].Y;
//Result.Z = ReceiverPositionSum.receiverPositionSum[ReceiverPositionSum.receiverPositionSum.Count - 1].Z;
return(true);
}
