/*读取地球自转文件*/
public static void readerp(string path, erp_t erp)
{
using (StreamReader sr = new StreamReader(path))
{
string line = "";
int state = 0;
erpb erpd = null;
while (!sr.EndOfStream)
{
line = sr.ReadLine();
if (line.Substring(0, 5).Trim() == "MJD")
{
state = 1; line = sr.ReadLine(); continue;
}
if (state == 1 && line != null)
{
string[] ss = line.Split(new string[] { " " }, StringSplitOptions.RemoveEmptyEntries);
erpd = new erpb();
if (ss.Length > 14)
{
erpd.mjd = double.Parse(ss[0]);
erpd.xp = double.Parse(ss[1]) * 1E-6 * AS2R;
erpd.yp = double.Parse(ss[2]) * 1E-6 * AS2R;
erpd.ut1_utc = double.Parse(ss[3]) * 1E-7;
erpd.lod = double.Parse(ss[4]) * 1E-7;
erpd.xpr = double.Parse(ss[12]) * 1E-6 * AS2R;
erpd.ypr = double.Parse(ss[13]) * 1E-6 * AS2R;
erp.erpdata.Add(erpd);
}
}
}
}
}
public static void tidedisp(time tutc, rtktime utc, double[] rr, erp_t erp, double[] disp)
{
time tut = new time();
rtktime ut = new rtktime();
double[] pos = new double[2], E = new double[9], erpv = new double[5], rs = new double[3], rm = new double[3];
double[] gmst = new double[1], drt = new double[3];
double r;
if (erp != null)
{
pppcmn.geterp(tutc, utc, erp, erpv);
}
tut.ut1 = tutc.utc + erpv[2];
ut = rtklibcmn.timeadd(utc, erpv[2]);
r = Math.Sqrt(rr[0] * rr[0] + rr[1] * rr[1] + rr[2] * rr[2]);
if (r <= 0)
{
return;
}
pos[0] = Math.Asin(rr[2] / r);
pos[1] = Math.Atan2(rr[1], rr[0]);
transcoor.mat_xyz2enu(matrix.Array2matrix(pos), E);
pppcmn.sunmoonpos(tutc, utc, erpv, rs, rm, gmst);
pppcmn.tide_solid(rs, rm, pos, E, gmst[0], drt);
for (int i = 0; i < 3; i++)
{
disp[i] += drt[i];
}
}
public static void propos(obs_t obss, dcb_t dcb, pcv_t pcv, station sta, nav_t nav, erp_t erp, List <result> res, RichTextBox text)
{
int nobs = 0;
rtktime solt = new rtktime();
string timestr = null;
obs_t obs = new obs_t();
ppp_t p3 = new ppp_t();
spp_t spp = new spp_t();
while ((nobs = inputobs(obs, obss)) > 0)
{
result re = new result();
solt = spp.tcur;
if (pntpos(obs.obs_b, nav, spp, dcb) == 0)
{
continue;
}
p3.spp = spp;
p3.soltime = spp.tcur;
if (solt.time_int != 0)
{
p3.tt = rtklibcmn.timediff(p3.soltime, solt);
}
if (pppos(p3, obs.obs_b, nav, dcb, sta, erp) != 1)
{
continue;
}
for (int j = 0; j < 3; j++)
{
spp.rr[j] = p3.x[j];
}
spp.dtr = p3.x[4];
re.time = obs.obs_b[0].t.calend;
re.X = p3.x[0]; re.Y = p3.x[1]; re.Z = p3.x[2];
timestr = re.time[0] + "/" + re.time[1] + "/" + re.time[2] + " " + re.time[3] + ":" + re.time[4] + ":" + re.time[5];
text.AppendText(string.Format("{0,-20}", timestr) + string.Format("{0,-20}", re.X) + string.Format("{0,-20}", re.Y) + string.Format("{0,-20}", re.Z) + "\r\n");
res.Add(re);
}
}
public static int res_ppp(List <obs_s> obs, station sta, ppp_t p3, int n, double[][] rs, double[][] dts, double[][] azel, double[] vare, erp_t erp,
dcb_t dcb, double[] x, double[] R, double[] v, matrix H, int[] svh)
{
double r, dtrp, vart = Math.Pow(0.01, 2), elmin = 15 * D2R;
double[] rr = new double[3], disp = new double[3], pos = new double[3], e = new double[3], meas = new double[2], varm = new double[2];
double[] dtdx = new double[3], dantr = new double[3], var = new double[nx * 2], dants = new double[2], phw = new double[1];
matrix pos_ = new matrix(3, 1);
int i, j, nv = 0, k, sat;
for (i = 0; i < 3; i++)
{
rr[i] = x[i];
}
/* earth tides correction */ //地球潮汐改正 固体潮
tidedisp(obs[0].t, rtklibcmn.gpst2utc(obs[0].rtkt), rr, erp, disp);
for (i = 0; i < 3; i++)
{
rr[i] += disp[i];
}
transcoor.ecef2pos(matrix.Array2matrix(rr), pos_);
for (i = 0; i < 3; i++)
{
pos[i] = pos_[i + 1, 1];
}
for (i = 0; i < 32; i++)
{
p3.vsat[i] = 0;
}
for (i = 0; i < n; i++)
{
sat = int.Parse(obs[i].sprn.Substring(1, 2));
//
if (p3.spp.vsat[sat - 1] == 0 || svh[i] < 0)
{
continue;
}
if ((r = geodist(rs[i], rr, e)) <= 0 || pppcmn.satel(rr, rs[i], azel[i]) < elmin)
{
continue;
}
dtrp = pppcmn.prectrop(obs[i].t, pos, azel[i], x[4], dtdx); //精密对流层模型
pppcmn.satanxpcv(rs[i], rr, pcv, dants, obs[i].sprn); //卫星天线相位偏差，返回每个频率的改正值
pppcmn.antxmodel(pcv, sta.atxdel, azel[i], dantr, sta.anxtype);
pppcmn.windup(obs[i].t, p3.soltime, rs[i], rr, phw);
if (corrmens(obs[i], dcb, pos, dantr, dants, phw, meas, varm) != 1)
{
continue;
}
/* satellite clock and tropospheric delay */ //卫星钟差和电离层延迟
r += -CLIGHT * dts[i][0] + dtrp;
for (j = 0; j < 2; j++)
{
if (meas[j] == 0)
{
continue;
}
v[nv] = meas[j] - r;
for (k = 0; k < nx; k++)
{
H[nv + 1, k + 1] = 0.0;
}
for (k = 0; k < 3; k++)
{
H[nv + 1, k + 1] = -e[k];
}
v[nv] -= x[3]; H[nv + 1, 4] = 1.0;
H[nv + 1, 5] = dtdx[0];
if (j == 0)
{
v[nv] -= x[4 + sat];
H[nv + 1, 5 + sat] = 1.0;
}
var[nv] = varm[j] + vare[i] + vart + varerr(azel[i][1], j);
if (Math.Abs(v[nv]) > 30)
{
continue;
}
if (j == 0)
{
p3.vsat[sat - 1] = 1;
}
nv++;
}
}
for (i = 0; i < nv; i++)
{
R[i] = var[i];
}
return(nv);
}
/*精密单点定位*/
public static int pppos(ppp_t pppt, List <obs_s> obs, nav_t nav, dcb_t dcb, station sta, erp_t erp)
{
int n = obs.Count, nv = 0, satprn;//当前历元观测的卫星个数
double[][] rs = new double[n][], dts = new double[n][], azel = new double[n][];
double[] var = new double[n], vare = new double[n];
int[] svh = new int[32];
double[] x = new double[nx], v = new double[2 * n], R = new double[2 * n];
matrix P = new matrix(37, 37);//
matrix H = new matrix(2 * n, nx);
for (int i = 0; i < n; i++)
{
rs[i] = new double[6]; //卫星坐标和速度
dts[i] = new double[2]; //卫星的钟差和钟漂
azel[i] = new double[2]; //卫星的方位角和高度角
}
//状态更新
udstate(pppt, dcb, obs);
pppcmn.satpos(obs, nav, sat, clk, pcv, rs, dts, vare, svh);
for (int i = 0; i < nx; i++)
{
x[i] = pppt.x[i];
}
for (int i = 0; i < 10; i++)//滤波迭代
{
if ((nv = res_ppp(obs, sta, pppt, n, rs, dts, azel, vare, erp, dcb, x, R, v, H, svh)) <= 0)
{
break;
}
for (int j = 0; j < nx; j++)
{
for (int k = 0; k < nx; k++)
{
P[j + 1, k + 1] = pppt.P[j + 1, k + 1];
}
}
kalman(x, R, v, P, H, nv);
}
for (int i = 0; i < nx; i++)
{
pppt.x[i] = x[i];
for (int j = 0; j < nx; j++)
{
pppt.P[i + 1, j + 1] = P[i + 1, j + 1];
}
}
for (int i = 0; i < 3; i++)
{
pppt.spp.rr[i] = pppt.x[i];
}
for (int i = 0; i < n; i++)
{
satprn = int.Parse(obs[i].sprn.Substring(1, 2));
if (pppt.vsat[satprn - 1] == 0)
{
continue;
}
pppt.outc[satprn - 1] = 0;
}
return(1);
}
