/*标准单点定位*/
public static int pntpos(List <obs_s> obs, nav_t nav, spp_t spp, dcb_t dcb) //obs均已按照PRN号大小排列好
{
/*
* n为观测历元观测到的卫星总数，stat为解算状态
* vsat为卫星的状态 0为error，1为正常
* svh为卫星坐标和钟差计算的状态，-1为不正常
* rs包括卫星坐标和速度，dts包括卫星钟差和钟漂，azel包括卫星方位角和高度角
* var为卫星坐标和钟差的方差
*/
int n = obs.Count, stat = 1;
int[] vsat = new int[32], svh = new int[32];
double[][] rs = new double[n][], dts = new double[n][], azel = new double[n][];
double[] var = new double[n], resp = new double[n];
for (int i = 0; i < n; i++)
{
rs[i] = new double[6]; //卫星坐标和速度
dts[i] = new double[2]; //卫星的钟差和钟漂
azel[i] = new double[2]; //卫星的方位角和高度角
}
pppcmn.satpos(obs, nav, sat, clk, pcv, rs, dts, var, svh);
stat = estpos(spp, obs, dcb, nav, n, rs, dts, var, azel, vsat, svh);
for (int i = 0; i < n; i++)
{
int sat = int.Parse(obs[i].sprn.Substring(1, 2));
if (vsat[i] == 1)
{
spp.vsat[sat - 1] = 1;
}
}
return(stat);
}
public static int rescod(int iter, List <obs_s> obs, nav_t nav, int n, double[][] rs, double[][] dts, double[] vare, double[][] azel, int[] vsat,
dcb_t dcb, double[] x, matrix H, double[] v, double[] var, int[] svh)
{
double r, dtr, P, elmin = 15 * D2R, el = 0.0;;
int i, j, nv = 0;
double[] rr = new double[3], pos = new double[3], e = new double[3], vardcb = new double[1], ion = new double[2], trop = new double[2];
matrix p = new matrix(3, 1);
for (i = 0; i < 3; i++)
{
rr[i] = x[i];
}
dtr = x[3];
transcoor.ecef2pos(matrix.Array2matrix(rr), p);
pos = matrix.matrx2Array(p);
for (i = 0; i < n; i++)
{
vsat[i] = 0; azel[i][0] = azel[i][1] = 0;
if (svh[i] < 0)
{
continue;
}
if ((r = geodist(rs[i], rr, e)) < 0 || (el = pppcmn.satel(rr, rs[i], azel[i])) < elmin)
{
continue;
}
if ((P = Prange(obs[i], dcb, azel[i], vardcb)) == 0)
{
continue;
}
if (ioncorr(obs[i].t, obs[i].rtkt, nav, pos, azel[i], ion) != 1)
{
continue; //克罗布歇电离层模型
}
if (tropcorr(obs[i].t, pos, azel[i], trop) != 1)
{
continue; //对流层改正
}
/* pseudorange residual */ //伪距残差
v[nv] = P - (r + dtr - CLIGHT * dts[i][0] + ion[0] + trop[0]);
for (j = 1; j <= 4; j++)
{
H[nv + 1, j] = j <= 3 ? -e[j - 1] : 1.0; //观测矩阵
}
vsat[i] = 1;
//测量误差
var[nv++] = Math.Pow(100, 2) * (Math.Pow(0.003, 2) + Math.Pow(0.003, 2) / Math.Sin(azel[i][1])) + vardcb[0] + ion[1] + trop[1] + vare[i];
}
return(nv);
}
/*读取导航文件*/
public static void readnav(string path, nav_t nav)
{
using (StreamReader sr = new StreamReader(path))
{
string line = "";
readnavh(sr, nav);
if (nav.sys == "G" && nav.ver < 3)
{
while ((line = sr.ReadLine()) != null)//读取到最后一行为null结束
{
// string s;
navb nav_b = new navb();
nav_b.prn = "G" + String.Format("{0:D2}", int.Parse(line.Substring(0, 2).Trim()));
nav_b.t = new time(line.Substring(3, 19), "n");
rtklibcmn.str2time(line.Substring(3, 19), nav_b.rtktoc);
for (int i = 0, j = 22; i < 3; i++, j += 19)
{
string s = line.Substring(j, 19);
if (s.Contains("D"))
{
s = s.Replace("D", "e");
}
nav_b.af[i] = double.Parse(s);
}
for (int i = 0; i < 7; i++)
{
line = sr.ReadLine();
if (i == 2 && line != null)
{
string s = line.Substring(3, 19);
if (s.Contains("D"))
{
s = s.Replace("D", "e");
}
nav_b.toes = double.Parse(s);
nav_b.rtktoe = rtklibcmn.adjweek(rtklibcmn.gpst2time(1999, nav_b.toes), nav_b.rtktoc);
}
}
nav.navdata.Add(nav_b);
}
}
}
}
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 void readnavh(StreamReader sr, nav_t nav)
{
string line = "";
while (!line.Contains("END OF HEADER"))
{
line = sr.ReadLine();
if (line.Contains("RINEX VERSION / TYPE"))//只提取了第一行的信息
{
nav.ver = double.Parse(line.Substring(0, 9));
if (line.Substring(40, 1).Trim() == "")
{
nav.sys = "G";
}
else
{
nav.sys = line.Substring(40, 1).Trim();
}
}
if (line.Contains("ION ALPHA"))
{
string ss = line.Replace('D', 'E');
string[] s = ss.Split(new string[] { " " }, StringSplitOptions.RemoveEmptyEntries);
for (int i = 0; i < 4; i++)
{
nav.alpha[i] = double.Parse(s[i]);
}
}
if (line.Contains("ION BETA"))
{
string ss = line.Replace('D', 'E');
string[] s = ss.Split(new string[] { " " }, StringSplitOptions.RemoveEmptyEntries);
for (int i = 0; i < 4; i++)
{
nav.beta[i] = double.Parse(s[i]);
}
}
}
}
//obs均已按照PRN号大小排列好
public static int estpos(spp_t spp, List <obs_s> obs, dcb_t dcb, nav_t nav, int n, double[][] rs, double[][] dts,
double[] vare, double[][] azel, int[] vsat, int[] svh)
{
double[] v_ = new double[n], x = new double[4], var = new double[n];
matrix H_ = new matrix(n, 4), H, v, dx, Q, P;
int nv;
double sig;
int i, j, k, stat = 1;
for (i = 0; i < 3; i++)
{
x[i] = spp.rr[i];
}
for (i = 0; i < 10; i++)//迭代
{
nv = rescod(i, obs, nav, n, rs, dts, vare, azel, vsat, dcb, x, H_, v_, var, svh);
if (nv < 4)
{
break; //nv是参与解算的实际卫星数
}
H = new matrix(nv, 4); v = new matrix(nv, 1); P = new matrix(nv, nv);
for (j = 0; j < nv; j++)
{
sig = Math.Sqrt(var[j]);
v[j + 1, 1] = v_[j] / sig;
for (k = 1; k <= 4; k++)
{
H[j + 1, k] = H_[j + 1, k] / sig;
}
}
dx = matrix.inv(matrix.transp(H) * H) * matrix.transp(H) * v;
Q = matrix.transp(H) * H;
for (j = 0; j < 4; j++)
{
x[j] += dx[j + 1, 1];
}
if (Math.Sqrt(dx[1, 1] * dx[1, 1] + dx[2, 1] * dx[2, 1] + dx[3, 1] * dx[3, 1] + dx[4, 1] * dx[4, 1]) < 1E-4)
{
spp.dtr = x[3] / CLIGHT;
for (j = 0; j < 3; j++)
{
spp.rr[j] = x[j];
}
spp.tcurr.gpsec = obs[0].t.gpsec - spp.dtr;
spp.tcur = rtklibcmn.timeadd(obs[0].rtkt, -x[3] / CLIGHT);
for (j = 0; j < 6; j++)
{
spp.tcurr.calend[j] = obs[0].t.calend[j];
}
stat = valsol(azel, vsat, 15.0 * D2R, n, v, nv, 4);
return(stat);
}
}
return(1);
}
/*精密单点定位*/
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);
}
/*电离层改正用于标准单点定位*/
public static int ioncorr(time t, rtktime rtkt, nav_t nav, double[] pos, double[] azel, double[] ion)
{
ion[0] = ionmodel(t, rtkt, nav.alpha, nav.beta, pos, azel);
ion[1] = Math.Pow(ion[0] * ERR_BRDCI, 2);
return(1);
}
