public static int corrmens(obs_s obs, dcb_t dcb, double[] pos, double[] dantr, double[] dants,
double[] phw, double[] meas, double[] var)
{
double c1, c2, L1 = 0.0, L2 = 0.0, P1 = 0.0, P2 = 0.0, P1_C1, gamma, lam1, lam2;
int i = 0, j = 1, k, sat;
lam1 = CLIGHT / FREQ1; lam2 = CLIGHT / FREQ2;
gamma = Math.Pow(FREQ1, 2) / Math.Pow(FREQ2, 2);
c1 = gamma / (gamma - 1.0); /* f1^2/(f1^2-f2^2) */
c2 = -1.0 / (gamma - 1.0); /* -f2^2/(f1^2-f2^2) */
sat = int.Parse(obs.sprn.Substring(1, 2));
foreach (var v in obs.type_value)
{
if (v.type == "L1")
{
L1 = v.value * lam1;
}
if (v.type == "L2")
{
L2 = v.value * lam2;
}
if (v.type == "P2")
{
P2 = v.value;
}
if (v.type == "C1")
{
P1 = v.value;
}
}
P1_C1 = dcb.dcbdata[sat - 1].P1_C1; //L1C/A
if (L1 == 0.0 || L2 == 0.0 || P1 == 0.0 || P2 == 0.0)
{
return(0);
}
meas[0] = c1 * L1 + c2 * L2 - (c1 * lam1 + c2 * lam2) * phw[0];
P1 += P1_C1;
meas[1] = c1 * P1 + c2 * P2; //伪距IF组合
var[1] = Math.Pow(ERR_CBIAS, 2);
for (k = 0; k < 2; k++)
{
if (dants != null)
{
meas[k] -= c1 * dants[i] + c2 * dants[j];
}
if (dantr != null)
{
meas[k] -= c1 * dantr[i] + c2 * dantr[j];
}
}
return(1);
}
public static int inputobs(obs_t obs, obs_t obss)
{
int n = 0, sat;
time t = new time();
List <obs_s> obs_ = new List <obs_s>();
for (int i = 0; i < 32; i++)
{
obs_.Add(null);
}
obs.obs_b = new List <obs_s>();
if (0 <= iobss && iobss < obss.n)
{
t = obss.obs_b[iobss].t;
}
for (int i = 0; i < obss.obs_b.Count; i++)
{
if ((iobss + i) >= obss.obs_b.Count)
{
n = i; break;
}
if ((obss.obs_b[iobss + i].t.gpsec - t.gpsec) > DT)
{
n = i; break;
}
}
for (int i = 0; i < n; i++)
{
sat = int.Parse(obss.obs_b[iobss + i].sprn.Substring(1, 2));
obs_[sat - 1] = obss.obs_b[iobss + i];
}
foreach (var v in obs_)
{
if (v != null)
{
obs.obs_b.Add(v);
}
}
iobss += n;
return(n);//n为当前历元观测的卫星数
}
/*GF组合探测周跳*/
public static void detslp_gf(ppp_t p3, List <obs_s> obs, int n)
{
double g0, g1;
int i, sat;
for (i = 0; i < n; i++)
{
sat = int.Parse(obs[i].sprn.Substring(1, 2));
if ((g1 = gfmeas(obs[i])) == 0)
{
continue;
}
g0 = p3.gf[sat - 1];
p3.gf[sat - 1] = g1;
if (g0 != 0 && Math.Abs(g1 - g0) > 0.5)
{
p3.cyslip[sat - 1] = 1;
}
}
}
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 void upbias_ppp(ppp_t p3, List <obs_s> obs, dcb_t dcb)
{
double[] meas = new double[2], var = new double[2], bias = new double[32], pos = new double[3], rr = new double[3], phw = new double[1];
double offset = 0.0;
matrix pos_ = new matrix(3, 1);
int i, j, k, sat, n = obs.Count;
for (i = 0; i < 32; i++)
{
p3.cyslip[i] = 0;
}
detslp_gf(p3, obs, n);
/* reset phase-bias if expire obs outage counter */
for (i = 0; i < 32; i++)
{
if (++p3.outc[i] > 5)
{
initx(p3, 0.0, 0.0, i + 5);
}
}
for (i = 0; i < 3; i++)
{
rr[i] = p3.spp.rr[i];
}
transcoor.ecef2pos(matrix.Array2matrix(rr), pos_);
for (i = 0; i < 3; i++)
{
pos[i] = pos_[i + 1, 1];
}
for (i = k = 0; i < n; i++)
{
sat = int.Parse(obs[i].sprn.Substring(1, 2));
j = sat + 4;
if ((corrmens(obs[i], dcb, pos, null, null, phw, meas, var)) != 1)
{
continue;
}
;
bias[i] = meas[0] - meas[1];
if (p3.x[j] == 0 || p3.cyslip[sat - 1] == 1)
{
continue;
}
offset += bias[i] - p3.x[j];
k++;
}
/* correct phase-code jump to enssure phase-code coherency */
if (k >= 2 && Math.Abs(offset / k) > 0.0005 * CLIGHT)
{
for (i = 0; i < 32; i++)
{
j = i + 5;
if (p3.x[j] != 0)
{
p3.x[j] += offset / k;
}
}
}
for (i = 0; i < n; i++)
{
sat = int.Parse(obs[i].sprn.Substring(1, 2));
j = sat + 4;
p3.P[j + 1, j + 1] += Math.Pow(1E-4, 2) * Math.Abs(p3.tt);
if (p3.x[j] != 0 && p3.cyslip[sat - 1] != 1)
{
continue; //已初始化且不发生周跳
}
if (bias[i] == 0)
{
continue;
}
initx(p3, bias[i], VAR_BIAS, j);
}
}
