/// <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 RelativePositioning()
{
BaselineResult.baselineResult.Clear();
BaselineOneEpo baselineOneEpo = new BaselineOneEpo();
List <BaselineOneEpo> baselineOneEpoSum = new List <BaselineOneEpo>();
GPSTime time_Master = new GPSTime(); //基准站历元时刻
GPSTime time_Rover = new GPSTime(); //移动站历元时刻
double deltaT0Si_Master; //卫星信号传播时间
double deltaT1Si_Master;
double deltaT0Si_Rover; //卫星信号传播时间
double deltaT1Si_Rover;
GPSTime TSi_Master = new GPSTime(); //卫星信号发射概略时刻
Position XSi_Master = new Position(); //卫星在TSi时刻的位置
Position XSiW_Master1 = new Position(); //卫星自转改正后的位置
Position XSiW_Master = new Position();
GPSTime TSi_Rover = new GPSTime(); //卫星信号发射概略时刻
Position XSi_Rover = new Position(); //卫星在TSi时刻的位置
Position XSiW_Rover = new Position(); //卫星自转改正后的位置
double RSi_Master; //测站和卫星的几何距离
double RSi_Rover; //测站和卫星的几何距离
double rho_Master1; //参考卫星伪距
double rho_Rover1;
double rho_Master; //历元内剩余卫星伪距
double rho_Rover;
double li, mi, ni, omci;
double[,] A;
double[,] AT;
double[,] D;
double[,] DT;
double[,] DDT;
double[,] C;
double[,] omc;
double[,] ATC;
double[,] ATCA;
double[,] ATComc;
double[,] invATCA;
double[,] x;
double sumX = 0;
double sumY = 0;
double sumZ = 0;
if (N_FileSum.n_FileDataSum.Count == 0)
{
MessageBox.Show("没有打开导航电文文件", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
return(false);
}
if (O_FlieSum.master_O_FileDataSum.Count == 0)
{
MessageBox.Show("没有打开基准站观测数据文件", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
return(false);
}
if (O_FlieSum.rover_O_FileDataSum.Count == 0)
{
MessageBox.Show("没有打开移动站观测数据文件", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
return(false);
}
//****************如果发现没有进行基准站单点定位，则首先进行单点定位****************//
if (SPP_Result.X == 0 && SPP_Result.Y == 0 && SPP_Result.Z == 0 && SPP_Result.Cdtr == 0)
{
SinglePointPositioning();
}
//****************如果发现没有进行基准站单点定位，则首先进行单点定位****************//
double xx = 0;
double yy = 0;
double zz = 0;
for (int i = 0; i < O_FlieSum.master_O_FileDataSum.Count; i++)
{
time_Master = TimeToGPSTime(O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Epoch);
time_Rover = TimeToGPSTime(O_FlieSum.rover_O_FileDataSum[i].o_FileDataHead.Epoch);
if (time_Master.GPSSecond != time_Rover.GPSSecond)
{
MessageBox.Show("观测历元不匹配", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
return(false);
}
//以基准站的卫星序列为顺序基准，查找找共视卫星，并将流动站卫星重新排序
int[] rover_PRN = new int[O_FlieSum.rover_O_FileDataSum[i].o_FileDataHead.Sat_Num];
double[] rover_C1 = new double[O_FlieSum.rover_O_FileDataSum[i].o_FileDataHead.Sat_Num];
for (int sat_Num = 0; sat_Num < O_FlieSum.rover_O_FileDataSum[i].o_FileDataHead.Sat_Num; sat_Num++)
{
for (int prn = 0; prn < O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num; prn++)
{
if (O_FlieSum.rover_O_FileDataSum[i].o_FileDataObs[sat_Num].PRN == O_FlieSum.master_O_FileDataSum[i].o_FileDataObs[prn].PRN)
{
rover_PRN[prn] = O_FlieSum.rover_O_FileDataSum[i].o_FileDataObs[sat_Num].PRN;
rover_C1[prn] = O_FlieSum.rover_O_FileDataSum[i].o_FileDataObs[sat_Num].C1;
break;
}
}
}
for (int ii = 0; ii < rover_PRN.Length; ii++)
{
O_FlieSum.rover_O_FileDataSum[i].o_FileDataObs[ii].PRN = rover_PRN[ii];
O_FlieSum.rover_O_FileDataSum[i].o_FileDataObs[ii].C1 = rover_C1[ii];
}
//************下一历元初值等于上一历元计算结果************//
roverReceiverPosition.X = SPP_Result.X + xx;
roverReceiverPosition.Y = SPP_Result.Y + yy;
roverReceiverPosition.Z = SPP_Result.Z + zz;
//************下一历元初值等于上一历元计算结果************//
for (int iteration = 0; iteration < 8; iteration++)//迭代8次
{
//第一颗卫星为参考卫星
rho_Master1 = O_FlieSum.master_O_FileDataSum[i].o_FileDataObs[0].C1;
rho_Rover1 = O_FlieSum.rover_O_FileDataSum[i].o_FileDataObs[0].C1;
int prnNum1 = FindBestEph(O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_PRN[0], O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Epoch);
GPSTime gpsTimeTOC = TimeToGPSTime(N_FileSum.n_FileDataSum[prnNum1].TOC);
deltaT1Si_Master = (rho_Master1 / Constant.SpeedOfLight) + N_FileSum.n_FileDataSum[prnNum1].ClkBias + N_FileSum.n_FileDataSum[prnNum1].ClkDrift * (time_Master.GPSSecond - gpsTimeTOC.GPSSecond) + N_FileSum.n_FileDataSum[prnNum1].ClkDriftRate * Math.Pow((time_Master.GPSSecond - gpsTimeTOC.GPSSecond), 2);
do
{
deltaT0Si_Master = deltaT1Si_Master;
TSi_Master.GPSSecond = time_Master.GPSSecond - deltaT0Si_Master;
TSi_Master.GPSWeek = time_Master.GPSWeek;
XSi_Master = SatellitePosition(N_FileSum.n_FileDataSum[prnNum1], TSi_Master);
XSiW_Master1 = EarthRotationCorrection(deltaT0Si_Master, XSi_Master);
RSi_Master = Math.Sqrt(Math.Pow(XSiW_Master1.XX - SPP_Result.X, 2) + Math.Pow(XSiW_Master1.YY - SPP_Result.Y, 2) + Math.Pow(XSiW_Master1.ZZ - SPP_Result.Z, 2));
deltaT1Si_Master = RSi_Master / Constant.SpeedOfLight;
} while (Math.Abs(deltaT1Si_Master - deltaT0Si_Master) > Math.Pow(10, -7));
deltaT1Si_Rover = (rho_Rover1 / Constant.SpeedOfLight) + N_FileSum.n_FileDataSum[prnNum1].ClkBias + N_FileSum.n_FileDataSum[prnNum1].ClkDrift * (time_Master.GPSSecond - gpsTimeTOC.GPSSecond) + N_FileSum.n_FileDataSum[prnNum1].ClkDriftRate * Math.Pow((time_Master.GPSSecond - gpsTimeTOC.GPSSecond), 2);
do
{
deltaT0Si_Rover = deltaT1Si_Rover;
TSi_Rover.GPSSecond = time_Master.GPSSecond - deltaT0Si_Rover;
TSi_Rover.GPSWeek = time_Master.GPSWeek;
XSi_Rover = SatellitePosition(N_FileSum.n_FileDataSum[prnNum1], TSi_Rover);
XSiW_Rover = EarthRotationCorrection(deltaT0Si_Rover, XSi_Rover);
RSi_Rover = Math.Sqrt(Math.Pow(XSiW_Rover.XX - roverReceiverPosition.X, 2) + Math.Pow(XSiW_Rover.YY - roverReceiverPosition.Y, 2) + Math.Pow(XSiW_Rover.ZZ - roverReceiverPosition.Z, 2));
deltaT1Si_Rover = RSi_Rover / Constant.SpeedOfLight;
} while (Math.Abs(deltaT1Si_Rover - deltaT0Si_Rover) > Math.Pow(10, -7));
rho_Master1 = RSi_Master;
rho_Rover1 = RSi_Rover;
//历元内剩余卫星
for (int sat = 1; sat < O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num; sat++)//循环历元内剩余卫星
{
rho_Master = O_FlieSum.master_O_FileDataSum[i].o_FileDataObs[sat].C1;
rho_Rover = O_FlieSum.rover_O_FileDataSum[i].o_FileDataObs[sat].C1;
int prnNum = FindBestEph(O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_PRN[sat], O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Epoch);
gpsTimeTOC = TimeToGPSTime(N_FileSum.n_FileDataSum[prnNum].TOC);
deltaT1Si_Master = (rho_Master / Constant.SpeedOfLight) + N_FileSum.n_FileDataSum[prnNum].ClkBias + N_FileSum.n_FileDataSum[prnNum].ClkDrift * (time_Master.GPSSecond - gpsTimeTOC.GPSSecond) + N_FileSum.n_FileDataSum[prnNum].ClkDriftRate * Math.Pow((time_Master.GPSSecond - gpsTimeTOC.GPSSecond), 2);
do
{
deltaT0Si_Master = deltaT1Si_Master;
TSi_Master.GPSSecond = time_Master.GPSSecond - deltaT0Si_Master;
TSi_Master.GPSWeek = time_Master.GPSWeek;
XSi_Master = SatellitePosition(N_FileSum.n_FileDataSum[prnNum], TSi_Master);
XSiW_Master = EarthRotationCorrection(deltaT0Si_Master, XSi_Master);
RSi_Master = Math.Sqrt(Math.Pow(XSiW_Master.XX - SPP_Result.X, 2) + Math.Pow(XSiW_Master.YY - SPP_Result.Y, 2) + Math.Pow(XSiW_Master.ZZ - SPP_Result.Z, 2));
deltaT1Si_Master = RSi_Master / Constant.SpeedOfLight;
} while (Math.Abs(deltaT1Si_Master - deltaT0Si_Master) > Math.Pow(10, -7));
deltaT1Si_Rover = (rho_Rover / Constant.SpeedOfLight) + N_FileSum.n_FileDataSum[prnNum].ClkBias + N_FileSum.n_FileDataSum[prnNum].ClkDrift * (time_Master.GPSSecond - gpsTimeTOC.GPSSecond) + N_FileSum.n_FileDataSum[prnNum].ClkDriftRate * Math.Pow((time_Master.GPSSecond - gpsTimeTOC.GPSSecond), 2);
do
{
deltaT0Si_Rover = deltaT1Si_Rover;
TSi_Rover.GPSSecond = time_Master.GPSSecond - deltaT0Si_Rover;
TSi_Rover.GPSWeek = time_Master.GPSWeek;
XSi_Rover = SatellitePosition(N_FileSum.n_FileDataSum[prnNum], TSi_Rover);
XSiW_Rover = EarthRotationCorrection(deltaT0Si_Rover, XSi_Rover);
RSi_Rover = Math.Sqrt(Math.Pow(XSiW_Rover.XX - roverReceiverPosition.X, 2) + Math.Pow(XSiW_Rover.YY - roverReceiverPosition.Y, 2) + Math.Pow(XSiW_Rover.ZZ - roverReceiverPosition.Z, 2));
deltaT1Si_Rover = RSi_Rover / Constant.SpeedOfLight;
} while (Math.Abs(deltaT1Si_Rover - deltaT0Si_Rover) > Math.Pow(10, -7));
rho_Master = RSi_Master;
rho_Rover = RSi_Rover;
li = ((XSiW_Master1.XX - roverReceiverPosition.X) / rho_Rover1) - ((XSiW_Master.XX - roverReceiverPosition.X) / rho_Rover);
mi = ((XSiW_Master1.YY - roverReceiverPosition.Y) / rho_Rover1) - ((XSiW_Master.YY - roverReceiverPosition.Y) / rho_Rover);
ni = ((XSiW_Master1.ZZ - roverReceiverPosition.Z) / rho_Rover1) - ((XSiW_Master.ZZ - roverReceiverPosition.Z) / rho_Rover);
double observed = O_FlieSum.master_O_FileDataSum[i].o_FileDataObs[0].C1 - O_FlieSum.rover_O_FileDataSum[i].o_FileDataObs[0].C1 - O_FlieSum.master_O_FileDataSum[i].o_FileDataObs[sat].C1 + O_FlieSum.rover_O_FileDataSum[i].o_FileDataObs[sat].C1;
double calculated = rho_Master1 - rho_Rover1 - rho_Master + rho_Rover;
omci = observed - calculated;
baselineOneEpo.l = li;
baselineOneEpo.m = mi;
baselineOneEpo.n = ni;
baselineOneEpo.omc = omci;
baselineOneEpoSum.Add(baselineOneEpo);
baselineOneEpo = new BaselineOneEpo();
}//for sat
A = new double[baselineOneEpoSum.Count, 3];
AT = new double[3, baselineOneEpoSum.Count];
omc = new double[baselineOneEpoSum.Count, 1];
D = new double[O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1, 2 * (O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1) + 2];
for (int Di = 0; Di < O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1; Di++)
{
D[Di, 0] = 1;
D[Di, O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num] = -1;
D[Di, Di + 1] = -1;
D[Di, Di + O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num + 1] = 1;
}
DT = new double[2 * (O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1) + 2, O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1];
DT = Matrix.Transfer(D, O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1, 2 * (O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1) + 2);
DDT = new double[O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1, O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1];
DDT = Matrix.Multiply(D, DT, O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1, 2 * (O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1) + 2, O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1);
C = new double[O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1, O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1];
C = Matrix.MatrixOpp(DDT);
for (int k = 0; k < baselineOneEpoSum.Count; k++)
{
A[k, 0] = baselineOneEpoSum[k].l;
A[k, 1] = baselineOneEpoSum[k].m;
A[k, 2] = baselineOneEpoSum[k].n;
omc[k, 0] = baselineOneEpoSum[k].omc;
}
AT = Matrix.Transfer(A, baselineOneEpoSum.Count, 3);
ATC = new double[3, O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_Num - 1];
ATC = Matrix.Multiply(AT, C, 3, baselineOneEpoSum.Count, baselineOneEpoSum.Count);
ATCA = new double[3, 3];
ATCA = Matrix.Multiply(ATC, A, 3, baselineOneEpoSum.Count, 3);
ATComc = new double[3, 1];
ATComc = Matrix.Multiply(ATC, omc, 3, baselineOneEpoSum.Count, 1);
invATCA = new double[3, 3];
invATCA = Matrix.MatrixOpp(ATCA);
x = new double[3, 1];
x = Matrix.Multiply(invATCA, ATComc, 3, 3, 1);
roverReceiverPosition.X = roverReceiverPosition.X + x[0, 0];
roverReceiverPosition.Y = roverReceiverPosition.Y + x[1, 0];
roverReceiverPosition.Z = roverReceiverPosition.Z + x[2, 0];
baselineOneEpoSum.Clear();
}//for iteration
baseOneEpo.X = roverReceiverPosition.X - SPP_Result.X;
baseOneEpo.Y = roverReceiverPosition.Y - SPP_Result.Y;
baseOneEpo.Z = roverReceiverPosition.Z - SPP_Result.Z;
xx = xx + baseOneEpo.X;
yy = yy + baseOneEpo.Y;
zz = zz + baseOneEpo.Z;
BaselineResult.baselineResult.Add(baseOneEpo);
baseOneEpo = new BaseOneEpo();
baselineOneEpoSum.Clear();
}//for i
for (int p = 0; p < BaselineResult.baselineResult.Count; p++)
{
sumX = sumX + BaselineResult.baselineResult[p].X;
sumY = sumY + BaselineResult.baselineResult[p].Y;
sumZ = sumZ + BaselineResult.baselineResult[p].Z;
}
RelativePositioning_Result.X = sumX / BaselineResult.baselineResult.Count;
RelativePositioning_Result.Y = sumY / BaselineResult.baselineResult.Count;
RelativePositioning_Result.Z = sumZ / BaselineResult.baselineResult.Count;
return(true);
}
public bool SinglePointPositioning()
{
MasterReceiverPositionSum.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;
double sumCdtr = 0;
if (N_FileSum.n_FileDataSum.Count == 0)
{
MessageBox.Show("没有打开导航电文文件", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
return(false);
}
if (O_FlieSum.master_O_FileDataSum.Count == 0)
{
MessageBox.Show("没有打开基准站观测数据文件", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
return(false);
}
for (int i = 0; i < O_FlieSum.master_O_FileDataSum.Count; i++)
{
x = xx;
y = yy;
z = zz; //每个历元初值等于上个历元的定位结果，第一个历元为0
cdtr = 0; //第一次循环为0
xi = new double[4, 1];
do //解算一个历元
{
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.master_O_FileDataSum[i].o_FileDataHead.Sat_Num; j++) //循环一个历元中的全部卫星
{
TR = TimeToGPSTime(O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Epoch); //O_FileDataHead中的历元时刻转换为GPS时
int prnNum = FindBestEph(O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_PRN[j], O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Epoch);
if (prnNum == -1)
{
MessageBox.Show("卫星{0}没有对应的星历文件", O_FlieSum.master_O_FileDataSum[i].o_FileDataHead.Sat_PRN[j].ToString());
}
rho = O_FlieSum.master_O_FileDataSum[i].o_FileDataObs[j].C1;
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);
RSi = Math.Sqrt(Math.Pow(XSiW.XX - (O_FlieSum.master_O_FileHeadSum.Approx_Position.XX + x), 2) + Math.Pow(XSiW.YY - (O_FlieSum.master_O_FileHeadSum.Approx_Position.YY + y), 2) + Math.Pow(XSiW.ZZ - (O_FlieSum.master_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;
b0Si = (O_FlieSum.master_O_FileHeadSum.Approx_Position.XX + x - XSiW.XX) / rho;
b1Si = (O_FlieSum.master_O_FileHeadSum.Approx_Position.YY + y - XSiW.YY) / rho;
b2Si = (O_FlieSum.master_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];
masterReceiverPosition.X = O_FlieSum.master_O_FileHeadSum.Approx_Position.XX + xx;
masterReceiverPosition.Y = O_FlieSum.master_O_FileHeadSum.Approx_Position.YY + yy;
masterReceiverPosition.Z = O_FlieSum.master_O_FileHeadSum.Approx_Position.ZZ + zz;
masterReceiverPosition.Cdtr = (cdtr + xi[3, 0]) / Constant.SpeedOfLight;
MasterReceiverPositionSum.receiverPositionSum.Add(masterReceiverPosition);
masterReceiverPosition = new MasterReceiverPosition();
oneEpochDataSum.Clear();
}//for i
for (int i = 0; i < MasterReceiverPositionSum.receiverPositionSum.Count; i++)
{
sumX = sumX + MasterReceiverPositionSum.receiverPositionSum[i].X;
sumY = sumY + MasterReceiverPositionSum.receiverPositionSum[i].Y;
sumZ = sumZ + MasterReceiverPositionSum.receiverPositionSum[i].Z;
sumCdtr = sumCdtr + MasterReceiverPositionSum.receiverPositionSum[i].Cdtr;
}
SPP_Result.X = sumX / MasterReceiverPositionSum.receiverPositionSum.Count;
SPP_Result.Y = sumY / MasterReceiverPositionSum.receiverPositionSum.Count;
SPP_Result.Z = sumZ / MasterReceiverPositionSum.receiverPositionSum.Count;
SPP_Result.Cdtr = sumCdtr / MasterReceiverPositionSum.receiverPositionSum.Count;
return(true);
}
