/*读取观测值头文件*/ public static int readobsh(StreamReader sr, obs_t obs, station sta) { int flag = 0; string line = ""; while (!sr.EndOfStream) { line = sr.ReadLine(); if (line.Contains("RINEX VERSION / TYPE")) { sta.ver = double.Parse(line.Substring(0, 9)); } if (line.Contains("MARKER NAME")) { sta.name = line.Substring(0, 59).Trim(); } if (line.Contains("ANT # / TYPE"))//接收机天线类型 { sta.anxtype = line.Substring(20, 20); } if (line.Contains("ANTENNA: DELTA H/E/N")) { string[] ss = line.Split(new string[] { " " }, StringSplitOptions.RemoveEmptyEntries); sta.atxdel[2] = double.Parse(ss[0]); //ss: H E N sta.atxdel[0] = double.Parse(ss[1]); //atxdel: E N U sta.atxdel[1] = double.Parse(ss[2]); } if (line.Contains("APPROX POSITION XYZ")) { for (int i = 0, j = 0; i < 3; i++, j += 14) { sta.xyz[i] = double.Parse(line.Substring(j, 14)); } } if (line.Contains("# / TYPES OF OBSERV")) { obs.ntype = int.Parse(line.Substring(0, 6)); for (int i = 0, j = 10; i < obs.ntype; i++, j += 6) { if (j > 58) { line = sr.ReadLine(); j = 10; } obs.obstype[i] = line.Substring(j, 2); } } if (line.Contains("END OF HEADER")) { return(1); } } return(flag); }
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); } }
const double CLIGHT = 299792458.0; /* speed of light (m/s) */ /*读取观测值文件 支持ver 2.11*/ public static int readobs(string path, obs_t obs, station sta) { using (StreamReader sr = new StreamReader(path)) { if (readobsh(sr, obs, sta) == 1 && (sta.ver < 3 && sta.ver > 0)) { string line = ""; while (!sr.EndOfStream) { time tcurrent = new time(); string[] sprn = new string[64]; rtktime rtktcur = new rtktime(); int ns = 0; //每个历元观测到的卫星数 line = sr.ReadLine(); if (line != null) //读取含历元时间信息和卫星数的一行 { tcurrent = new time(line.Substring(0, 26), "o"); //读取时间信息 rtklibcmn.str2time(line.Substring(0, 26), rtktcur); ns = int.Parse(line.Substring(29, 3)); //读取当前历元观测到的卫星数 sprn = new string[ns]; //卫星prn for (int i = 0, j = 32; i < ns; i++, j += 3) { if (j >= 68) { line = sr.ReadLine(); j = 32; } sprn[i] = line.Substring(j, 3); } } for (int i = 0; i < ns; i++)//读取每个卫星相应的观测值 { obs_s sat = new obs_s(); sat.t = tcurrent; sat.rtkt = rtktcur; sat.sprn = sprn[i]; line = sr.ReadLine(); for (int k = 0, j = 0; k < obs.ntype; k++, j += 16)//读取一颗卫星的所有观测值信息 { obssat obsat = new obssat(); if (j >= 80) { line = sr.ReadLine(); j = 0; } obsat.type = obs.obstype[k]; if (j >= line.Length) { obsat.value = 0; } else { if (line.Substring(j, 14).Trim() == "") { obsat.value = 0; } else { obsat.value = double.Parse(line.Substring(j, 14)); } } sat.type_value.Add(obsat); } obs.obs_b.Add(sat); } obs.n += ns; } return(1); } else { return(0); } } }
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); }