/*! * \file rtklib_rtkpos.cc * \brief rtklib ppp-related functions * \authors * * This is a derived work from RTKLIB http://www.rtklib.com/ * The original source code at https://github.com/tomojitakasu/RTKLIB is * released under the BSD 2-clause license with an additional exclusive clause * that does not apply here. This additional clause is reproduced below: * * " The software package includes some companion executive binaries or shared * libraries necessary to execute APs on Windows. These licenses succeed to the * original ones of these software. " * * Neither the executive binaries nor the shared libraries are required by, used * or included in GNSS-SDR. * * ----------------------------------------------------------------------------- * Copyright (C) 2007-2013, T. Takasu * Copyright (C) 2017, Javier Arribas * Copyright (C) 2017, Carles Fernandez * All rights reserved. * * SPDX-License-Identifier: BSD-2-Clause * ----------------------------------------------------------------------------- */ #include "rtklib_rtkpos.h" #include "rtklib_ephemeris.h" #include "rtklib_lambda.h" #include "rtklib_pntpos.h" #include "rtklib_ppp.h" #include "rtklib_tides.h" #include #include #include #include static int resamb_WLNL(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)), const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute__((unused)), const nav_t *nav __attribute((unused)), const double *azel __attribute((unused))) { return 0; } static int resamb_TCAR(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)), const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute((unused)), const nav_t *nav __attribute((unused)), const double *azel __attribute((unused))) { return 0; } /* global variables ----------------------------------------------------------*/ static int statlevel = 0; /* rtk status output level (0:off) */ static FILE *fp_stat = nullptr; /* rtk status file pointer */ static char file_stat[1024] = ""; /* rtk status file original path */ static gtime_t time_stat = {0, 0}; /* rtk status file time */ /* open solution status file --------------------------------------------------- * open solution status file and set output level * args : char *file I rtk status file * int level I rtk status level (0: off) * return : status (1:ok,0:error) * notes : file can constain time keywords (%Y,%y,%m...) defined in reppath(). * The time to replace keywords is based on UTC of CPU time. * output : solution status file record format * * $POS,week,tow,stat,posx,posy,posz,posxf,posyf,poszf * week/tow : gps week no/time of week (s) * stat : solution status * posx/posy/posz : position x/y/z ecef (m) float * posxf/posyf/poszf : position x/y/z ecef (m) fixed * * $VELACC,week,tow,stat,vele,veln,velu,acce,accn,accu,velef,velnf,veluf,accef,accnf,accuf * week/tow : gps week no/time of week (s) * stat : solution status * vele/veln/velu : velocity e/n/u (m/s) float * acce/accn/accu : acceleration e/n/u (m/s^2) float * velef/velnf/veluf : velocity e/n/u (m/s) fixed * accef/accnf/accuf : acceleration e/n/u (m/s^2) fixed * * $CLK,week,tow,stat,clk1,clk2,clk3,clk4 * week/tow : gps week no/time of week (s) * stat : solution status * clk1 : receiver clock bias GPS (ns) * clk2 : receiver clock bias GLO-GPS (ns) * clk3 : receiver clock bias GAL-GPS (ns) * clk4 : receiver clock bias BDS-GPS (ns) * * $ION,week,tow,stat,sat,az,el,ion,ion-fixed * week/tow : gps week no/time of week (s) * stat : solution status * sat : satellite id * az/el : azimuth/elevation angle(deg) * ion : vertical ionospheric delay L1 (m) float * ion-fixed: vertical ionospheric delay L1 (m) fixed * * $TROP,week,tow,stat,rcv,ztd,ztdf * week/tow : gps week no/time of week (s) * stat : solution status * rcv : receiver (1:rover,2:base station) * ztd : zenith total delay (m) float * ztdf : zenith total delay (m) fixed * * $HWBIAS,week,tow,stat,frq,bias,biasf * week/tow : gps week no/time of week (s) * stat : solution status * frq : frequency (1:L1,2:L2,...) * bias : h/w bias coefficient (m/MHz) float * biasf : h/w bias coefficient (m/MHz) fixed * * $SAT,week,tow,sat,frq,az,el,resp,resc,vsat,snr,fix,slip,lock,outc,slipc,rejc * week/tow : gps week no/time of week (s) * sat/frq : satellite id/frequency (1:L1,2:L2,...) * az/el : azimuth/elevation angle (deg) * resp : pseudorange residual (m) * resc : carrier-phase residual (m) * vsat : valid data flag (0:invalid,1:valid) * snr : signal strength (dbHz) * fix : ambiguity flag (0:no data,1:float,2:fixed,3:hold,4:ppp) * slip : cycle-slip flag (bit1:slip,bit2:parity unknown) * lock : carrier-lock count * outc : data outage count * slipc : cycle-slip count * rejc : data reject (outlier) count * *-----------------------------------------------------------------------------*/ int rtkopenstat(const char *file, int level) { gtime_t time = utc2gpst(timeget()); std::string path; trace(3, "rtkopenstat: file=%s level=%d\n", file, level); if (level <= 0) { return 0; } reppath(file, path, time, "", ""); if (!(fp_stat = fopen(path.data(), "we"))) { trace(1, "rtkopenstat: file open error path=%s\n", path.data()); return 0; } if (strlen(file) < 1025) { std::strncpy(file_stat, file, 1024); file_stat[1023] = '\0'; } else { trace(1, "File name is too long"); } time_stat = time; statlevel = level; return 1; } /* close solution status file -------------------------------------------------- * close solution status file * args : none * return : none *-----------------------------------------------------------------------------*/ void rtkclosestat() { trace(3, "rtkclosestat:\n"); if (fp_stat) { fclose(fp_stat); } fp_stat = nullptr; file_stat[0] = '\0'; statlevel = 0; } /* write solution status to buffer -------------------------------------------*/ void rtkoutstat(rtk_t *rtk, char *buff __attribute__((unused))) { ssat_t *ssat; double tow; double pos[3]; double vel[3]; double acc[3]; double vela[3] = {0}; double acca[3] = {0}; double xa[3]; int i; int j; int week; int est; int nfreq; int nf = NF_RTK(&rtk->opt); if (statlevel <= 0 || !fp_stat) { return; } trace(3, "outsolstat:\n"); /* swap solution status file */ swapsolstat(); est = rtk->opt.mode >= PMODE_DGPS; nfreq = est ? nf : 1; tow = time2gpst(rtk->sol.time, &week); /* receiver position */ if (est) { for (i = 0; i < 3; i++) { xa[i] = i < rtk->na ? rtk->xa[i] : 0.0; } fprintf(fp_stat, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow, rtk->sol.stat, rtk->x[0], rtk->x[1], rtk->x[2], xa[0], xa[1], xa[2]); } else { fprintf(fp_stat, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow, rtk->sol.stat, rtk->sol.rr[0], rtk->sol.rr[1], rtk->sol.rr[2], 0.0, 0.0, 0.0); } /* receiver velocity and acceleration */ if (est && rtk->opt.dynamics) { ecef2pos(rtk->sol.rr, pos); ecef2enu(pos, rtk->x + 3, vel); ecef2enu(pos, rtk->x + 6, acc); if (rtk->na >= 6) { ecef2enu(pos, rtk->xa + 3, vela); } if (rtk->na >= 9) { ecef2enu(pos, rtk->xa + 6, acca); } fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], acc[0], acc[1], acc[2], vela[0], vela[1], vela[2], acca[0], acca[1], acca[2]); } else { ecef2pos(rtk->sol.rr, pos); ecef2enu(pos, rtk->sol.rr + 3, vel); fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); } /* receiver clocks */ fprintf(fp_stat, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", week, tow, rtk->sol.stat, 1, rtk->sol.dtr[0] * 1E9, rtk->sol.dtr[1] * 1E9, rtk->sol.dtr[2] * 1E9, rtk->sol.dtr[3] * 1E9); /* ionospheric parameters */ if (est && rtk->opt.ionoopt == IONOOPT_EST) { for (i = 0; i < MAXSAT; i++) { ssat = rtk->ssat + i; if (!ssat->vs) { continue; } auto id = satno2id(i + 1); j = II_RTK(i + 1, &rtk->opt); xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; fprintf(fp_stat, "$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n", week, tow, rtk->sol.stat, id.data(), ssat->azel[0] * R2D, ssat->azel[1] * R2D, rtk->x[j], xa[0]); } } /* tropospheric parameters */ if (est && (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG)) { for (i = 0; i < 2; i++) { j = IT_RTK(i, &rtk->opt); xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; fprintf(fp_stat, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, i + 1, rtk->x[j], xa[0]); } } /* receiver h/w bias */ if (est && rtk->opt.glomodear == 2) { for (i = 0; i < nfreq; i++) { j = IL_RTK(i, &rtk->opt); xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; fprintf(fp_stat, "$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, i + 1, rtk->x[j], xa[0]); } } if (rtk->sol.stat == SOLQ_NONE || statlevel <= 1) { return; } /* residuals and status */ for (i = 0; i < MAXSAT; i++) { ssat = rtk->ssat + i; if (!ssat->vs) { continue; } auto id = satno2id(i + 1); for (j = 0; j < nfreq; j++) { fprintf(fp_stat, "$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n", week, tow, id.data(), j + 1, ssat->azel[0] * R2D, ssat->azel[1] * R2D, ssat->resp[j], ssat->resc[j], ssat->vsat[j], ssat->snr[j] * 0.25, ssat->fix[j], ssat->slip[j] & 3, ssat->lock[j], ssat->outc[j], ssat->slipc[j], ssat->rejc[j]); } } } /* swap solution status file -------------------------------------------------*/ void swapsolstat() { gtime_t time = utc2gpst(timeget()); std::string path; if (static_cast(time2gpst(time, nullptr) / INT_SWAP_STAT) == static_cast(time2gpst(time_stat, nullptr) / INT_SWAP_STAT)) { return; } time_stat = time; if (!reppath(file_stat, path, time, "", "")) { return; } if (fp_stat) { fclose(fp_stat); } if (!(fp_stat = fopen(path.data(), "we"))) { trace(2, "swapsolstat: file open error path=%s\n", path.data()); return; } trace(3, "swapsolstat: path=%s\n", path.data()); } /* output solution status ----------------------------------------------------*/ void outsolstat(rtk_t *rtk) { ssat_t *ssat; double tow; double pos[3]; double vel[3]; double acc[3]; double vela[3] = {0}; double acca[3] = {0}; double xa[3]; int i; int j; int week; int est; int nfreq; int nf = NF_RTK(&rtk->opt); if (statlevel <= 0 || !fp_stat) { return; } trace(3, "outsolstat:\n"); /* swap solution status file */ swapsolstat(); est = rtk->opt.mode >= PMODE_DGPS; nfreq = est ? nf : 1; tow = time2gpst(rtk->sol.time, &week); /* receiver position */ if (est) { for (i = 0; i < 3; i++) { xa[i] = i < rtk->na ? rtk->xa[i] : 0.0; } fprintf(fp_stat, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow, rtk->sol.stat, rtk->x[0], rtk->x[1], rtk->x[2], xa[0], xa[1], xa[2]); } else { fprintf(fp_stat, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow, rtk->sol.stat, rtk->sol.rr[0], rtk->sol.rr[1], rtk->sol.rr[2], 0.0, 0.0, 0.0); } /* receiver velocity and acceleration */ if (est && rtk->opt.dynamics) { ecef2pos(rtk->sol.rr, pos); ecef2enu(pos, rtk->x + 3, vel); ecef2enu(pos, rtk->x + 6, acc); if (rtk->na >= 6) { ecef2enu(pos, rtk->xa + 3, vela); } if (rtk->na >= 9) { ecef2enu(pos, rtk->xa + 6, acca); } fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], acc[0], acc[1], acc[2], vela[0], vela[1], vela[2], acca[0], acca[1], acca[2]); } else { ecef2pos(rtk->sol.rr, pos); ecef2enu(pos, rtk->sol.rr + 3, vel); fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); } /* receiver clocks */ fprintf(fp_stat, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", week, tow, rtk->sol.stat, 1, rtk->sol.dtr[0] * 1E9, rtk->sol.dtr[1] * 1E9, rtk->sol.dtr[2] * 1E9, rtk->sol.dtr[3] * 1E9); /* ionospheric parameters */ if (est && rtk->opt.ionoopt == IONOOPT_EST) { for (i = 0; i < MAXSAT; i++) { ssat = rtk->ssat + i; if (!ssat->vs) { continue; } auto id = satno2id(i + 1); j = II_RTK(i + 1, &rtk->opt); xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; fprintf(fp_stat, "$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n", week, tow, rtk->sol.stat, id.data(), ssat->azel[0] * R2D, ssat->azel[1] * R2D, rtk->x[j], xa[0]); } } /* tropospheric parameters */ if (est && (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG)) { for (i = 0; i < 2; i++) { j = IT_RTK(i, &rtk->opt); xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; fprintf(fp_stat, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, i + 1, rtk->x[j], xa[0]); } } /* receiver h/w bias */ if (est && rtk->opt.glomodear == 2) { for (i = 0; i < nfreq; i++) { j = IL_RTK(i, &rtk->opt); xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; fprintf(fp_stat, "$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, i + 1, rtk->x[j], xa[0]); } } if (rtk->sol.stat == SOLQ_NONE || statlevel <= 1) { return; } /* residuals and status */ for (i = 0; i < MAXSAT; i++) { ssat = rtk->ssat + i; if (!ssat->vs) { continue; } auto id = satno2id(i + 1); for (j = 0; j < nfreq; j++) { fprintf(fp_stat, "$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n", week, tow, id.data(), j + 1, ssat->azel[0] * R2D, ssat->azel[1] * R2D, ssat->resp[j], ssat->resc[j], ssat->vsat[j], ssat->snr[j] * 0.25, ssat->fix[j], ssat->slip[j] & 3, ssat->lock[j], ssat->outc[j], ssat->slipc[j], ssat->rejc[j]); } } } /* save error message --------------------------------------------------------*/ void errmsg(rtk_t *rtk, const char *format, ...) { char buff[256]; char tstr[32]; int n; va_list ap; time2str(rtk->sol.time, tstr, 2); n = std::snprintf(buff, sizeof(buff), "%s: ", tstr + 11); va_start(ap, format); n += vsnprintf(buff + n, sizeof(buff) - n, format, ap); va_end(ap); n = n < MAXERRMSG - rtk->neb ? n : MAXERRMSG - rtk->neb; memcpy(rtk->errbuf + rtk->neb, buff, n); rtk->neb += n; trace(2, "%s", buff); } /* single-differenced observable ---------------------------------------------*/ double sdobs(const obsd_t *obs, int i, int j, int f) { double pi = f < NFREQ ? obs[i].L[f] : obs[i].P[f - NFREQ]; double pj = f < NFREQ ? obs[j].L[f] : obs[j].P[f - NFREQ]; return pi == 0.0 || pj == 0.0 ? 0.0 : pi - pj; } /* single-differenced geometry-free linear combination of phase --------------*/ double gfobs_L1L2(const obsd_t *obs, int i, int j, const double *lam) { double pi = sdobs(obs, i, j, 0) * lam[0]; double pj = sdobs(obs, i, j, 1) * lam[1]; return pi == 0.0 || pj == 0.0 ? 0.0 : pi - pj; } double gfobs_L1L5(const obsd_t *obs, int i, int j, const double *lam) { double pi = sdobs(obs, i, j, 0) * lam[0]; double pj = sdobs(obs, i, j, 2) * lam[2]; return pi == 0.0 || pj == 0.0 ? 0.0 : pi - pj; } /* single-differenced measurement error variance -----------------------------*/ double varerr(int sat __attribute((unused)), int sys, double el, double bl, double dt, int f, const prcopt_t *opt) { double a; double b; double c = opt->err[3] * bl / 1e4; double d = SPEED_OF_LIGHT_M_S * opt->sclkstab * dt; double fact = 1.0; double sinel = sin(el); int i = sys == SYS_GLO ? 1 : (sys == SYS_GAL ? 2 : 0); int nf = NF_RTK(opt); /* extended error model */ if (f >= nf && opt->exterr.ena[0]) { /* code */ a = opt->exterr.cerr[i][(f - nf) * 2]; b = opt->exterr.cerr[i][1 + (f - nf) * 2]; if (sys == SYS_SBS) { a *= EFACT_SBS; b *= EFACT_SBS; } } else if (f < nf && opt->exterr.ena[1]) { /* phase */ a = opt->exterr.perr[i][f * 2]; b = opt->exterr.perr[i][1 + f * 2]; if (sys == SYS_SBS) { a *= EFACT_SBS; b *= EFACT_SBS; } } else { /* normal error model */ if (f >= nf) { fact = opt->eratio[f - nf]; } if (fact <= 0.0) { fact = opt->eratio[0]; } fact *= sys == SYS_GLO ? EFACT_GLO : (sys == SYS_SBS ? EFACT_SBS : EFACT_GPS); a = fact * opt->err[1]; b = fact * opt->err[2]; } return 2.0 * (opt->ionoopt == IONOOPT_IFLC ? 3.0 : 1.0) * (a * a + b * b / sinel / sinel + c * c) + d * d; } /* baseline length -----------------------------------------------------------*/ double baseline(const double *ru, const double *rb, double *dr) { int i; for (i = 0; i < 3; i++) { dr[i] = ru[i] - rb[i]; } return norm_rtk(dr, 3); } /* initialize state and covariance -------------------------------------------*/ void initx_rtk(rtk_t *rtk, double xi, double var, int i) { int j; rtk->x[i] = xi; for (j = 0; j < rtk->nx; j++) { rtk->P[i + j * rtk->nx] = rtk->P[j + i * rtk->nx] = i == j ? var : 0.0; } } /* select common satellites between rover and reference station --------------*/ int selsat(const obsd_t *obs, const double *azel, int nu, int nr, const prcopt_t *opt, int *sat, int *iu, int *ir) { int i; int j; int k = 0; trace(3, "selsat : nu=%d nr=%d\n", nu, nr); for (i = 0, j = nu; i < nu && j < nu + nr; i++, j++) { if (obs[i].sat < obs[j].sat) { j--; } else if (obs[i].sat > obs[j].sat) { i--; } else if (azel[1 + j * 2] >= opt->elmin) { /* elevation at base station */ sat[k] = obs[i].sat; iu[k] = i; ir[k++] = j; trace(4, "(%2d) sat=%3d iu=%2d ir=%2d\n", k - 1, obs[i].sat, i, j); } } return k; } /* temporal update of position/velocity/acceleration -------------------------*/ void udpos(rtk_t *rtk, double tt) { double *F; double *FP; double *xp; double pos[3]; double Q[9] = {0}; double Qv[9]; double var = 0.0; int i; int j; trace(3, "udpos : tt=%.3f\n", tt); /* fixed mode */ if (rtk->opt.mode == PMODE_FIXED) { for (i = 0; i < 3; i++) { initx_rtk(rtk, rtk->opt.ru[i], 1E-8, i); } return; } /* initialize position for first epoch */ if (norm_rtk(rtk->x, 3) <= 0.0) { for (i = 0; i < 3; i++) { initx_rtk(rtk, rtk->sol.rr[i], VAR_POS, i); } if (rtk->opt.dynamics) { for (i = 3; i < 6; i++) { initx_rtk(rtk, rtk->sol.rr[i], VAR_VEL, i); } for (i = 6; i < 9; i++) { initx_rtk(rtk, 1E-6, VAR_ACC, i); } } } /* static mode */ if (rtk->opt.mode == PMODE_STATIC) { return; } /* kinmatic mode without dynamics */ if (!rtk->opt.dynamics) { for (i = 0; i < 3; i++) { initx_rtk(rtk, rtk->sol.rr[i], VAR_POS, i); } return; } /* check variance of estimated position */ for (i = 0; i < 3; i++) { var += rtk->P[i + i * rtk->nx]; } var /= 3.0; if (var > VAR_POS) { /* reset position with large variance */ for (i = 0; i < 3; i++) { initx_rtk(rtk, rtk->sol.rr[i], VAR_POS, i); } for (i = 3; i < 6; i++) { initx_rtk(rtk, rtk->sol.rr[i], VAR_VEL, i); } for (i = 6; i < 9; i++) { initx_rtk(rtk, 1E-6, VAR_ACC, i); } trace(2, "reset rtk position due to large variance: var=%.3f\n", var); return; } /* state transition of position/velocity/acceleration */ F = eye(rtk->nx); FP = mat(rtk->nx, rtk->nx); xp = mat(rtk->nx, 1); for (i = 0; i < 6; i++) { F[i + (i + 3) * rtk->nx] = tt; } /* x=F*x, P=F*P*F+Q */ matmul("NN", rtk->nx, 1, rtk->nx, 1.0, F, rtk->x, 0.0, xp); matcpy(rtk->x, xp, rtk->nx, 1); matmul("NN", rtk->nx, rtk->nx, rtk->nx, 1.0, F, rtk->P, 0.0, FP); matmul("NT", rtk->nx, rtk->nx, rtk->nx, 1.0, FP, F, 0.0, rtk->P); /* process noise added to only acceleration */ Q[0] = Q[4] = std::pow(rtk->opt.prn[3], 2.0); Q[8] = std::pow(rtk->opt.prn[4], 2.0); ecef2pos(rtk->x, pos); covecef(pos, Q, Qv); for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { rtk->P[i + 6 + (j + 6) * rtk->nx] += Qv[i + j * 3]; } } free(F); free(FP); free(xp); } /* temporal update of ionospheric parameters ---------------------------------*/ void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns) { double el; double fact; int i; int j; trace(3, "udion : tt=%.1f bl=%.0f ns=%d\n", tt, bl, ns); for (i = 1; i <= MAXSAT; i++) { j = II_RTK(i, &rtk->opt); if (rtk->x[j] != 0.0 && rtk->ssat[i - 1].outc[0] > GAP_RESION && rtk->ssat[i - 1].outc[1] > GAP_RESION) { rtk->x[j] = 0.0; } } for (i = 0; i < ns; i++) { j = II_RTK(sat[i], &rtk->opt); if (rtk->x[j] == 0.0) { initx_rtk(rtk, 1E-6, std::pow(rtk->opt.std[1] * bl / 1e4, 2.0), j); } else { /* elevation dependent factor of process noise */ el = rtk->ssat[sat[i] - 1].azel[1]; fact = cos(el); rtk->P[j + j * rtk->nx] += std::pow(rtk->opt.prn[1] * bl / 1e4 * fact, 2.0) * tt; } } } /* temporal update of tropospheric parameters --------------------------------*/ void udtrop(rtk_t *rtk, double tt, double bl __attribute((unused))) { int i; int j; int k; trace(3, "udtrop : tt=%.1f\n", tt); for (i = 0; i < 2; i++) { j = IT_RTK(i, &rtk->opt); if (rtk->x[j] == 0.0) { initx_rtk(rtk, INIT_ZWD, std::pow(rtk->opt.std[2], 2.0), j); /* initial zwd */ if (rtk->opt.tropopt >= TROPOPT_ESTG) { for (k = 0; k < 2; k++) { initx_rtk(rtk, 1e-6, VAR_GRA, ++j); } } } else { rtk->P[j + j * rtk->nx] += std::pow(rtk->opt.prn[2], 2.0) * tt; if (rtk->opt.tropopt >= TROPOPT_ESTG) { for (k = 0; k < 2; k++) { rtk->P[++j * (1 + rtk->nx)] += std::pow(rtk->opt.prn[2] * 0.3, 2.0) * fabs(rtk->tt); } } } } } /* temporal update of receiver h/w biases ------------------------------------*/ void udrcvbias(rtk_t *rtk, double tt) { int i; int j; trace(3, "udrcvbias: tt=%.1f\n", tt); for (i = 0; i < NFREQGLO; i++) { j = IL_RTK(i, &rtk->opt); if (rtk->x[j] == 0.0) { initx_rtk(rtk, 1E-6, VAR_HWBIAS, j); } /* hold to fixed solution */ else if (rtk->nfix >= rtk->opt.minfix && rtk->sol.ratio > rtk->opt.thresar[0]) { initx_rtk(rtk, rtk->xa[j], rtk->Pa[j + j * rtk->na], j); } else { rtk->P[j + j * rtk->nx] += std::pow(PRN_HWBIAS, 2.0) * tt; } } } /* detect cycle slip by LLI --------------------------------------------------*/ void detslp_ll(rtk_t *rtk, const obsd_t *obs, int i, int rcv) { unsigned int slip; unsigned int LLI; int f; int sat = obs[i].sat; trace(3, "detslp_ll: i=%d rcv=%d\n", i, rcv); for (f = 0; f < rtk->opt.nf; f++) { if (obs[i].L[f] == 0.0) { continue; } /* restore previous LLI */ if (rcv == 1) { LLI = getbitu(&rtk->ssat[sat - 1].slip[f], 0, 2); /* rover */ } else { LLI = getbitu(&rtk->ssat[sat - 1].slip[f], 2, 2); /* base */ } /* detect slip by cycle slip flag in LLI */ if (rtk->tt >= 0.0) { /* forward */ if (obs[i].LLI[f] & 1) { errmsg(rtk, "slip detected forward (sat=%2d rcv=%d F=%d LLI=%x)\n", sat, rcv, f + 1, obs[i].LLI[f]); } slip = obs[i].LLI[f]; } else { /* backward */ if (LLI & 1) { errmsg(rtk, "slip detected backward (sat=%2d rcv=%d F=%d LLI=%x)\n", sat, rcv, f + 1, LLI); } slip = LLI; } /* detect slip by parity unknown flag transition in LLI */ if (((LLI & 2) && !(obs[i].LLI[f] & 2)) || (!(LLI & 2) && (obs[i].LLI[f] & 2))) { errmsg(rtk, "slip detected half-cyc (sat=%2d rcv=%d F=%d LLI=%x->%x)\n", sat, rcv, f + 1, LLI, obs[i].LLI[f]); slip |= 1; } /* save current LLI */ if (rcv == 1) { setbitu(&rtk->ssat[sat - 1].slip[f], 0, 2, obs[i].LLI[f]); } else { setbitu(&rtk->ssat[sat - 1].slip[f], 2, 2, obs[i].LLI[f]); } /* save slip and half-cycle valid flag */ rtk->ssat[sat - 1].slip[f] |= static_cast(slip); rtk->ssat[sat - 1].half[f] = (obs[i].LLI[f] & 2) ? 0 : 1; } } /* detect cycle slip by L1-L2 geometry free phase jump -----------------------*/ void detslp_gf_L1L2(rtk_t *rtk, const obsd_t *obs, int i, int j, const nav_t *nav) { int sat = obs[i].sat; double g0; double g1; trace(3, "detslp_gf_L1L2: i=%d j=%d\n", i, j); if (rtk->opt.nf <= 1 || (g1 = gfobs_L1L2(obs, i, j, nav->lam[sat - 1])) == 0.0) { return; } g0 = rtk->ssat[sat - 1].gf; rtk->ssat[sat - 1].gf = g1; if (g0 != 0.0 && fabs(g1 - g0) > rtk->opt.thresslip) { rtk->ssat[sat - 1].slip[0] |= 1; rtk->ssat[sat - 1].slip[1] |= 1; errmsg(rtk, "slip detected (sat=%2d GF_L1_L2=%.3f %.3f)\n", sat, g0, g1); } } /* detect cycle slip by L1-L5 geometry free phase jump -----------------------*/ void detslp_gf_L1L5(rtk_t *rtk, const obsd_t *obs, int i, int j, const nav_t *nav) { int sat = obs[i].sat; double g0; double g1; trace(3, "detslp_gf_L1L5: i=%d j=%d\n", i, j); if (rtk->opt.nf <= 2 || (g1 = gfobs_L1L5(obs, i, j, nav->lam[sat - 1])) == 0.0) { return; } g0 = rtk->ssat[sat - 1].gf2; rtk->ssat[sat - 1].gf2 = g1; if (g0 != 0.0 && fabs(g1 - g0) > rtk->opt.thresslip) { rtk->ssat[sat - 1].slip[0] |= 1; rtk->ssat[sat - 1].slip[2] |= 1; errmsg(rtk, "slip detected (sat=%2d GF_L1_L5=%.3f %.3f)\n", sat, g0, g1); } } /* detect cycle slip by doppler and phase difference -------------------------*/ void detslp_dop(rtk_t *rtk __attribute__((unused)), const obsd_t *obs __attribute__((unused)), int i __attribute__((unused)), int rcv __attribute__((unused)), const nav_t *nav __attribute__((unused))) { /* detection with doppler disabled because of clock-jump issue (v.2.3.0) */ #if 0 int f, sat = obs[i].sat; double tt, dph, dpt, lam, thres; trace(3, "detslp_dop: i=%d rcv=%d\n", i, rcv); for (f = 0; f < rtk->opt.nf; f++) { if (obs[i].L[f] == 0.0 || obs[i].D[f] == 0.0 || rtk->ph[rcv - 1][sat - 1][f] == 0.0) { continue; } if (fabs(tt = timediff(obs[i].time, rtk->pt[rcv - 1][sat - 1][f])) < DTTOL) continue; if ((lam = nav->lam[sat - 1][f]) <= 0.0) continue; /* cycle slip threshold (cycle) */ thres = MAXACC * tt * tt / 2.0 / lam + rtk->opt.err[4] * fabs(tt) * 4.0; /* phase difference and doppler x time (cycle) */ dph = obs[i].L[f] - rtk->ph[rcv - 1][sat - 1][f]; dpt = -obs[i].D[f] * tt; if (fabs(dph - dpt) <= thres) continue; rtk->slip[sat - 1][f] | = 1; errmsg(rtk, "slip detected (sat=%2d rcv=%d L%d=%.3f %.3f thres=%.3f)\n", sat, rcv, f + 1, dph, dpt, thres); } #endif } /* temporal update of phase biases -------------------------------------------*/ void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat, const int *iu, const int *ir, int ns, const nav_t *nav) { double cp; double pr; double cp1; double cp2; double pr1; double pr2; double *bias; double offset; double lami; double lam1; double lam2; double C1; double C2; int i; int j; int f; int slip; int reset; int nf = NF_RTK(&rtk->opt); trace(3, "udbias : tt=%.1f ns=%d\n", tt, ns); for (i = 0; i < ns; i++) { /* detect cycle slip by LLI */ for (f = 0; f < rtk->opt.nf; f++) { rtk->ssat[sat[i] - 1].slip[f] &= 0xFC; } detslp_ll(rtk, obs, iu[i], 1); detslp_ll(rtk, obs, ir[i], 2); /* detect cycle slip by geometry-free phase jump */ detslp_gf_L1L2(rtk, obs, iu[i], ir[i], nav); detslp_gf_L1L5(rtk, obs, iu[i], ir[i], nav); /* detect cycle slip by doppler and phase difference */ detslp_dop(rtk, obs, iu[i], 1, nav); detslp_dop(rtk, obs, ir[i], 2, nav); /* update half-cycle valid flag */ for (f = 0; f < nf; f++) { rtk->ssat[sat[i] - 1].half[f] = !((obs[iu[i]].LLI[f] & 2) || (obs[ir[i]].LLI[f] & 2)); } } for (f = 0; f < nf; f++) { /* reset phase-bias if instantaneous AR or expire obs outage counter */ for (i = 1; i <= MAXSAT; i++) { reset = ++rtk->ssat[i - 1].outc[f] > static_cast(rtk->opt.maxout); if (rtk->opt.modear == ARMODE_INST && rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0) { initx_rtk(rtk, 0.0, 0.0, IB_RTK(i, f, &rtk->opt)); } else if (reset && rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0) { initx_rtk(rtk, 0.0, 0.0, IB_RTK(i, f, &rtk->opt)); trace(3, "udbias : obs outage counter overflow (sat=%3d L%d n=%d)\n", i, f + 1, rtk->ssat[i - 1].outc[f]); } if (rtk->opt.modear != ARMODE_INST && reset) { rtk->ssat[i - 1].lock[f] = -rtk->opt.minlock; } } /* reset phase-bias if detecting cycle slip */ for (i = 0; i < ns; i++) { j = IB_RTK(sat[i], f, &rtk->opt); rtk->P[j + j * rtk->nx] += rtk->opt.prn[0] * rtk->opt.prn[0] * tt; slip = rtk->ssat[sat[i] - 1].slip[f]; if (rtk->opt.ionoopt == IONOOPT_IFLC) { slip |= rtk->ssat[sat[i] - 1].slip[1]; } if (rtk->opt.modear == ARMODE_INST || !(slip & 1)) { continue; } rtk->x[j] = 0.0; rtk->ssat[sat[i] - 1].lock[f] = -rtk->opt.minlock; } bias = zeros(ns, 1); /* estimate approximate phase-bias by phase - code */ for (i = j = 0, offset = 0.0; i < ns; i++) { if (rtk->opt.ionoopt != IONOOPT_IFLC) { cp = sdobs(obs, iu[i], ir[i], f); /* cycle */ pr = sdobs(obs, iu[i], ir[i], f + NFREQ); lami = nav->lam[sat[i] - 1][f]; if (cp == 0.0 || pr == 0.0 || lami <= 0.0) { continue; } bias[i] = cp - pr / lami; } else { cp1 = sdobs(obs, iu[i], ir[i], 0); cp2 = sdobs(obs, iu[i], ir[i], 1); pr1 = sdobs(obs, iu[i], ir[i], NFREQ); pr2 = sdobs(obs, iu[i], ir[i], NFREQ + 1); lam1 = nav->lam[sat[i] - 1][0]; lam2 = nav->lam[sat[i] - 1][1]; if (cp1 == 0.0 || cp2 == 0.0 || pr1 == 0.0 || pr2 == 0.0 || lam1 <= 0.0 || lam2 <= 0.0) { continue; } C1 = std::pow(lam2, 2.0) / (std::pow(lam2, 2.0) - std::pow(lam1, 2.0)); C2 = -std::pow(lam1, 2.0) / (std::pow(lam2, 2.0) - std::pow(lam1, 2.0)); bias[i] = (C1 * lam1 * cp1 + C2 * lam2 * cp2) - (C1 * pr1 + C2 * pr2); } if (rtk->x[IB_RTK(sat[i], f, &rtk->opt)] != 0.0) { offset += bias[i] - rtk->x[IB_RTK(sat[i], f, &rtk->opt)]; j++; } } /* correct phase-bias offset to enssure phase-code coherency */ if (j > 0) { for (i = 1; i <= MAXSAT; i++) { if (rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0) { rtk->x[IB_RTK(i, f, &rtk->opt)] += offset / j; } } } /* set initial states of phase-bias */ for (i = 0; i < ns; i++) { if (bias[i] == 0.0 || rtk->x[IB_RTK(sat[i], f, &rtk->opt)] != 0.0) { continue; } initx_rtk(rtk, bias[i], std::pow(rtk->opt.std[0], 2.0), IB_RTK(sat[i], f, &rtk->opt)); } free(bias); } } /* temporal update of states --------------------------------------------------*/ void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat, const int *iu, const int *ir, int ns, const nav_t *nav) { double tt = fabs(rtk->tt); double bl = 0.0; double dr[3]; trace(3, "udstate : ns=%d\n", ns); /* temporal update of position/velocity/acceleration */ udpos(rtk, tt); /* temporal update of ionospheric parameters */ if (rtk->opt.ionoopt >= IONOOPT_EST) { bl = baseline(rtk->x, rtk->rb, dr); udion(rtk, tt, bl, sat, ns); } /* temporal update of tropospheric parameters */ if (rtk->opt.tropopt >= TROPOPT_EST) { udtrop(rtk, tt, bl); } /* temporal update of eceiver h/w bias */ if (rtk->opt.glomodear == 2 && (rtk->opt.navsys & SYS_GLO)) { udrcvbias(rtk, tt); } /* temporal update of phase-bias */ if (rtk->opt.mode > PMODE_DGPS) { udbias(rtk, tt, obs, sat, iu, ir, ns, nav); } } /* undifferenced phase/code residual for satellite ---------------------------*/ void zdres_sat(int base, double r, const obsd_t *obs, const nav_t *nav, const double *azel, const double *dant, const prcopt_t *opt, double *y) { const double *lam = nav->lam[obs->sat - 1]; double f1; double f2; double C1; double C2; double dant_if; int i; int nf = NF_RTK(opt); if (opt->ionoopt == IONOOPT_IFLC) { /* iono-free linear combination */ if (lam[0] == 0.0 || lam[1] == 0.0) { return; } if (testsnr(base, 0, azel[1], obs->SNR[0] * 0.25, &opt->snrmask) || testsnr(base, 1, azel[1], obs->SNR[1] * 0.25, &opt->snrmask)) { return; } f1 = SPEED_OF_LIGHT_M_S / lam[0]; f2 = SPEED_OF_LIGHT_M_S / lam[1]; C1 = std::pow(f1, 2.0) / (std::pow(f1, 2.0) - std::pow(f2, 2.0)); C2 = -std::pow(f2, 2.0) / (std::pow(f1, 2.0) - std::pow(f2, 2.0)); dant_if = C1 * dant[0] + C2 * dant[1]; if (obs->L[0] != 0.0 && obs->L[1] != 0.0) { y[0] = C1 * obs->L[0] * lam[0] + C2 * obs->L[1] * lam[1] - r - dant_if; } if (obs->P[0] != 0.0 && obs->P[1] != 0.0) { y[1] = C1 * obs->P[0] + C2 * obs->P[1] - r - dant_if; } } else { for (i = 0; i < nf; i++) { if (lam[i] == 0.0) { continue; } /* check snr mask */ if (testsnr(base, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask)) { continue; } /* residuals = observable - pseudorange */ if (obs->L[i] != 0.0) { y[i] = obs->L[i] * lam[i] - r - dant[i]; } if (obs->P[i] != 0.0) { y[i + nf] = obs->P[i] - r - dant[i]; } } } } /* undifferenced phase/code residuals ----------------------------------------*/ int zdres(int base, const obsd_t *obs, int n, const double *rs, const double *dts, const int *svh, const nav_t *nav, const double *rr, const prcopt_t *opt, int index, double *y, double *e, double *azel) { double r; double rr_[3]; double pos[3]; double dant[NFREQ] = {0}; double disp[3]; double zhd; double zazel[] = {0.0, 90.0 * D2R}; int i; int nf = NF_RTK(opt); trace(3, "zdres : n=%d\n", n); for (i = 0; i < n * nf * 2; i++) { y[i] = 0.0; } if (norm_rtk(rr, 3) <= 0.0) { return 0; /* no receiver position */ } for (i = 0; i < 3; i++) { rr_[i] = rr[i]; } /* earth tide correction */ if (opt->tidecorr) { tidedisp(gpst2utc(obs[0].time), rr_, opt->tidecorr, &nav->erp, opt->odisp[base], disp); for (i = 0; i < 3; i++) { rr_[i] += disp[i]; } } ecef2pos(rr_, pos); for (i = 0; i < n; i++) { /* compute geometric-range and azimuth/elevation angle */ if ((r = geodist(rs + i * 6, rr_, e + i * 3)) <= 0.0) { continue; } if (satazel(pos, e + i * 3, azel + i * 2) < opt->elmin) { continue; } /* excluded satellite? */ if (satexclude(obs[i].sat, svh[i], opt)) { continue; } /* satellite clock-bias */ r += -SPEED_OF_LIGHT_M_S * dts[i * 2]; /* troposphere delay model (hydrostatic) */ zhd = tropmodel(obs[0].time, pos, zazel, 0.0); r += tropmapf(obs[i].time, pos, azel + i * 2, nullptr) * zhd; /* receiver antenna phase center correction */ antmodel(opt->pcvr + index, opt->antdel[index], azel + i * 2, opt->posopt[1], dant); /* undifferenced phase/code residual for satellite */ zdres_sat(base, r, obs + i, nav, azel + i * 2, dant, opt, y + i * nf * 2); } trace(4, "rr_=%.3f %.3f %.3f\n", rr_[0], rr_[1], rr_[2]); trace(4, "pos=%.9f %.9f %.3f\n", pos[0] * R2D, pos[1] * R2D, pos[2]); for (i = 0; i < n; i++) { trace(4, "sat=%2d %13.3f %13.3f %13.3f %13.10f %6.1f %5.1f\n", obs[i].sat, rs[i * 6], rs[1 + i * 6], rs[2 + i * 6], dts[i * 2], azel[i * 2] * R2D, azel[1 + i * 2] * R2D); } trace(4, "y=\n"); tracemat(4, y, nf * 2, n, 13, 3); return 1; } /* test valid observation data -----------------------------------------------*/ int validobs(int i, int j, int f, int nf, const double *y) { /* if no phase observable, psudorange is also unusable */ return y[f + i * nf * 2] != 0.0 && y[f + j * nf * 2] != 0.0 && (f < nf || (y[f - nf + i * nf * 2] != 0.0 && y[f - nf + j * nf * 2] != 0.0)); } /* double-differenced measurement error covariance ---------------------------*/ void ddcov(const int *nb, int n, const double *Ri, const double *Rj, int nv, double *R) { int i; int j; int k = 0; int b; trace(3, "ddcov : n=%d\n", n); for (i = 0; i < nv * nv; i++) { R[i] = 0.0; } for (b = 0; b < n; k += nb[b++]) { for (i = 0; i < nb[b]; i++) { for (j = 0; j < nb[b]; j++) { R[k + i + (k + j) * nv] = Ri[k + i] + (i == j ? Rj[k + i] : 0.0); } } } trace(5, "R=\n"); tracemat(5, R, nv, nv, 8, 6); } /* baseline length constraint ------------------------------------------------*/ int constbl(rtk_t *rtk, const double *x, const double *P, double *v, double *H, double *Ri, double *Rj, int index) { const double thres = 0.1; /* threshold for nonliearity (v.2.3.0) */ double xb[3]; double b[3]; double bb; double var = 0.0; int i; trace(3, "constbl : \n"); /* no constraint */ if (rtk->opt.baseline[0] <= 0.0) { return 0; } /* time-adjusted baseline vector and length */ for (i = 0; i < 3; i++) { xb[i] = rtk->rb[i] + rtk->rb[i + 3] * rtk->sol.age; b[i] = x[i] - xb[i]; } bb = norm_rtk(b, 3); /* approximate variance of solution */ if (P) { for (i = 0; i < 3; i++) { var += P[i + i * rtk->nx]; } var /= 3.0; } /* check nonlinearity */ if (var > thres * thres * bb * bb) { trace(3, "constbl : equation nonlinear (bb=%.3f var=%.3f)\n", bb, var); return 0; } /* constraint to baseline length */ v[index] = rtk->opt.baseline[0] - bb; if (H) { for (i = 0; i < 3; i++) { H[i + index * rtk->nx] = b[i] / bb; } } Ri[index] = 0.0; Rj[index] = std::pow(rtk->opt.baseline[1], 2.0); trace(4, "baseline len v=%13.3f R=%8.6f %8.6f\n", v[index], Ri[index], Rj[index]); return 1; } /* precise tropspheric model -------------------------------------------------*/ double prectrop(gtime_t time, const double *pos, int r, const double *azel, const prcopt_t *opt, const double *x, double *dtdx) { double m_w = 0.0; double cotz; double grad_n; double grad_e; int i = IT_RTK(r, opt); /* wet mapping function */ tropmapf(time, pos, azel, &m_w); if (opt->tropopt >= TROPOPT_ESTG && azel[1] > 0.0) { /* m_w=m_0+m_0*cot(el)*(Gn*cos(az)+Ge*sin(az)): ref [6] */ cotz = 1.0 / tan(azel[1]); grad_n = m_w * cotz * cos(azel[0]); grad_e = m_w * cotz * sin(azel[0]); m_w += grad_n * x[i + 1] + grad_e * x[i + 2]; dtdx[1] = grad_n * x[i]; dtdx[2] = grad_e * x[i]; } else { dtdx[1] = dtdx[2] = 0.0; } dtdx[0] = m_w; return m_w * x[i]; } /* glonass inter-channel bias correction -------------------------------------*/ double gloicbcorr(int sat1 __attribute((unused)), int sat2 __attribute((unused)), const prcopt_t *opt, double lam1, double lam2, int f) { double dfreq; if (f >= NFREQGLO || f >= opt->nf || !opt->exterr.ena[2]) { return 0.0; } dfreq = (SPEED_OF_LIGHT_M_S / lam1 - SPEED_OF_LIGHT_M_S / lam2) / (f == 0 ? DFRQ1_GLO : DFRQ2_GLO); return opt->exterr.gloicb[f] * 0.01 * dfreq; /* (m) */ } /* test navi system (m=0:gps/qzs/sbs,1:glo,2:gal,3:bds) ----------------------*/ int test_sys(int sys, int m) { switch (sys) { case SYS_GPS: return m == 0; case SYS_QZS: return m == 0; case SYS_SBS: return m == 0; case SYS_GLO: return m == 1; case SYS_GAL: return m == 2; case SYS_BDS: return m == 3; } return 0; } /* double-differenced phase/code residuals -----------------------------------*/ int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x, const double *P, const int *sat, double *y, const double *e, double *azel, const int *iu, const int *ir, int ns, double *v, double *H, double *R, int *vflg) { prcopt_t *opt = &rtk->opt; double bl; double dr[3]; double posu[3]; double posr[3]; double didxi = 0.0; double didxj = 0.0; double *im; double *tropr; double *tropu; double *dtdxr; double *dtdxu; double *Ri; double *Rj; double lami; double lamj; double fi; double fj; double df; double *Hi = nullptr; int i; int j; int k; int m; int f; int ff; int nv = 0; int nb[NFREQ * 4 * 2 + 2] = {0}; int b = 0; int sysi; int sysj; int nf = NF_RTK(opt); trace(3, "ddres : dt=%.1f nx=%d ns=%d\n", dt, rtk->nx, ns); bl = baseline(x, rtk->rb, dr); ecef2pos(x, posu); ecef2pos(rtk->rb, posr); Ri = mat(ns * nf * 2 + 2, 1); Rj = mat(ns * nf * 2 + 2, 1); im = mat(ns, 1); tropu = mat(ns, 1); tropr = mat(ns, 1); dtdxu = mat(ns, 3); dtdxr = mat(ns, 3); for (i = 0; i < MAXSAT; i++) { for (j = 0; j < NFREQ; j++) { rtk->ssat[i].resp[j] = rtk->ssat[i].resc[j] = 0.0; } } /* compute factors of ionospheric and tropospheric delay */ for (i = 0; i < ns; i++) { if (opt->ionoopt >= IONOOPT_EST) { im[i] = (ionmapf(posu, azel + iu[i] * 2) + ionmapf(posr, azel + ir[i] * 2)) / 2.0; } if (opt->tropopt >= TROPOPT_EST) { tropu[i] = prectrop(rtk->sol.time, posu, 0, azel + iu[i] * 2, opt, x, dtdxu + i * 3); tropr[i] = prectrop(rtk->sol.time, posr, 1, azel + ir[i] * 2, opt, x, dtdxr + i * 3); } } for (m = 0; m < 4; m++) { /* m=0:gps/qzs/sbs, 1:glo, 2:gal, 3:bds */ for (f = opt->mode > PMODE_DGPS ? 0 : nf; f < nf * 2; f++) { /* search reference satellite with highest elevation */ for (i = -1, j = 0; j < ns; j++) { sysi = rtk->ssat[sat[j] - 1].sys; if (!test_sys(sysi, m)) { continue; } if (!validobs(iu[j], ir[j], f, nf, y)) { continue; } if (i < 0 || azel[1 + iu[j] * 2] >= azel[1 + iu[i] * 2]) { i = j; } } if (i < 0) { continue; } /* make double difference */ for (j = 0; j < ns; j++) { if (i == j) { continue; } sysi = rtk->ssat[sat[i] - 1].sys; sysj = rtk->ssat[sat[j] - 1].sys; if (!test_sys(sysj, m)) { continue; } if (!validobs(iu[j], ir[j], f, nf, y)) { continue; } ff = f % nf; lami = nav->lam[sat[i] - 1][ff]; lamj = nav->lam[sat[j] - 1][ff]; if (lami <= 0.0 || lamj <= 0.0) { continue; } if (H) { Hi = H + nv * rtk->nx; for (k = 0; k < rtk->nx; k++) { Hi[k] = 0.0; } } /* double-differenced residual */ v[nv] = (y[f + iu[i] * nf * 2] - y[f + ir[i] * nf * 2]) - (y[f + iu[j] * nf * 2] - y[f + ir[j] * nf * 2]); /* partial derivatives by rover position */ if (H) { for (k = 0; k < 3; k++) { Hi[k] = -e[k + iu[i] * 3] + e[k + iu[j] * 3]; } } /* double-differenced ionospheric delay term */ if (opt->ionoopt == IONOOPT_EST) { fi = lami / LAM_CARR[0]; fj = lamj / LAM_CARR[0]; didxi = (f < nf ? -1.0 : 1.0) * fi * fi * im[i]; didxj = (f < nf ? -1.0 : 1.0) * fj * fj * im[j]; v[nv] -= didxi * x[II_RTK(sat[i], opt)] - didxj * x[II_RTK(sat[j], opt)]; if (H) { Hi[II_RTK(sat[i], opt)] = didxi; Hi[II_RTK(sat[j], opt)] = -didxj; } } /* double-differenced tropospheric delay term */ if (opt->tropopt == TROPOPT_EST || opt->tropopt == TROPOPT_ESTG) { v[nv] -= (tropu[i] - tropu[j]) - (tropr[i] - tropr[j]); for (k = 0; k < (opt->tropopt < TROPOPT_ESTG ? 1 : 3); k++) { if (!H) { continue; } Hi[IT_RTK(0, opt) + k] = (dtdxu[k + i * 3] - dtdxu[k + j * 3]); Hi[IT_RTK(1, opt) + k] = -(dtdxr[k + i * 3] - dtdxr[k + j * 3]); } } /* double-differenced phase-bias term */ if (f < nf) { if (opt->ionoopt != IONOOPT_IFLC) { v[nv] -= lami * x[IB_RTK(sat[i], f, opt)] - lamj * x[IB_RTK(sat[j], f, opt)]; if (H) { Hi[IB_RTK(sat[i], f, opt)] = lami; Hi[IB_RTK(sat[j], f, opt)] = -lamj; } } else { v[nv] -= x[IB_RTK(sat[i], f, opt)] - x[IB_RTK(sat[j], f, opt)]; if (H) { Hi[IB_RTK(sat[i], f, opt)] = 1.0; Hi[IB_RTK(sat[j], f, opt)] = -1.0; } } } /* glonass receiver h/w bias term */ if (rtk->opt.glomodear == 2 && sysi == SYS_GLO && sysj == SYS_GLO && ff < NFREQGLO) { df = (SPEED_OF_LIGHT_M_S / lami - SPEED_OF_LIGHT_M_S / lamj) / 1E6; /* freq-difference (MHz) */ v[nv] -= df * x[IL_RTK(ff, opt)]; if (H) { Hi[IL_RTK(ff, opt)] = df; } } /* glonass interchannel bias correction */ else if (sysi == SYS_GLO && sysj == SYS_GLO) { v[nv] -= gloicbcorr(sat[i], sat[j], &rtk->opt, lami, lamj, f); } if (f < nf) { rtk->ssat[sat[j] - 1].resc[f] = v[nv]; } else { rtk->ssat[sat[j] - 1].resp[f - nf] = v[nv]; } /* test innovation */ if (opt->maxinno > 0.0 && fabs(v[nv]) > opt->maxinno) { if (f < nf) { rtk->ssat[sat[i] - 1].rejc[f]++; rtk->ssat[sat[j] - 1].rejc[f]++; } errmsg(rtk, "outlier rejected (sat=%3d-%3d %s%d v=%.3f)\n", sat[i], sat[j], f < nf ? "L" : "P", f % nf + 1, v[nv]); continue; } /* single-differenced measurement error variances */ Ri[nv] = varerr(sat[i], sysi, azel[1 + iu[i] * 2], bl, dt, f, opt); Rj[nv] = varerr(sat[j], sysj, azel[1 + iu[j] * 2], bl, dt, f, opt); /* set valid data flags */ if (opt->mode > PMODE_DGPS) { if (f < nf) { rtk->ssat[sat[i] - 1].vsat[f] = rtk->ssat[sat[j] - 1].vsat[f] = 1; } } else { rtk->ssat[sat[i] - 1].vsat[f - nf] = rtk->ssat[sat[j] - 1].vsat[f - nf] = 1; } trace(4, "sat=%3d-%3d %s%d v=%13.3f R=%8.6f %8.6f\n", sat[i], sat[j], f < nf ? "L" : "P", f % nf + 1, v[nv], Ri[nv], Rj[nv]); vflg[nv++] = (sat[i] << 16) | (sat[j] << 8) | ((f < nf ? 0 : 1) << 4) | (f % nf); nb[b]++; } #if 0 /* residuals referenced to reference satellite (2.4.2 p11) */ /* restore single-differenced residuals assuming sum equal zero */ if (f < nf) { for (j = 0, s = 0.0; j < MAXSAT; j++) s += rtk->ssat[j].resc[f]; s /= nb[b] + 1; for (j = 0; j < MAXSAT; j++) { if (j == sat[i] - 1 || rtk->ssat[j].resc[f] != 0.0) rtk->ssat[j].resc[f] -= s; } } else { for (j = 0, s = 0.0; j < MAXSAT; j++) s += rtk->ssat[j].resp[f - nf]; s /= nb[b] + 1; for (j = 0; j < MAXSAT; j++) { if (j == sat[i] - 1 || rtk->ssat[j].resp[f - nf] != 0.0) rtk->ssat[j].resp[f - nf] -= s; } } #endif b++; } } /* end of system loop */ /* baseline length constraint for moving baseline */ if (opt->mode == PMODE_MOVEB && constbl(rtk, x, P, v, H, Ri, Rj, nv)) { vflg[nv++] = 3 << 4; nb[b++]++; } if (H) { trace(5, "H=\n"); tracemat(5, H, rtk->nx, nv, 7, 4); } /* double-differenced measurement error covariance */ ddcov(nb, b, Ri, Rj, nv, R); free(Ri); free(Rj); free(im); free(tropu); free(tropr); free(dtdxu); free(dtdxr); return nv; } /* time-interpolation of residuals (for post-mission) ------------------------*/ double intpres(gtime_t time, const obsd_t *obs, int n, const nav_t *nav, rtk_t *rtk, double *y) { static obsd_t obsb[MAXOBS]; static double yb[MAXOBS * NFREQ * 2]; static double rs[MAXOBS * 6]; static double dts[MAXOBS * 2]; static double var[MAXOBS]; static double e[MAXOBS * 3]; static double azel[MAXOBS * 2]; static int nb = 0; static int svh[MAXOBS * 2]; prcopt_t *opt = &rtk->opt; double tt = timediff(time, obs[0].time); double ttb; double *p; double *q; int i; int j; int k; int nf = NF_RTK(opt); trace(3, "intpres : n=%d tt=%.1f\n", n, tt); if (nb == 0 || fabs(tt) < DTTOL) { nb = n; for (i = 0; i < n; i++) { obsb[i] = obs[i]; } return tt; } ttb = timediff(time, obsb[0].time); if (fabs(ttb) > opt->maxtdiff * 2.0 || ttb == tt) { return tt; } satposs(time, obsb, nb, nav, opt->sateph, rs, dts, var, svh); if (!zdres(1, obsb, nb, rs, dts, svh, nav, rtk->rb, opt, 1, yb, e, azel)) { return tt; } for (i = 0; i < n; i++) { for (j = 0; j < nb; j++) { if (obsb[j].sat == obs[i].sat) { break; } } if (j >= nb) { continue; } for (k = 0, p = y + i * nf * 2, q = yb + j * nf * 2; k < nf * 2; k++, p++, q++) { if (*p == 0.0 || *q == 0.0) { *p = 0.0; } else { *p = (ttb * (*p) - tt * (*q)) / (ttb - tt); } } } return fabs(ttb) > fabs(tt) ? ttb : tt; } /* single to double-difference transformation matrix (D') --------------------*/ int ddmat(rtk_t *rtk, double *D) { int i; int j; int k; int m; int f; int nb = 0; int nx = rtk->nx; int na = rtk->na; int nf = NF_RTK(&rtk->opt); int nofix; trace(3, "ddmat :\n"); for (i = 0; i < MAXSAT; i++) { for (j = 0; j < NFREQ; j++) { rtk->ssat[i].fix[j] = 0; } } for (i = 0; i < na; i++) { D[i + i * nx] = 1.0; } for (m = 0; m < 4; m++) { /* m=0:gps/qzs/sbs, 1:glo, 2:gal, 3:bds */ nofix = (m == 1 && rtk->opt.glomodear == 0) || (m == 3 && rtk->opt.bdsmodear == 0); for (f = 0, k = na; f < nf; f++, k += MAXSAT) { if (i < k + MAXSAT) { for (i = k; i < k + MAXSAT; i++) { if (rtk->x[i] == 0.0 || !test_sys(rtk->ssat[i - k].sys, m) || !rtk->ssat[i - k].vsat[f] || !rtk->ssat[i - k].half[f]) { continue; } if (rtk->ssat[i - k].lock[f] > 0 && !(rtk->ssat[i - k].slip[f] & 2) && rtk->ssat[i - k].azel[1] >= rtk->opt.elmaskar && !nofix) { rtk->ssat[i - k].fix[f] = 2; /* fix */ break; } rtk->ssat[i - k].fix[f] = 1; } for (j = k; j < k + MAXSAT; j++) { if (i == j || rtk->x[j] == 0.0 || !test_sys(rtk->ssat[j - k].sys, m) || !rtk->ssat[j - k].vsat[f]) { continue; } if (rtk->ssat[j - k].lock[f] > 0 && !(rtk->ssat[j - k].slip[f] & 2) && rtk->ssat[i - k].vsat[f] && rtk->ssat[j - k].azel[1] >= rtk->opt.elmaskar && !nofix) { D[i + (na + nb) * nx] = 1.0; D[j + (na + nb) * nx] = -1.0; nb++; rtk->ssat[j - k].fix[f] = 2; /* fix */ } else { rtk->ssat[j - k].fix[f] = 1; } } } } } trace(5, "D=\n"); tracemat(5, D, nx, na + nb, 2, 0); return nb; } /* restore single-differenced ambiguity --------------------------------------*/ void restamb(rtk_t *rtk, const double *bias, int nb __attribute((unused)), double *xa) { int i; int n; int m; int f; std::vector index(MAXSAT); int nv = 0; int nf = NF_RTK(&rtk->opt); trace(3, "restamb :\n"); for (i = 0; i < rtk->nx; i++) { xa[i] = rtk->x[i]; } for (i = 0; i < rtk->na; i++) { xa[i] = rtk->xa[i]; } for (m = 0; m < 4; m++) { for (f = 0; f < nf; f++) { for (n = i = 0; i < MAXSAT; i++) { if (!test_sys(rtk->ssat[i].sys, m) || rtk->ssat[i].fix[f] != 2) { continue; } index[n++] = IB_RTK(i + 1, f, &rtk->opt); } if (n < 2) { continue; } xa[index[0]] = rtk->x[index[0]]; for (i = 1; i < n; i++) { xa[index[i]] = xa[index[0]] - bias[nv++]; } } } } /* hold integer ambiguity ----------------------------------------------------*/ void holdamb(rtk_t *rtk, const double *xa) { double *v; double *H; double *R; int i; int n; int m; int f; int info; std::vector index(MAXSAT); int nb = rtk->nx - rtk->na; int nv = 0; int nf = NF_RTK(&rtk->opt); trace(3, "holdamb :\n"); v = mat(nb, 1); H = zeros(nb, rtk->nx); for (m = 0; m < 4; m++) { for (f = 0; f < nf; f++) { for (n = i = 0; i < MAXSAT; i++) { if (!test_sys(rtk->ssat[i].sys, m) || rtk->ssat[i].fix[f] != 2 || rtk->ssat[i].azel[1] < rtk->opt.elmaskhold) { continue; } index[n++] = IB_RTK(i + 1, f, &rtk->opt); rtk->ssat[i].fix[f] = 3; /* hold */ } /* constraint to fixed ambiguity */ for (i = 1; i < n; i++) { v[nv] = (xa[index[0]] - xa[index[i]]) - (rtk->x[index[0]] - rtk->x[index[i]]); H[index[0] + nv * rtk->nx] = 1.0; H[index[i] + nv * rtk->nx] = -1.0; nv++; } } } if (nv > 0) { R = zeros(nv, nv); for (i = 0; i < nv; i++) { R[i + i * nv] = VAR_HOLDAMB; } /* update states with constraints */ if ((info = filter(rtk->x, rtk->P, H, v, R, rtk->nx, nv))) { errmsg(rtk, "filter error (info=%d)\n", info); } free(R); } free(v); free(H); } /* resolve integer ambiguity by LAMBDA ---------------------------------------*/ int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa) { prcopt_t *opt = &rtk->opt; int i; int j; int ny; int nb; int info; int nx = rtk->nx; int na = rtk->na; double *D; double *DP; double *y; double *Qy; double *b; double *db; double *Qb; double *Qab; double *QQ; double s[2]; trace(3, "resamb_LAMBDA : nx=%d\n", nx); rtk->sol.ratio = 0.0; if (rtk->opt.mode <= PMODE_DGPS || rtk->opt.modear == ARMODE_OFF || rtk->opt.thresar[0] < 1.0) { return 0; } /* single to double-difference transformation matrix (D') */ D = zeros(nx, nx); if ((nb = ddmat(rtk, D)) <= 0) { errmsg(rtk, "no valid double-difference\n"); free(D); return 0; } ny = na + nb; y = mat(ny, 1); Qy = mat(ny, ny); DP = mat(ny, nx); b = mat(nb, 2); db = mat(nb, 1); Qb = mat(nb, nb); Qab = mat(na, nb); QQ = mat(na, nb); /* transform single to double-differenced phase-bias (y=D'*x, Qy=D'*P*D) */ matmul("TN", ny, 1, nx, 1.0, D, rtk->x, 0.0, y); matmul("TN", ny, nx, nx, 1.0, D, rtk->P, 0.0, DP); matmul("NN", ny, ny, nx, 1.0, DP, D, 0.0, Qy); /* phase-bias covariance (Qb) and real-parameters to bias covariance (Qab) */ for (i = 0; i < nb; i++) { for (j = 0; j < nb; j++) { Qb[i + j * nb] = Qy[na + i + (na + j) * ny]; } } for (i = 0; i < na; i++) { for (j = 0; j < nb; j++) { Qab[i + j * na] = Qy[i + (na + j) * ny]; } } trace(4, "N(0)="); tracemat(4, y + na, 1, nb, 10, 3); /* lambda/mlambda integer least-square estimation */ if (!(info = lambda(nb, 2, y + na, Qb, b, s))) { trace(4, "N(1)="); tracemat(4, b, 1, nb, 10, 3); trace(4, "N(2)="); tracemat(4, b + nb, 1, nb, 10, 3); rtk->sol.ratio = s[0] > 0 ? static_cast(s[1] / s[0]) : 0.0F; if (rtk->sol.ratio > 999.9) { rtk->sol.ratio = 999.9F; } /* validation by popular ratio-test */ if (s[0] <= 0.0 || s[1] / s[0] >= opt->thresar[0]) { /* transform float to fixed solution (xa=xa-Qab*Qb\(b0-b)) */ for (i = 0; i < na; i++) { rtk->xa[i] = rtk->x[i]; for (j = 0; j < na; j++) { rtk->Pa[i + j * na] = rtk->P[i + j * nx]; } } for (i = 0; i < nb; i++) { bias[i] = b[i]; y[na + i] -= b[i]; } if (!matinv(Qb, nb)) { matmul("NN", nb, 1, nb, 1.0, Qb, y + na, 0.0, db); matmul("NN", na, 1, nb, -1.0, Qab, db, 1.0, rtk->xa); /* covariance of fixed solution (Qa=Qa-Qab*Qb^-1*Qab') */ matmul("NN", na, nb, nb, 1.0, Qab, Qb, 0.0, QQ); matmul("NT", na, na, nb, -1.0, QQ, Qab, 1.0, rtk->Pa); trace(3, "resamb : validation ok (nb=%d ratio=%.2f s=%.2f/%.2f)\n", nb, s[0] == 0.0 ? 0.0 : s[1] / s[0], s[0], s[1]); /* restore single-differenced ambiguity */ restamb(rtk, bias, nb, xa); } else { nb = 0; } } else { /* validation failed */ errmsg(rtk, "ambiguity validation failed (nb=%d ratio=%.2f s=%.2f/%.2f)\n", nb, s[1] / s[0], s[0], s[1]); nb = 0; } } else { errmsg(rtk, "lambda error (info=%d)\n", info); } free(D); free(y); free(Qy); free(DP); free(b); free(db); free(Qb); free(Qab); free(QQ); return nb; /* number of ambiguities */ } /* validation of solution ----------------------------------------------------*/ int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg, int nv, double thres) { #if 0 prcopt_t *opt = &rtk->opt; double vv = 0.0; #endif double fact = thres * thres; int i; int stat = 1; int sat1; int sat2; int type; int freq; char stype; trace(3, "valpos : nv=%d thres=%.1f\n", nv, thres); /* post-fit residual test */ for (i = 0; i < nv; i++) { if (v[i] * v[i] <= fact * R[i + i * nv]) { continue; } sat1 = (vflg[i] >> 16) & 0xFF; sat2 = (vflg[i] >> 8) & 0xFF; type = (vflg[i] >> 4) & 0xF; freq = vflg[i] & 0xF; stype = type == 0 ? 'L' : (type == 1 ? 'L' : 'C'); errmsg(rtk, "large residual (sat=%2d-%2d %c%d v=%6.3f sig=%.3f)\n", sat1, sat2, stype, freq + 1, v[i], std::sqrt(R[i + i * nv])); } #if 0 /* omitted v.2.4.0 */ if (stat && nv > NP(opt)) { /* chi-square validation */ for (i = 0; i < nv; i++) vv += v[i] * v[i] / R[i + i * nv]; if (vv > chisqr[nv - NP(opt) - 1]) { errmsg(rtk, "residuals validation failed (nv=%d np=%d vv=%.2f cs=%.2f)\n", nv, NP(opt), vv, chisqr[nv - NP(opt) - 1]); stat = 0; } else { trace(3, "valpos : validation ok (%s nv=%d np=%d vv=%.2f cs=%.2f)\n", rtk->tstr, nv, NP(opt), vv, chisqr[nv - NP(opt) - 1]); } } #endif return stat; } /* relative positioning ------------------------------------------------------*/ int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, const nav_t *nav) { prcopt_t *opt = &rtk->opt; gtime_t time = obs[0].time; double *rs; double *dts; double *var; double *y; double *e; double *azel; double *v; double *H; double *R; double *xp; double *Pp; double *xa; double *bias; double dt; int i; int j; int f; int n = nu + nr; int ns; int ny; int nv; std::vector sat(MAXSAT); std::vector iu(MAXSAT); std::vector ir(MAXSAT); int niter; int info; std::vector vflg(MAXOBS * NFREQ * 2 + 1); std::vector svh(MAXOBS * 2); int stat = rtk->opt.mode <= PMODE_DGPS ? SOLQ_DGPS : SOLQ_FLOAT; int nf = opt->ionoopt == IONOOPT_IFLC ? 1 : opt->nf; trace(3, "relpos : nx=%d nu=%d nr=%d\n", rtk->nx, nu, nr); dt = timediff(time, obs[nu].time); rs = mat(6, n); dts = mat(2, n); var = mat(1, n); y = mat(nf * 2, n); e = mat(3, n); azel = zeros(2, n); for (i = 0; i < MAXSAT; i++) { rtk->ssat[i].sys = satsys(i + 1, nullptr); for (j = 0; j < NFREQ; j++) { rtk->ssat[i].vsat[j] = rtk->ssat[i].snr[j] = 0; } } /* satellite positions/clocks */ satposs(time, obs, n, nav, opt->sateph, rs, dts, var, svh.data()); /* undifferenced residuals for base station */ if (!zdres(1, obs + nu, nr, rs + nu * 6, dts + nu * 2, &svh[nu], nav, rtk->rb, opt, 1, y + nu * nf * 2, e + nu * 3, azel + nu * 2)) { errmsg(rtk, "initial base station position error\n"); free(rs); free(dts); free(var); free(y); free(e); free(azel); return 0; } /* time-interpolation of residuals (for post-processing) */ if (opt->intpref) { dt = intpres(time, obs + nu, nr, nav, rtk, y + nu * nf * 2); } /* select common satellites between rover and base-station */ if ((ns = selsat(obs, azel, nu, nr, opt, sat.data(), iu.data(), ir.data())) <= 0) { errmsg(rtk, "no common satellite\n"); free(rs); free(dts); free(var); free(y); free(e); free(azel); return 0; } /* temporal update of states */ udstate(rtk, obs, sat.data(), iu.data(), ir.data(), ns, nav); trace(4, "x(0)="); tracemat(4, rtk->x, 1, NR_RTK(opt), 13, 4); xp = mat(rtk->nx, 1); Pp = zeros(rtk->nx, rtk->nx); xa = mat(rtk->nx, 1); matcpy(xp, rtk->x, rtk->nx, 1); ny = ns * nf * 2 + 2; v = mat(ny, 1); H = zeros(rtk->nx, ny); R = mat(ny, ny); bias = mat(rtk->nx, 1); /* add 2 iterations for baseline-constraint moving-base */ niter = opt->niter + (opt->mode == PMODE_MOVEB && opt->baseline[0] > 0.0 ? 2 : 0); for (i = 0; i < niter; i++) { /* undifferenced residuals for rover */ if (!zdres(0, obs, nu, rs, dts, svh.data(), nav, xp, opt, 0, y, e, azel)) { errmsg(rtk, "rover initial position error\n"); stat = SOLQ_NONE; break; } /* double-differenced residuals and partial derivatives */ if ((nv = ddres(rtk, nav, dt, xp, Pp, sat.data(), y, e, azel, iu.data(), ir.data(), ns, v, H, R, vflg.data())) < 1) { errmsg(rtk, "no double-differenced residual\n"); stat = SOLQ_NONE; break; } /* kalman filter measurement update */ matcpy(Pp, rtk->P, rtk->nx, rtk->nx); if ((info = filter(xp, Pp, H, v, R, rtk->nx, nv))) { errmsg(rtk, "filter error (info=%d)\n", info); stat = SOLQ_NONE; break; } trace(4, "x(%d)=", i + 1); tracemat(4, xp, 1, NR_RTK(opt), 13, 4); } if (stat != SOLQ_NONE && zdres(0, obs, nu, rs, dts, svh.data(), nav, xp, opt, 0, y, e, azel)) { /* post-fit residuals for float solution */ nv = ddres(rtk, nav, dt, xp, Pp, sat.data(), y, e, azel, iu.data(), ir.data(), ns, v, nullptr, R, vflg.data()); /* validation of float solution */ if (valpos(rtk, v, R, vflg.data(), nv, 4.0)) { /* update state and covariance matrix */ matcpy(rtk->x, xp, rtk->nx, 1); matcpy(rtk->P, Pp, rtk->nx, rtk->nx); /* update ambiguity control struct */ rtk->sol.ns = 0; for (i = 0; i < ns; i++) { for (f = 0; f < nf; f++) { if (!rtk->ssat[sat[i] - 1].vsat[f]) { continue; } rtk->ssat[sat[i] - 1].lock[f]++; rtk->ssat[sat[i] - 1].outc[f] = 0; if (f == 0) { rtk->sol.ns++; /* valid satellite count by L1 */ } } } /* lack of valid satellites */ if (rtk->sol.ns < 4) { stat = SOLQ_NONE; } } else { stat = SOLQ_NONE; } } /* resolve integer ambiguity by WL-NL */ if (stat != SOLQ_NONE && rtk->opt.modear == ARMODE_WLNL) { if (resamb_WLNL(rtk, obs, sat.data(), iu.data(), ir.data(), ns, nav, azel)) { stat = SOLQ_FIX; } } /* resolve integer ambiguity by TCAR */ else if (stat != SOLQ_NONE && rtk->opt.modear == ARMODE_TCAR) { if (resamb_TCAR(rtk, obs, sat.data(), iu.data(), ir.data(), ns, nav, azel)) { stat = SOLQ_FIX; } } /* resolve integer ambiguity by LAMBDA */ else if (stat != SOLQ_NONE && resamb_LAMBDA(rtk, bias, xa) > 1) { if (zdres(0, obs, nu, rs, dts, svh.data(), nav, xa, opt, 0, y, e, azel)) { /* post-fit reisiduals for fixed solution */ nv = ddres(rtk, nav, dt, xa, nullptr, sat.data(), y, e, azel, iu.data(), ir.data(), ns, v, nullptr, R, vflg.data()); /* validation of fixed solution */ if (valpos(rtk, v, R, vflg.data(), nv, 4.0)) { /* hold integer ambiguity */ if (++rtk->nfix >= rtk->opt.minfix && rtk->opt.modear == ARMODE_FIXHOLD) { holdamb(rtk, xa); } stat = SOLQ_FIX; } } } /* save solution status */ if (stat == SOLQ_FIX) { for (i = 0; i < 3; i++) { rtk->sol.rr[i] = rtk->xa[i]; rtk->sol.qr[i] = static_cast(rtk->Pa[i + i * rtk->na]); } rtk->sol.qr[3] = static_cast(rtk->Pa[1]); rtk->sol.qr[4] = static_cast(rtk->Pa[1 + 2 * rtk->na]); rtk->sol.qr[5] = static_cast(rtk->Pa[2]); } else { for (i = 0; i < 3; i++) { rtk->sol.rr[i] = rtk->x[i]; rtk->sol.qr[i] = static_cast(rtk->P[i + i * rtk->nx]); } rtk->sol.qr[3] = static_cast(rtk->P[1]); rtk->sol.qr[4] = static_cast(rtk->P[1 + 2 * rtk->nx]); rtk->sol.qr[5] = static_cast(rtk->P[2]); rtk->nfix = 0; } for (i = 0; i < n; i++) { for (j = 0; j < nf; j++) { if (obs[i].L[j] == 0.0) { continue; } rtk->ssat[obs[i].sat - 1].pt[obs[i].rcv - 1][j] = obs[i].time; rtk->ssat[obs[i].sat - 1].ph[obs[i].rcv - 1][j] = obs[i].L[j]; } } for (i = 0; i < ns; i++) { for (j = 0; j < nf; j++) { /* output snr of rover receiver */ rtk->ssat[sat[i] - 1].snr[j] = obs[iu[i]].SNR[j]; } } for (i = 0; i < MAXSAT; i++) { for (j = 0; j < nf; j++) { if (rtk->ssat[i].fix[j] == 2 && stat != SOLQ_FIX) { rtk->ssat[i].fix[j] = 1; } if (rtk->ssat[i].slip[j] & 1) { rtk->ssat[i].slipc[j]++; } } } free(rs); free(dts); free(var); free(y); free(e); free(azel); free(xp); free(Pp); free(xa); free(v); free(H); free(R); free(bias); if (stat != SOLQ_NONE) { rtk->sol.stat = stat; } return stat != SOLQ_NONE; } /* initialize rtk control ------------------------------------------------------ * initialize rtk control struct * args : rtk_t *rtk IO rtk control/result struct * prcopt_t *opt I positioning options (see rtklib.h) * return : none *-----------------------------------------------------------------------------*/ void rtkinit(rtk_t *rtk, const prcopt_t *opt) { sol_t sol0 = {{0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0}; ambc_t ambc0 = {{{0, 0}, {0, 0}, {0, 0}, {0, 0}}, {}, {}, {}, 0, {}}; ssat_t ssat0 = {0, 0, {0.0}, {0.0}, {0.0}, {'0'}, {'0'}, {'0'}, {'0'}, {'0'}, {}, {}, {}, {}, 0.0, 0.0, 0.0, 0.0, {{{0, 0}}, {{0, 0}}}, {{}, {}}}; int i; trace(3, "rtkinit :\n"); rtk->sol = sol0; for (i = 0; i < 6; i++) { rtk->rb[i] = 0.0; } rtk->nx = opt->mode <= PMODE_FIXED ? NX_RTK(opt) : pppnx(opt); rtk->na = opt->mode <= PMODE_FIXED ? NR_RTK(opt) : pppnx(opt); rtk->tt = 0.0; rtk->x = zeros(rtk->nx, 1); rtk->P = zeros(rtk->nx, rtk->nx); rtk->xa = zeros(rtk->na, 1); rtk->Pa = zeros(rtk->na, rtk->na); rtk->nfix = rtk->neb = 0; for (i = 0; i < MAXSAT; i++) { rtk->ambc[i] = ambc0; rtk->ssat[i] = ssat0; } for (i = 0; i < MAXERRMSG; i++) { rtk->errbuf[i] = 0; } rtk->opt = *opt; } /* free rtk control ------------------------------------------------------------ * free memory for rtk control struct * args : rtk_t *rtk IO rtk control/result struct * return : none *-----------------------------------------------------------------------------*/ void rtkfree(rtk_t *rtk) { trace(3, "rtkfree :\n"); rtk->nx = rtk->na = 0; free(rtk->x); rtk->x = nullptr; free(rtk->P); rtk->P = nullptr; free(rtk->xa); rtk->xa = nullptr; free(rtk->Pa); rtk->Pa = nullptr; } /* precise positioning --------------------------------------------------------- * input observation data and navigation message, compute rover position by * precise positioning * args : rtk_t *rtk IO rtk control/result struct * rtk->sol IO solution * .time O solution time * .rr[] IO rover position/velocity * (I:fixed mode,O:single mode) * .dtr[0] O receiver clock bias (s) * .dtr[1] O receiver glonass-gps time offset (s) * .Qr[] O rover position covarinace * .stat O solution status (SOLQ_???) * .ns O number of valid satellites * .age O age of differential (s) * .ratio O ratio factor for ambiguity validation * rtk->rb[] IO base station position/velocity * (I:relative mode,O:moving-base mode) * rtk->nx I number of all states * rtk->na I number of integer states * rtk->ns O number of valid satellite * rtk->tt O time difference between current and previous (s) * rtk->x[] IO float states pre-filter and post-filter * rtk->P[] IO float covariance pre-filter and post-filter * rtk->xa[] O fixed states after AR * rtk->Pa[] O fixed covariance after AR * rtk->ssat[s] IO sat(s+1) status * .sys O system (SYS_???) * .az [r] O azimuth angle (rad) (r=0:rover,1:base) * .el [r] O elevation angle (rad) (r=0:rover,1:base) * .vs [r] O data valid single (r=0:rover,1:base) * .resp [f] O freq(f+1) pseudorange residual (m) * .resc [f] O freq(f+1) carrier-phase residual (m) * .vsat [f] O freq(f+1) data valid (0:invalid,1:valid) * .fix [f] O freq(f+1) ambiguity flag * (0:nodata,1:float,2:fix,3:hold) * .slip [f] O freq(f+1) slip flag * (bit8-7:rcv1 LLI, bit6-5:rcv2 LLI, * bit2:parity unknown, bit1:slip) * .lock [f] IO freq(f+1) carrier lock count * .outc [f] IO freq(f+1) carrier outage count * .slipc[f] IO freq(f+1) cycle slip count * .rejc [f] IO freq(f+1) data reject count * .gf IO geometry-free phase (L1-L2) (m) * .gf2 IO geometry-free phase (L1-L5) (m) * rtk->nfix IO number of continuous fixes of ambiguity * rtk->neb IO bytes of error message buffer * rtk->errbuf IO error message buffer * rtk->tstr O time string for debug * rtk->opt I processing options * obsd_t *obs I observation data for an epoch * obs[i].rcv=1:rover,2:reference * sorted by receiver and satellte * int n I number of observation data * nav_t *nav I navigation messages * return : status (0:no solution,1:valid solution) * notes : before calling function, base station position rtk->sol.rb[] should * be properly set for relative mode except for moving-baseline *-----------------------------------------------------------------------------*/ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) { prcopt_t *opt = &rtk->opt; sol_t solb = {{0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0}; gtime_t time; int i; int nu; int nr; char msg[128] = ""; trace(3, "rtkpos : time=%s n=%d\n", time_str(obs[0].time, 3), n); trace(4, "obs=\n"); traceobs(4, obs, n); /*trace(5,"nav=\n"); tracenav(5,nav);*/ /* set base station position */ if (opt->refpos <= POSOPT_RINEX && opt->mode != PMODE_SINGLE && opt->mode != PMODE_MOVEB) { for (i = 0; i < 6; i++) { rtk->rb[i] = i < 3 ? opt->rb[i] : 0.0; } } /* count rover/base station observations */ for (nu = 0; nu < n && obs[nu].rcv == 1; nu++) { } for (nr = 0; nu + nr < n && obs[nu + nr].rcv == 2; nr++) { } time = rtk->sol.time; /* previous epoch */ /* rover position by single point positioning */ if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, nullptr, rtk->ssat, msg)) { errmsg(rtk, "point pos error (%s)\n", msg); if (!rtk->opt.dynamics) { outsolstat(rtk); return 0; } } if (time.time != 0) { rtk->tt = timediff(rtk->sol.time, time); } /* single point positioning */ if (opt->mode == PMODE_SINGLE) { outsolstat(rtk); return 1; } /* suppress output of single solution */ if (!opt->outsingle) { rtk->sol.stat = SOLQ_NONE; } /* precise point positioning */ if (opt->mode >= PMODE_PPP_KINEMA) { pppos(rtk, obs, nu, nav); outsolstat(rtk); return 1; } /* check number of data of base station and age of differential */ if (nr == 0) { errmsg(rtk, "no base station observation data for rtk\n"); outsolstat(rtk); return 1; } if (opt->mode == PMODE_MOVEB) { /* moving baseline */ /* estimate position/velocity of base station */ if (!pntpos(obs + nu, nr, nav, &rtk->opt, &solb, nullptr, nullptr, msg)) { errmsg(rtk, "base station position error (%s)\n", msg); return 0; } rtk->sol.age = static_cast(timediff(rtk->sol.time, solb.time)); if (std::fabs(rtk->sol.age) > TTOL_MOVEB) { errmsg(rtk, "time sync error for moving-base (age=%.1f)\n", rtk->sol.age); return 0; } for (i = 0; i < 6; i++) { rtk->rb[i] = solb.rr[i]; } /* time-synchronized position of base station */ for (i = 0; i < 3; i++) { rtk->rb[i] += rtk->rb[i + 3] * rtk->sol.age; } } else { rtk->sol.age = static_cast(timediff(obs[0].time, obs[nu].time)); if (std::fabs(rtk->sol.age) > opt->maxtdiff) { errmsg(rtk, "age of differential error (age=%.1f)\n", rtk->sol.age); outsolstat(rtk); return 1; } } /* relative potitioning */ relpos(rtk, obs, nu, nr, nav); outsolstat(rtk); return 1; }