#include #include "rtklib.h" /* constants/macros ----------------------------------------------------------*/ #define SQR(x) ((x) * (x)) #define SQRT(x) ((x) <= 0.0 || (x) != (x) ? 0.0 : sqrt(x)) #define MIN(x, y) ((x) <= (y) ? (x) : (y)) #define MAX(x, y) ((x) >= (y) ? (x) : (y)) #define ROUND(x) (int)floor((x) + 0.5) #define VAR_POS SQR(30.0) /* initial variance of receiver pos (m^2) */ #define VAR_POS_FIX SQR(1e-4) /* initial variance of fixed receiver pos (m^2) */ #define VAR_VEL SQR(10.0) /* initial variance of receiver vel ((m/s)^2) */ #define VAR_ACC SQR(10.0) /* initial variance of receiver acc ((m/ss)^2) */ #define VAR_GRA SQR(0.001) /* initial variance of gradient (m^2) */ #define INIT_ZWD 0.15 /* initial zwd (m) */ #define GAP_RESION 120 /* gap to reset ionosphere parameters (epochs) */ #define TTOL_MOVEB (1.0 + 2 * DTTOL) /* time sync tolerance for moving-baseline (s) */ /* number of parameters (pos,ionos,tropos,hw-bias,phase-bias,real,estimated) */ #define NF(opt) ((opt)->ionoopt == IONOOPT_IFLC ? 1 : (opt)->nf) #define NP(opt) ((opt)->dynamics == 0 ? 3 : 9) #define NI(opt) ((opt)->ionoopt != IONOOPT_EST ? 0 : MAXSAT) #define NT(opt) ((opt)->tropopt < TROPOPT_EST ? 0 : ((opt)->tropopt < TROPOPT_ESTG ? 2 : 6)) #define NL(opt) ((opt)->glomodear != GLO_ARMODE_AUTOCAL ? 0 : NFREQGLO) #define NB(opt) ((opt)->mode <= PMODE_DGPS ? 0 : MAXSAT * NF(opt)) #define NR(opt) (NP(opt) + NI(opt) + NT(opt) + NL(opt)) #define NX(opt) (NR(opt) + NB(opt)) /* state variable index */ #define II(s, opt) (NP(opt) + (s)-1) /* ionos (s:satellite no) */ #define IT(r, opt) (NP(opt) + NI(opt) + NT(opt) / 2 * (r)) /* tropos (r:0=rov,1:ref) */ #define IL(f, opt) (NP(opt) + NI(opt) + NT(opt) + (f)) /* receiver h/w bias */ #define IB(s, f, opt) (NR(opt) + MAXSAT * (f) + (s)-1) /* phase bias (s:satno,f:freq) */ /* poly coeffs used to adjust AR ratio by # of sats, derived by fitting to example from: https://www.tudelft.nl/citg/over-faculteit/afdelingen/geoscience-remote-sensing/research/lambda/lambda*/ static double ar_poly_coeffs[3][5] = { {-1.94058448e-01, -7.79023476e+00, 1.24231120e+02, -4.03126050e+02, 3.50413202e+02}, {6.42237302e-01, -8.39813962e+00, 2.92107285e+01, -2.37577308e+01, -1.14307128e+00}, {-2.22600390e-02, 3.23169103e-01, -1.39837429e+00, 2.19282996e+00, -5.34583971e-02}}; /* global variables ----------------------------------------------------------*/ static int statlevel = 0; /* rtk status output level (0:off) */ static FILE *fp_stat = NULL; /* rtk status file pointer */ static char file_stat[1024] = ""; /* rtk status file original path */ static gtime_t time_stat = {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,cmn_bias * 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) * cmn_bias : common phase bias removed from all states * * $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,icbias,bias,bias_var,lambda * 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 * icbias : interchannel bias (GLONASS) * bias : phase bias * bias_var : variance of phase bias * lambda : wavelength * *-----------------------------------------------------------------------------*/ extern int rtkopenstat(const char *file, int level) { gtime_t time = utc2gpst(timeget()); char path[1024]; trace(3, "rtkopenstat: file=%s level=%d\n", file, level); if (level <= 0) return 0; reppath(file, path, time, "", ""); if (!(fp_stat = fopen(path, "w"))) { trace(1, "rtkopenstat: file open error path=%s\n", path); return 0; } strcpy(file_stat, file); time_stat = time; statlevel = level; return 1; } /* close solution status file -------------------------------------------------- * close solution status file * args : none * return : none *-----------------------------------------------------------------------------*/ extern void rtkclosestat(void) { trace(3, "rtkclosestat:\n"); if (fp_stat) fclose(fp_stat); fp_stat = NULL; file_stat[0] = '\0'; statlevel = 0; } /* write solution status to buffer -------------------------------------------*/ extern int rtkoutstat(rtk_t *rtk, char *buff) { ssat_t *ssat; double tow, pos[3], vel[3], acc[3], vela[3] = {0}, acca[3] = {0}, xa[3]; int i, j, week, est, nfreq, nf = NF(&rtk->opt); char id[32], *p = buff; if (rtk->sol.stat <= SOLQ_NONE) { return 0; } 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; p += sprintf(p, "$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 { p += sprintf(p, "$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); p += sprintf(p, "$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); p += sprintf(p, "$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 */ p += sprintf(p, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f,%.2f\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, rtk->com_bias); /* ionospheric parameters */ if (est && rtk->opt.ionoopt == IONOOPT_EST) { for (i = 0; i < MAXSAT; i++) { ssat = rtk->ssat + i; if (!ssat->vs) continue; satno2id(i + 1, id); j = II(i + 1, &rtk->opt); xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; p += sprintf(p, "$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n", week, tow, rtk->sol.stat, id, 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(i, &rtk->opt); xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; p += sprintf(p, "$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 == GLO_ARMODE_AUTOCAL) { for (i = 0; i < nfreq; i++) { j = IL(i, &rtk->opt); xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; p += sprintf(p, "$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, i + 1, rtk->x[j], xa[0]); } } return (int)(p - buff); } /* swap solution status file -------------------------------------------------*/ static void swapsolstat(void) { gtime_t time = utc2gpst(timeget()); char path[1024]; if ((int)(time2gpst(time, NULL) / INT_SWAP_STAT) == (int)(time2gpst(time_stat, NULL) / 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, "w"))) { trace(2, "swapsolstat: file open error path=%s\n", path); return; } trace(3, "swapsolstat: path=%s\n", path); } /* output solution status ----------------------------------------------------*/ static void outsolstat(rtk_t *rtk, const nav_t *nav) { ssat_t *ssat; double tow; char buff[MAXSOLMSG + 1], id[32]; int i, j, k, n, week, nfreq, nf = NF(&rtk->opt); if (statlevel <= 0 || !fp_stat || !rtk->sol.stat) return; trace(3, "outsolstat:\n"); /* swap solution status file */ swapsolstat(); /* write solution status */ n = rtkoutstat(rtk, buff); buff[n] = '\0'; fputs(buff, fp_stat); if (rtk->sol.stat == SOLQ_NONE || statlevel <= 1) return; tow = time2gpst(rtk->sol.time, &week); nfreq = rtk->opt.mode >= PMODE_DGPS ? nf : 1; /* write residuals and status */ for (i = 0; i < MAXSAT; i++) { ssat = rtk->ssat + i; if (!ssat->vs) continue; satno2id(i + 1, id); for (j = 0; j < nfreq; j++) { k = IB(i + 1, j, &rtk->opt); fprintf(fp_stat, "$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d,%.2f,%.6f,%.5f\n", week, tow, id, j + 1, ssat->azel[0] * R2D, ssat->azel[1] * R2D, ssat->resp[j], ssat->resc[j], ssat->vsat[j], ssat->snr_rover[j] * SNR_UNIT, ssat->fix[j], ssat->slip[j] & 3, ssat->lock[j], ssat->outc[j], ssat->slipc[j], ssat->rejc[j], rtk->x[k], rtk->P[k + k * rtk->nx], ssat->icbias[j]); } } } /* save error message --------------------------------------------------------*/ static void errmsg(rtk_t *rtk, const char *format, ...) { char buff[256], tstr[32]; int n; va_list ap; time2str(rtk->sol.time, tstr, 2); n = sprintf(buff, "%s: ", tstr + 11); va_start(ap, format); n += vsprintf(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 ---------------------------------------------*/ static double sdobs(const obsd_t *obs, int i, int j, int k) { double pi = (k < NFREQ) ? obs[i].L[k] : obs[i].P[k - NFREQ]; double pj = (k < NFREQ) ? obs[j].L[k] : obs[j].P[k - NFREQ]; return pi == 0.0 || pj == 0.0 ? 0.0 : pi - pj; } /* single-differenced geometry-free linear combination of phase --------------*/ static double gfobs(const obsd_t *obs, int i, int j, int k, const nav_t *nav) { double freq1, freq2, L1, L2; freq1 = sat2freq(obs[i].sat, obs[i].code[0], nav); freq2 = sat2freq(obs[i].sat, obs[i].code[k], nav); L1 = sdobs(obs, i, j, 0); L2 = sdobs(obs, i, j, k); if (freq1 == 0.0 || freq2 == 0.0 || L1 == 0.0 || L2 == 0.0) return 0.0; return L1 * CLIGHT / freq1 - L2 * CLIGHT / freq2; } /* single-differenced measurement error variance -----------------------------*/ static double varerr(int sat, int sys, double el, double snr_rover, double snr_base, double bl, double dt, int f, const prcopt_t *opt, const obsd_t *obs) { double a, b, c, d, e; double snr_max = opt->err[5]; double fact = 1.0; double sinel = sin(el), var; int nf = NF(opt), frq, code; frq = f % nf; code = f < nf ? 0 : 1; /* increase variance for pseudoranges */ if (code) fact = opt->eratio[frq]; if (fact <= 0.0) fact = opt->eratio[0]; /* adjust variances for constellation */ switch (sys) { case SYS_GPS: fact *= EFACT_GPS; break; case SYS_GLO: fact *= EFACT_GLO; break; case SYS_GAL: fact *= EFACT_GAL; break; case SYS_SBS: fact *= EFACT_SBS; break; case SYS_QZS: fact *= EFACT_QZS; break; case SYS_CMP: fact *= EFACT_CMP; break; case SYS_IRN: fact *= EFACT_IRN; break; default: fact *= EFACT_GPS; break; } /* adjust variance for config parameters */ a = fact * opt->err[1]; /* base term */ b = fact * opt->err[2]; /* el term */ c = opt->err[3] * bl / 1E4; /* baseline term */ d = CLIGHT * opt->sclkstab * dt; /* clock term */ /* calculate variance */ var = 2.0 * (a * a + b * b / sinel / sinel + c * c) + d * d; if (opt->err[6] > 0) { /* add SNR term */ e = fact * opt->err[6]; var += e * e * (pow(10, 0.1 * MAX(snr_max - snr_rover, 0)) + pow(10, 0.1 * MAX(snr_max - snr_base, 0))); } if (opt->err[7] > 0.0) { /* add rcvr stdevs term */ if (code) var += SQR(opt->err[7] * 0.01 * (1 << (obs->Pstd[frq] + 5))); /* 0.01*2^(n+5) */ else var += SQR(opt->err[7] * obs->Lstd[frq] * 0.004 * 0.2); /* 0.004 cycles -> m) */ } var *= (opt->ionoopt == IONOOPT_IFLC) ? SQR(3.0) : 1.0; return var; } /* baseline length -----------------------------------------------------------*/ static 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(dr, 3); } /* initialize state and covariance -------------------------------------------*/ static void initx(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 选择共视卫星 主要有高度角的限制 --------------*/ static int selsat(const obsd_t *obs, double *azel, int nu, int nr, const prcopt_t *opt, int *sat, int *iu, int *ir) { int i, j, 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 更新rtk中的位置、速度、加速度值和协方差 函数参数，2个： rtk_t *rtk IO rtk控制结构体 double tt I 本次更新与上次更新的时间差 返回类型: 无 -------------------------*/ static void udpos(rtk_t *rtk, double tt) { double *F, *P, *FP, *x, *xp, pos[3], Q[9] = {0}, Qv[9], var = 0.0; int i, j, *ix, nx; trace(3, "udpos : tt=%.3f\n", tt); /* fixed mode 1、若为 PMODE_FIXED 模式，直接从选项中取得位置值给rtk->x，然后返回*/ if (rtk->opt.mode == PMODE_FIXED) { for (i = 0; i < 3; i++) initx(rtk, rtk->opt.ru[i], VAR_POS_FIX, i); return; } /* initialize position for first epoch 2、若为第一个历元，用rtk->sol中的位置（单点定位结果）值初始化rtk->x。 若为dynamics模式（即需要估计速度和加速度），一并初始化*/ if (norm(rtk->x, 3) <= 0.0) { for (i = 0; i < 3; i++) initx(rtk, rtk->sol.rr[i], VAR_POS, i); if (rtk->opt.dynamics) { for (i = 3; i < 6; i++) initx(rtk, rtk->sol.rr[i], VAR_VEL, i); for (i = 6; i < 9; i++) initx(rtk, 1E-6, VAR_ACC, i); } } /* static mode 3、若为 PMODE_STATIC 模式，不进行更新，返回*/ if (rtk->opt.mode == PMODE_STATIC || rtk->opt.mode == PMODE_STATIC_START) return; /* kinmatic mode without dynamics 若动态模型为一阶模型，则直接重置方差和位置 4、若非dynamics模式，用rtk->sol中的位置值初始化rtk->x，然后返回*/ if (!rtk->opt.dynamics) { for (i = 0; i < 3; i++) initx(rtk, rtk->sol.rr[i], VAR_POS, i); return; } /* check variance of estimated position 5、检查位置协方差(P阵)，若大于阈值VAR_POS则用rtk->sol中的位置值重置rtk->x*/ for (i = 0; i < 3; i++) var += rtk->P[i + i * rtk->nx]; var /= 3.0; if (var > VAR_POS) { /* reset position with（when） large variance */ for (i = 0; i < 3; i++) initx(rtk, rtk->sol.rr[i], VAR_POS, i); for (i = 3; i < 6; i++) initx(rtk, rtk->sol.rr[i], VAR_VEL, i); for (i = 6; i < 9; i++) initx(rtk, 1E-6, VAR_ACC, i); trace(2, "reset rtk position due to large variance: var=%.3f\n", var); return; } /* generate valid state index 6、检查状态量中有效的状态数量，并记录索引id，检查卫星数量*/ ix = imat(rtk->nx, 1); for (i = nx = 0; i < rtk->nx; i++) { /* TODO: The b34 code causes issues so use b33 code for now */ if (i < 9 || (rtk->x[i] != 0.0 && rtk->P[i + i * rtk->nx] > 0.0)) ix[nx++] = i; } /* state transition of position/velocity/acceleration 7、位置速度加速度状态更新*/ /*7.1首先，更新F阵和状态量x，以及P阵*/ F = eye(nx); P = mat(nx, nx); FP = mat(nx, nx); x = mat(nx, 1); xp = mat(nx, 1); for (i = 0; i < 6; i++) { F[i + (i + 3) * nx] = tt; } /* include accel terms if filter is converged */ if (var < rtk->opt.thresar[1]) { for (i = 0; i < 3; i++) { F[i + (i + 6) * nx] = SQR(tt) / 2.0; } } else trace(3, "pos var too high for accel term\n"); for (i = 0; i < nx; i++) { x[i] = rtk->x[ix[i]]; for (j = 0; j < nx; j++) { P[i + j * nx] = rtk->P[ix[i] + ix[j] * rtk->nx]; } } /* x=F*x, P=F*P*F+Q 7.2 其次，进行状态递推。 根据Kalman滤波的预测方程 x=Fx 和 P=FP*F+Q 更新(参考RTKLIB Manual P161 E.7.4, E.7.5)。其中更新Q时需要坐标转换*/ matmul("NN", nx, 1, nx, 1.0, F, x, 0.0, xp); matmul("NN", nx, nx, nx, 1.0, F, P, 0.0, FP); matmul("NT", nx, nx, nx, 1.0, FP, F, 0.0, P); for (i = 0; i < nx; i++) { rtk->x[ix[i]] = xp[i]; for (j = 0; j < nx; j++) { rtk->P[ix[i] + ix[j] * rtk->nx] = P[i + j * nx]; } } /* process noise added to only acceleration 7.3 最后，考虑驱动噪声，Q阵(更新方式)分别设置水平和垂直噪声驱动方差？转化？*/ Q[0] = Q[4] = SQR(rtk->opt.prn[3]) * fabs(tt); Q[8] = SQR(rtk->opt.prn[4]) * fabs(tt); 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(ix); free(F); free(P); free(FP); free(x); free(xp); } /* temporal update of ionospheric parameters ---------------------------------*/ static void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns) { double el, fact; int i, j; trace(3, "udion : tt=%.3f bl=%.0f ns=%d\n", tt, bl, ns); for (i = 1; i <= MAXSAT; i++) { j = II(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(sat[i], &rtk->opt); if (rtk->x[j] == 0.0) { initx(rtk, 1E-6, SQR(rtk->opt.std[1] * bl / 1E4), 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] += SQR(rtk->opt.prn[1] * bl / 1E4 * fact) * fabs(tt); } } } /* temporal update of tropospheric parameters --------------------------------*/ static void udtrop(rtk_t *rtk, double tt, double bl) { int i, j, k; trace(3, "udtrop : tt=%.3f\n", tt); for (i = 0; i < 2; i++) { j = IT(i, &rtk->opt); if (rtk->x[j] == 0.0) { initx(rtk, INIT_ZWD, SQR(rtk->opt.std[2]), j); /* initial zwd */ if (rtk->opt.tropopt >= TROPOPT_ESTG) { for (k = 0; k < 2; k++) initx(rtk, 1E-6, VAR_GRA, ++j); } } else { rtk->P[j + j * rtk->nx] += SQR(rtk->opt.prn[2]) * fabs(tt); if (rtk->opt.tropopt >= TROPOPT_ESTG) { for (k = 0; k < 2; k++) { rtk->P[++j * (1 + rtk->nx)] += SQR(rtk->opt.prn[2] * 0.3) * fabs(tt); } } } } } /* temporal update of receiver h/w biases ------------------------------------*/ static void udrcvbias(rtk_t *rtk, double tt) { int i, j; trace(3, "udrcvbias: tt=%.3f\n", tt); for (i = 0; i < NFREQGLO; i++) { j = IL(i, &rtk->opt); if (rtk->x[j] == 0.0) { /* add small offset to avoid initializing with zero */ initx(rtk, rtk->opt.thresar[2] + 1e-6, rtk->opt.thresar[3], j); } /* hold to fixed solution */ else if (rtk->nfix >= rtk->opt.minfix) { initx(rtk, rtk->xa[j], rtk->Pa[j + j * rtk->na], j); } else { rtk->P[j + j * rtk->nx] += SQR(rtk->opt.thresar[4]) * fabs(tt); } } } /* detect cycle slip by LLI --------------------------------------------------*/ static void detslp_ll(rtk_t *rtk, const obsd_t *obs, int i, int rcv) { uint32_t slip, LLI; int f, sat = obs[i].sat; trace(4, "detslp_ll: i=%d rcv=%d\n", i, rcv); for (f = 0; f < rtk->opt.nf; f++) { if ((obs[i].L[f] == 0.0 && obs[i].LLI[f] == 0) || fabs(timediff(obs[i].time, rtk->ssat[sat - 1].pt[rcv - 1][f])) < DTTOL) { 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] |= (uint8_t)slip; rtk->ssat[sat - 1].half[f] = (obs[i].LLI[f] & 2) ? 0 : 1; } } /* detect cycle slip by geometry free phase jump -----------------------------*/ static void detslp_gf(rtk_t *rtk, const obsd_t *obs, int i, int j, const nav_t *nav) { int k, sat = obs[i].sat; double gf0, gf1; trace(4, "detslp_gf: i=%d j=%d\n", i, j); /* skip check if slip already detected */ for (k = 0; k < rtk->opt.nf; k++) if (rtk->ssat[sat - 1].slip[k] & 1) return; for (k = 1; k < rtk->opt.nf; k++) { /* calc SD geomotry free LC of phase between freq0 and freqk */ if ((gf1 = gfobs(obs, i, j, k, nav)) == 0.0) continue; gf0 = rtk->ssat[sat - 1].gf[k - 1]; /* retrieve previous gf */ rtk->ssat[sat - 1].gf[k - 1] = gf1; /* save current gf for next epoch */ if (gf0 != 0.0 && fabs(gf1 - gf0) > rtk->opt.thresslip) { rtk->ssat[sat - 1].slip[0] |= 1; rtk->ssat[sat - 1].slip[k] |= 1; errmsg(rtk, "slip detected GF jump (sat=%2d L1-L%d dGF=%.3f)\n", sat, k + 1, gf0 - gf1); } } } /* detect cycle slip by doppler and phase difference -------------------------*/ static void detslp_dop(rtk_t *rtk, const obsd_t *obs, int i, int rcv, const nav_t *nav) { int f, sat = obs[i].sat; double tt, dph, dpt, lam, thres, maxacc; trace(4, "detslp_dop: i=%d rcv=%d\n", i, rcv); if (rtk->opt.thresdop <= 0) return; /* skip test if doppler thresh <= 0 */ for (f = 0; f < rtk->opt.nf; f++) { if (obs[i].L[f] == 0.0 || obs[i].D[f] == 0.0 || rtk->ssat[sat - 1].ph[rcv - 1][f] == 0.0) { continue; } if (fabs(tt = timediff(obs[i].time, rtk->ssat[sat - 1].pt[rcv - 1][f])) < DTTOL) continue; if ((lam = sat2freq(obs[i].sat, obs[i].code[f], nav)) <= 0.0) continue; /* cycle slip threshold (cycle) */ if (rtk->opt.mode == PMODE_STATIC) maxacc = 0; else maxacc = rtk->opt.prn[3] * 4; /* set max accel to 4 stdevs of haccel */ thres = maxacc * tt * tt / 2.0 / lam + rtk->opt.thresdop * fabs(tt); /* phase difference and doppler x time (cycle) */ dph = obs[i].L[f] - rtk->ssat[sat - 1].ph[rcv - 1][f]; dpt = -obs[i].D[f] * tt; if (fabs(dph - dpt) <= thres) continue; rtk->ssat[sat - 1].slip[f] |= 1; errmsg(rtk, "slip detected doppler (sat=%2d rcv=%d dL%d=%.3f thres=%.3f)\n", sat, rcv, f + 1, dph - dpt, thres); } } /* temporal update of phase biases -------------------------------------------*/ static 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, pr, cp1, cp2, pr1, pr2, *bias, offset, freqi, freq1, freq2, C1, C2; int i, j, k, slip, rejc, reset, nf = NF(&rtk->opt), sysi; trace(3, "udbias : tt=%.3f ns=%d\n", tt, ns); for (i = 0; i < ns; i++) { /* detect cycle slip by LLI */ for (k = 0; k < rtk->opt.nf; k++) rtk->ssat[sat[i] - 1].slip[k] &= 0xFC; detslp_ll(rtk, obs, iu[i], 1); detslp_ll(rtk, obs, ir[i], 2); /* detect cycle slip by doppler and phase difference */ detslp_dop(rtk, obs, iu[i], 1, nav); detslp_dop(rtk, obs, ir[i], 2, nav); /* detect cycle slip by geometry-free phase jump */ detslp_gf(rtk, obs, iu[i], ir[i], nav); /* update half-cycle valid flag */ for (k = 0; k < nf; k++) { rtk->ssat[sat[i] - 1].half[k] = !((obs[iu[i]].LLI[k] & 2) || (obs[ir[i]].LLI[k] & 2)); } } for (k = 0; k < nf; k++) { /* reset phase-bias if instantaneous AR or expire obs outage counter */ for (i = 1; i <= MAXSAT; i++) { reset = ++rtk->ssat[i - 1].outc[k] > (uint32_t)rtk->opt.maxout; if (rtk->opt.modear == ARMODE_INST && rtk->x[IB(i, k, &rtk->opt)] != 0.0) { initx(rtk, 0.0, 0.0, IB(i, k, &rtk->opt)); } else if (reset && rtk->x[IB(i, k, &rtk->opt)] != 0.0) { initx(rtk, 0.0, 0.0, IB(i, k, &rtk->opt)); trace(3, "udbias : obs outage counter overflow (sat=%3d L%d n=%d)\n", i, k + 1, rtk->ssat[i - 1].outc[k]); rtk->ssat[i - 1].outc[k] = 0; } if (rtk->opt.modear != ARMODE_INST && reset) { rtk->ssat[i - 1].lock[k] = -rtk->opt.minlock; } } /* reset phase-bias state if detecting cycle slip or outlier */ for (i = 0; i < ns; i++) { j = IB(sat[i], k, &rtk->opt); rtk->P[j + j * rtk->nx] += rtk->opt.prn[0] * rtk->opt.prn[0] * fabs(tt); slip = rtk->ssat[sat[i] - 1].slip[k]; rejc = rtk->ssat[sat[i] - 1].rejc[k]; if (rtk->opt.ionoopt == IONOOPT_IFLC) slip |= rtk->ssat[sat[i] - 1].slip[1]; if (rtk->opt.modear == ARMODE_INST || (!(slip & 1) && rejc < 2)) continue; rtk->x[j] = 0.0; rtk->ssat[sat[i] - 1].rejc[k] = 0; rtk->ssat[sat[i] - 1].lock[k] = -rtk->opt.minlock; /* retain icbiases for GLONASS sats */ if (rtk->ssat[sat[i] - 1].sys != SYS_GLO) rtk->ssat[sat[i] - 1].icbias[k] = 0; } bias = zeros(ns, 1); /* estimate approximate phase-bias by delta phase - delta code */ for (i = j = 0, offset = 0.0; i < ns; i++) { freqi = sat2freq(sat[i], obs[iu[i]].code[k], nav); if (rtk->opt.ionoopt != IONOOPT_IFLC) { /* phase diff between rover and base in cycles */ cp = sdobs(obs, iu[i], ir[i], k); /* cycle */ /* pseudorange diff between rover and base in meters */ pr = sdobs(obs, iu[i], ir[i], k + NFREQ); if (cp == 0.0 || pr == 0.0 || freqi == 0.0) continue; /* translate cycles diff to meters and subtract pseudorange diff */ bias[i] = cp * (CLIGHT / freqi) - pr; } else { /* use ionosphere free calc with 2 freqs */ 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); freq1 = sat2freq(sat[i], obs[iu[i]].code[0], nav); freq2 = sat2freq(sat[i], obs[iu[i]].code[1], nav); if (cp1 == 0.0 || cp2 == 0.0 || pr1 == 0.0 || pr2 == 0.0 || freq1 <= 0.0 || freq2 <= 0.0) continue; C1 = SQR(freq1) / (SQR(freq1) - SQR(freq2)); C2 = -SQR(freq2) / (SQR(freq1) - SQR(freq2)); bias[i] = (C1 * cp1 * CLIGHT / freq1 + C2 * cp2 * CLIGHT / freq2) - (C1 * pr1 + C2 * pr2); } /* offset = sum of (bias - phase-bias) for all valid sats in meters */ if (rtk->x[IB(sat[i], k, &rtk->opt)] != 0.0) { offset += bias[i] - rtk->x[IB(sat[i], k, &rtk->opt)] * CLIGHT / freqi; j++; } } /* set initial states of phase-bias for uninitialized satellites */ rtk->com_bias = j > 0 ? offset / j : 0; for (i = 0; i < ns; i++) { if (bias[i] == 0.0 || rtk->x[IB(sat[i], k, &rtk->opt)] != 0.0) continue; freqi = sat2freq(sat[i], obs[iu[i]].code[k], nav); sysi = rtk->ssat[sat[i] - 1].sys; initx(rtk, (bias[i] - rtk->com_bias) * freqi / CLIGHT, SQR(rtk->opt.std[0]), IB(sat[i], k, &rtk->opt)); trace(3, " sat=%3d, F=%d: init phase=%.3f\n", sat[i], k + 1, (bias[i] - rtk->com_bias * freqi / CLIGHT)); rtk->ssat[sat[i] - 1].lock[k] = -rtk->opt.minlock; } free(bias); } } /* temporal update of states 更新状态值 rtk->x 及其误差协方差 rtk->P 函数参数，7个： rtk_t *rtk IO rtk控制结构体 obsd_t *obs I 观测数据 int sat I 接收机和基站共同观测的卫星号列表 int *iu I 接收机和基站共同观测的卫星在接收机观测值中的index值列表 int *ir I 接收机和基站共同观测的卫星在基站观测值中的index值列表 int ns I 接收机和基站共同观测的卫星个数 nav_t *nav I 导航数据 返回类型: 无 状态变量包含接收机位置、速度、加速度值、[每颗卫星的电离层参数]、[对流层参数]、[接收机硬件偏移]、每颗卫星的载波偏移。 其中载波偏移包含周整模糊度以及小数部分，可参考 RTKLIB Manual P139 E.3.5 --------------------------------------------------*/ static 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 = rtk->tt, bl, dr[3]; trace(3, "udstate : ns=%d\n", ns); /* temporal update of position/velocity/acceleration 1、调用 udpos 根据不同模式更新rtk中的位置、速度、加速度值和协方差*/ udpos(rtk, tt); /* temporal update of ionospheric parameters 2、若 电离层模式>=IONOOPT_EST，调用 udion 更新状态 rtk->x 中的电离层参数（MAXSAT个）及其协方差*/ /*过程2、3中更新状态x，只根据需要作初始化，给初值；更新时只更新协方差*/ if (rtk->opt.ionoopt >= IONOOPT_EST) { bl = baseline(rtk->x, rtk->rb, dr); udion(rtk, tt, bl, sat, ns); } /* temporal update of tropospheric parameters 3、若 对流层模式>=TROPOPT_EST，调用 udtrop 更新状态 rtk->x 中的对流层参数（2或6个）及其协方差。 TROPOPT_EST初始化状态 rtk->x 中的对流层参数*/ if (rtk->opt.tropopt >= TROPOPT_EST) { udtrop(rtk, tt, bl); } /* temporal update of receiver h/w bias 4、若为 GLONASS AR模式，调用 udrcvbias 更新接收机硬件偏移*/ if (rtk->opt.glomodear == GLO_ARMODE_AUTOCAL && (rtk->opt.navsys & SYS_GLO)) { udrcvbias(rtk, tt); } /* temporal update of phase-bias 5、若 模式>PMODE_DGPS，调用 udbias 更新载波相位偏移状态值以及其误差协方差*/ if (rtk->opt.mode > PMODE_DGPS) { udbias(rtk, tt, obs, sat, iu, ir, ns, nav); } } /* UD (undifferenced) phase/code residual for satellite 计算接收机或基站对某一颗卫星的没有差分的相位/码残差（Zero-Difference Residuals）。y = 观测值 - r - dant。 函数参数，9个： int base I 0表示接收机，1表示基站 double r I 经过钟差和对流层校正后的几何距离。 obsd_t *obs I 观测数据 nav_t *nav I 导航数据 double *azel I 方位角和俯仰角 (rad) double *dant I 接收机天线校正值 prcopt_t *opt I 处理过程选项 double *y O 相位/码残差 返回类型: 无 */ static 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, double *freq) { double freq1, freq2, C1, C2, dant_if; int i, nf = NF(opt); // 1、按电离层校正模式是否为 IONOOPT_IFLC 分两种情况。判断是否无电离层组合（双频观测量：可配置： 无电离层组合） if (opt->ionoopt == IONOOPT_IFLC) { /* iono-free linear combination 2、若是：检测信噪比，计算天线校正值 dant_if，然后计算残差*/ freq1 = sat2freq(obs->sat, obs->code[0], nav); freq2 = sat2freq(obs->sat, obs->code[1], nav); if (freq1 == 0.0 || freq2 == 0.0) return; if (testsnr(base, 0, azel[1], obs->SNR[0] * SNR_UNIT, &opt->snrmask) || testsnr(base, 1, azel[1], obs->SNR[1] * SNR_UNIT, &opt->snrmask)) return; C1 = SQR(freq1) / (SQR(freq1) - SQR(freq2)); C2 = -SQR(freq2) / (SQR(freq1) - SQR(freq2)); 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] * CLIGHT / freq1 + C2 * obs->L[1] * CLIGHT / freq2 - 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; } freq[0] = 1.0; } // 3、若否：检测信噪比，然后计算残差 else { for (i = 0; i < nf; i++) { if ((freq[i] = sat2freq(obs->sat, obs->code[i], nav)) == 0.0) continue; /* check SNR mask */ if (testsnr(base, i, azel[1], obs->SNR[i] * SNR_UNIT, &opt->snrmask)) { continue; } /* residuals = observable - estimated range */ if (obs->L[i] != 0.0) y[i] = obs->L[i] * CLIGHT / freq[i] - r - dant[i]; if (obs->P[i] != 0.0) y[i + nf] = obs->P[i] - r - dant[i]; } } } /* undifferenced phase/code residuals ---------------------------------------- calculate zero diff residuals [observed pseudorange - range] output is in y[0:nu-1], only shared input with base is nav args: I base: 0=base,1=rover I obs = sat observations I n = # of sats I rs [(0:2)+i*6]= sat position {x,y,z} (m) I dts[(0:1)+i*2]= sat clock {bias,drift} (s|s/s) I var = variance of ephemeris I svh = sat health flags I nav = sat nav data I rr = rcvr pos (x,y,z) I opt = options I index: 0=base,1=rover O y[(0:1)+i*2] = zero diff residuals {phase,code} (m) O e = line of sight unit vectors to sats O azel = [az, el] to sats 计算接收机或基站的没有差分的相位/码残差（Zero-Difference Residuals） 函数参数，13个： int base I 0表示接收机，1表示基站 obsd_t *obs I 观测数据 int n I 观测数据的数量 double *rs I 卫星位置和速度，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s) double *dts I 卫星钟差，长度为2*n， {bias,drift} (s|s/s) int *svh I 卫星健康标志 (-1:correction not available) nav_t *nav I 导航数据 double *rr I 接收机/基站的位置和速度，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s) prcopt_t *opt I 处理过程选项 int index I 0表示接收机，1表示基站，与参数 base 重复了 double *y O 相位/码残差 double *e O 观测矢量 (ecef) double *azel O 方位角和俯仰角 (rad) 返回类型: int O (1:ok,0:error) */ static int zdres(int base, const obsd_t *obs, int n, const double *rs, const double *dts, const double *var, const int *svh, const nav_t *nav, const double *rr, const prcopt_t *opt, int index, double *y, double *e, double *azel, double *freq) { double r, rr_[3], pos[3], dant[NFREQ] = {0}, disp[3]; // r 几何距离 double zhd, zazel[] = {0.0, 90.0 * D2R}; int i, nf = NF(opt); #ifdef ONLY_2FREQ int use_freq, k; #endif trace(3, "zdres : n=%d\n", n); /* init residuals to zero */ for (i = 0; i < n * nf * 2; i++) y[i] = 0.0; if (norm(rr, 3) <= 0.0) return 0; /* no receiver position 1、若没有接收机位置，返回0。*/ /* rr_ = local copy of rcvr pos 2、接收机位置传给 rr_*/ for (i = 0; i < 3; i++) rr_[i] = rr[i]; /* adjust rcvr pos for earth tide correction 3、若需要地球潮校正，调用 tidedisp 对 rr_ 进行校正。地球潮包含固体潮、极潮和海潮负荷*/ 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]; } /* translate rcvr pos from ecef to geodetic 4、rr_ 转换坐标系到大地坐标系（经纬高） pos*/ ecef2pos(rr_, pos); /* loop through satellites 5、大循环，对每一个观测量 重复6~11直到大循环结束*/ for (i = 0; i < n; i++) { /* compute geometric-range and azimuth/elevation angle 6、调用 geodist 计算卫星到接收机的几何距离 r，调用 satazel 计算仰角，若仰角低于阈值，排除此观测量*/ 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? 7、若设置有卫星需要排除的，排除掉*/ if (satexclude(obs[i].sat, var[i], svh[i], opt)) continue; /*test available number of observations 解决设置双频但单频解算问题*/ #ifdef ONLY_2FREQ use_freq = 0; for (k = 0; k < nf; k++) { if (obs[i].L[k] == 0.0 || obs[i].P[k] == 0.0) use_freq = 1; } if (use_freq == 1) { trace(3, "No enough frequency according to the conf: time = %s sat =%2d \n", time_str(obs[0].time, 2), obs[i].sat); continue; } #endif /* adjust range for satellite clock-bias 8、根据卫星钟差校正 r */ r += -CLIGHT * dts[i * 2]; /* adjust range for troposphere delay model (hydrostatic) 9、根据对流层模型设置，调用 tropmodel 和 tropmapf 计算对流层延时校正值并校正 r*/ zhd = tropmodel(obs[0].time, pos, zazel, 0.0); r += tropmapf(obs[i].time, pos, azel + i * 2, NULL) * zhd; /* calc receiver antenna phase center correction 10、根据接收机天线模型调用 antmodel 计算校正值 dant（对每一个频率都有一个值）*/ // antmodel(opt->pcvr + index, opt->antdel[index], azel + i * 2, opt->posopt[1], // dant); /* calc undifferenced phase/code residual for satellite 11、调用 zdres_sat 计算没有差分的相位/码残差 y*/ zdres_sat(base, r, obs + i, nav, azel + i * 2, dant, opt, y + i * nf * 2, freq + i * nf); } 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++) { if ((obs[i].L[0] == 0 && obs[i].L[1] == 0) || base == 0) continue; trace(3, "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(3, "y=\n"); tracemat(3, y, nf * 2, n, 13, 3); return 1; } /* test valid observation data -----------------------------------------------*/ static int validobs(int i, int j, int f, int nf, 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 --------------------------- * * nb[n]: # of sat pairs in group * n: # of groups (2 for each system, phase and code) * Ri[nv]: variances of first sats in double diff pairs * Rj[nv]: variances of 2nd sats in double diff pairs * nv: total # of sat pairs * R[nv][nv]: double diff measurement err covariance matrix */ static void ddcov(const int *nb, int n, const double *Ri, const double *Rj, int nv, double *R) { int i, j, k = 0, 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++]) { /* loop through each system */ 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 ------------------------------------------------*/ static 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], b[3], bb, 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]; b[i] = x[i] - xb[i]; } bb = norm(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 > SQR(thres * bb)) { trace(3, "constbl : equation nonlinear (bb=%.3f var=%.3f)\n", bb, var); /* return 0; */ /* threshold too strict for all use cases, report error but continue on */ } /* 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] = SQR(rtk->opt.baseline[1]); trace(4, "baseline len v=%13.3f R=%8.6f %8.6f\n", v[index], Ri[index], Rj[index]); return 1; } /* precise tropspheric model -------------------------------------------------*/ static 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, cotz, grad_n, grad_e; int i = IT(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]; } /* test satellite system (m=0:GPS/SBS,1:GLO,2:GAL,3:BDS,4:QZS,5:IRN) ---------*/ static int test_sys(int sys, int m) { switch (sys) { case SYS_GPS: return m == 0; case SYS_SBS: return m == 0; case SYS_GLO: return m == 1; case SYS_GAL: return m == 2; case SYS_CMP: return m == 3; case SYS_QZS: return m == 4; case SYS_IRN: return m == 5; } return 0; } /* double-differenced residuals and partial derivatives ----------------------------------- O rtk->ssat[i].resp[j] = residual pseudorange error O rtk->ssat[i].resc[j] = residual carrier phase error I rtk->rb= base location I nav = sat nav data I dt = time diff between base and rover observations (usually 0) I x = rover pos & vel and sat phase biases (float solution) I P = error covariance matrix of float states I sat = list of common sats I y = zero diff residuals (code and phase, base and rover) I e = line of sight unit vectors to sats I azel = [az, el] to sats I iu,ir = user and ref indices to sats I ns = # of sats O v = double diff innovations (measurement-model) (phase and code) O H = linearized translation from innovations to states (az/el to sats) O R = measurement error covariances O vflg = bit encoded list of sats used for each double diff 计算接收机或基站的双差相位/码残差以及量测阵 函数参数，16个： rtk_t *rtk IO rtk控制结构体 nav_t *nav I 导航数据 double dt I 接收机和基站的时间差 double *x IO 状态变量 double *P IO 状态变量的误差协方差阵 int sat I 接收机和基站共同观测的卫星号列表 double *y IO 相位/码残差 double *e IO 观测矢量 (ecef) double *azel O 方位角和俯仰角 (rad) int *iu I 接收机和基站共同观测的卫星在接收机观测值中的index值列表 int *ir I 接收机和基站共同观测的卫星在基站观测值中的index值列表 int ns I 接收机和基站共同观测的卫星个数 double *v O 实际观测量与预测观测量的残差 double *H O 观测矩阵 double *R O 测量误差的协方差 int *vflg O 数据有效标志 返回类型: int O (>0:ok,0:error) */ static int ddres(rtk_t *rtk, const nav_t *nav, const obsd_t *obs, double dt, const double *x, const double *P, const int *sat, double *y, double *e, double *azel, double *freq, const int *iu, const int *ir, int ns, double *v, double *H, double *R, int *vflg) { // 1、一些初始化和坐标转换 prcopt_t *opt = &rtk->opt; double bl, dr[3], posu[3], posr[3], didxi = 0.0, didxj = 0.0, *im, icb, threshadj; double *tropr, *tropu, *dtdxr, *dtdxu, *Ri, *Rj, freqi, freqj, *Hi = NULL, df; int i, j, k, m, f, nv = 0, nb[NFREQ * 4 * 2 + 2] = {0}, b = 0, sysi, sysj, nf = NF(opt); int ii, jj, frq, code; trace(3, "ddres : dt=%.1f nx=%d ns=%d\n", dt, rtk->nx, ns); /* bl=distance from base to rover, dr=x,y,z components 2、计算基线长度*/ bl = baseline(x, rtk->rb, dr); /* translate ecef pos to geodetic pos 3、基站和流动站位置转化 ecef→lat lon h*/ ecef2pos(x, posu); ecef2pos(rtk->rb, posr); // 4、变量内存申请和变量初始化 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); /* zero out residual phase and code biases for all satellites */ 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 - only used if kalman filter contains states for ION and TROP delays usually insignificant for short baselines (<10km)*/ for (i = 0; i < ns; i++) { // 5、若 电离层模式 >= IONOOPT_EST，调用 ionmapf 计算电离层延迟因子 if (opt->ionoopt >= IONOOPT_EST) { im[i] = (ionmapf(posu, azel + iu[i] * 2) + ionmapf(posr, azel + ir[i] * 2)) / 2.0; } // 6、若 对流层模式>=TROPOPT_EST，调用 prectrop 计算对流层延迟因子 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); } } /* 遍历不同系统和频点，计算双差残差，其中，若为差分伪距模式，需要限制遍历次数 注： 所有模式分为6类，每一类分别挑选参考卫星，再计算双差的结果，考虑到系统时差的影响*/ /*step through sat systems: m=0:gps/sbs,1:glo,2:gal,3:bds 4:qzs 5:irn 7、遍历系统：m = 0:gps / sbs, 1 : glo, 2 : gal, 3 : bds 4 : qzs 5 : irn*/ for (m = 0; m < 6; m++) { /* step through phases/codes 8、大循环，对每一个频率，循环9~21*/ //注：遍历次数设置： 载波相位在0-nf，nf至2nf为伪距，因此伪距差分定位从nf开始 for (f = opt->mode > PMODE_DGPS ? 0 : nf; f < nf * 2; f++) { frq = f % nf; code = f < nf ? 0 : 1; /* find reference satellite with highest elevation, set to i 9、选取仰角最高的参考卫星,若选取失败，则返回 其中，m代表不同的频点和系统，分为6类，每一类选取一次参考卫星*/ for (i = -1, j = 0; j < ns; j++) { sysi = rtk->ssat[sat[j] - 1].sys; if (!test_sys(sysi, m) || sysi == SYS_SBS) continue; if (!validobs(iu[j], ir[j], f, nf, y)) continue; if (rtk->ssat[sat[j] - 1].slip[frq] & LLI_SLIP) continue; if (i < 0 || azel[1 + iu[j] * 2] >= azel[1 + iu[i] * 2]) i = j; } if (i < 0) continue; /* calculate double differences of residuals (code/phase) for each sat 10、小循环，对每一种导航系统，遍历每一颗卫星，循环11~21步，计算双差*/ for (j = 0; j < ns; j++) { if (i == j) continue; /* skip ref sat */ // 11、初始检测频点和观测量是否有效 sysi = rtk->ssat[sat[i] - 1].sys; sysj = rtk->ssat[sat[j] - 1].sys; freqi = freq[frq + iu[i] * nf]; freqj = freq[frq + iu[j] * nf]; if (freqi <= 0.0 || freqj <= 0.0) continue; if (!test_sys(sysj, m)) continue; if (!validobs(iu[j], ir[j], f, nf, y)) continue; // 12、获取对应观测量的H阵的位置，并进行赋初值 if (H) { Hi = H + nv * rtk->nx; for (k = 0; k < rtk->nx; k++) Hi[k] = 0.0; } /* double-differenced measurements from 2 receivers and 2 sats in meters 13、双差残差：用传入的没有差分的相位/码残差y计算双差残差v，并计算对应的H*/ 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, combine unit vectors from two sats 更新H阵*/ if (H) { for (k = 0; k < 3; k++) { Hi[k] = -e[k + iu[i] * 3] + e[k + iu[j] * 3]; /* translation of innovation to position states */ } } if (opt->ionoopt == IONOOPT_EST) { /* adjust double-differenced measurements by double-differenced ionospheric delay term 14、若要估计电离层参数，模式IONOOPT_EST，用电离层延迟因子修正v和H*/ didxi = (code ? -1.0 : 1.0) * im[i] * SQR(FREQL1 / freqi); didxj = (code ? -1.0 : 1.0) * im[j] * SQR(FREQL1 / freqj); v[nv] -= didxi * x[II(sat[i], opt)] - didxj * x[II(sat[j], opt)]; if (H) { Hi[II(sat[i], opt)] = didxi; Hi[II(sat[j], opt)] = -didxj; } } if (opt->tropopt == TROPOPT_EST || opt->tropopt == TROPOPT_ESTG) { /* adjust double-differenced measurements by double-differenced tropospheric delay term 15、若要估计对流层参数，模式TROPOPT_EST，用对流层延迟因子修正v和H*/ 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(0, opt) + k] = (dtdxu[k + i * 3] - dtdxu[k + j * 3]); Hi[IT(1, opt) + k] = -(dtdxr[k + i * 3] - dtdxr[k + j * 3]); } } if (!code) { /* adjust phase residual by double-differenced phase-bias term, IB=look up index by sat&freq 16、用相位偏移修正v和H*/ if (opt->ionoopt != IONOOPT_IFLC) { /* phase-bias states are single-differenced so need to difference them*/ v[nv] -= CLIGHT / freqi * x[IB(sat[i], frq, opt)] - CLIGHT / freqj * x[IB(sat[j], frq, opt)]; if (H) { Hi[IB(sat[i], frq, opt)] = CLIGHT / freqi; Hi[IB(sat[j], frq, opt)] = -CLIGHT / freqj; //双差载波残差大的时候，有飞点存在的情况，针对飞点这个问题重置rtk配置 if (H == NULL && P != NULL) //限定为验后残差的ddres { if (fabs(v[nv]) > 1 && f < nf) { rtk->opt.exsats[sat[j] - 1] = 1; //排除参考星外的卫星i外的卫星j trace(3, "ddres:The satellite residuals are large and are rejected : dt=%.1f nx=%d ns=%d\n", dt, rtk->nx, ns); } } if (H == NULL && P == NULL) //限定为模糊度固定后的ddres { if (fabs(v[nv]) > 0.4 && f < nf) { initx(rtk, 0.0, 0.0, IB(sat[j], frq, opt)); trace(3, "ddres:After the ambuigity is fixed, the residual difference is large, and the ambuigity is reset : dt=%.1f nx=%d ns=%d\n", dt, rtk->nx, ns); } } } } else { v[nv] -= x[IB(sat[i], frq, opt)] - x[IB(sat[j], frq, opt)]; if (H) { Hi[IB(sat[i], frq, opt)] = 1.0; Hi[IB(sat[j], frq, opt)] = -1.0; } } } /* adjust double-difference for glonass sats 17、若是GLONASS系统观测值，做相关修正*/ if (sysi == SYS_GLO && sysj == SYS_GLO) { if (rtk->opt.glomodear == GLO_ARMODE_AUTOCAL && frq < NFREQGLO) { /* auto-cal method */ df = (freqi - freqj) / (f == 0 ? DFRQ1_GLO : DFRQ2_GLO); v[nv] -= df * x[IL(frq, opt)]; if (H) Hi[IL(frq, opt)] = df; } else if (rtk->opt.glomodear == GLO_ARMODE_FIXHOLD && frq < NFREQGLO) { /* fix-and-hold method */ icb = rtk->ssat[sat[i] - 1].icbias[frq] * CLIGHT / freqi - rtk->ssat[sat[j] - 1].icbias[frq] * CLIGHT / freqj; v[nv] -= icb; } } /* adjust double-difference for sbas sats */ if (sysj == SYS_SBS && sysi == SYS_GPS) { if (rtk->opt.glomodear == GLO_ARMODE_FIXHOLD && frq < NFREQ) { /* fix-and-hold method */ icb = rtk->ssat[sat[i] - 1].icbias[frq] * CLIGHT / freqi - rtk->ssat[sat[j] - 1].icbias[frq] * CLIGHT / freqj; v[nv] -= icb; } } /* save residuals 18、分别保存伪距和载波残差信息*/ if (code) rtk->ssat[sat[j] - 1].resp[frq] = v[nv]; /* pseudorange */ else rtk->ssat[sat[j] - 1].resc[frq] = v[nv]; /* carrier phase */ /* if residual too large, flag as outlier */ ii = IB(sat[i], frq, opt); jj = IB(sat[j], frq, opt); /* adjust threshold by error stdev ratio unless one of the phase biases was just initialized 通过误差标准差调整阈值*/ threshadj = code || (rtk->P[ii + rtk->nx * ii] == SQR(rtk->opt.std[0])) || (rtk->P[jj + rtk->nx * jj] == SQR(rtk->opt.std[0])) ? opt->eratio[frq] : 1; // 19、根据选项maxinno的值检测是否要排除此观测数据 if (opt->maxinno > 0.0 && fabs(v[nv]) > opt->maxinno * threshadj) { rtk->ssat[sat[j] - 1].vsat[frq] = 0; rtk->ssat[sat[j] - 1].rejc[frq]++; errmsg(rtk, "outlier rejected (sat=%3d-%3d %s%d v=%.3f)\n", sat[i], sat[j], code ? "P" : "L", frq + 1, v[nv]); continue; } /* single-differenced measurement error variances (m) 20、计算单差的测量误差协方差（单差观测量噪声）Ri、Rj*/ Ri[nv] = varerr(sat[i], sysi, azel[1 + iu[i] * 2], SNR_UNIT * rtk->ssat[sat[i] - 1].snr_rover[frq], SNR_UNIT * rtk->ssat[sat[i] - 1].snr_base[frq], bl, dt, f, opt, &obs[iu[i]]); Rj[nv] = varerr(sat[j], sysj, azel[1 + iu[j] * 2], SNR_UNIT * rtk->ssat[sat[j] - 1].snr_rover[frq], SNR_UNIT * rtk->ssat[sat[j] - 1].snr_base[frq], bl, dt, f, opt, &obs[iu[j]]); /* set valid data flags 21、设置数据有效标志*/ if (opt->mode > PMODE_DGPS) { if (!code) rtk->ssat[sat[i] - 1].vsat[frq] = rtk->ssat[sat[j] - 1].vsat[frq] = 1; } else { rtk->ssat[sat[i] - 1].vsat[frq] = rtk->ssat[sat[j] - 1].vsat[frq] = 1; } if (rtk->opt.glomodear == GLO_ARMODE_AUTOCAL) icb = x[IL(frq, opt)]; else icb = rtk->ssat[sat[i] - 1].icbias[frq] * CLIGHT / freqi - rtk->ssat[sat[j] - 1].icbias[frq] * CLIGHT / freqj; trace(3, "sat=%3d-%3d %s%d v=%13.3f R=%9.6f %9.6f icb=%9.3f lock=%5d x=%9.3f\n", sat[i], sat[j], code ? "P" : "L", frq + 1, v[nv], Ri[nv], Rj[nv], icb, rtk->ssat[sat[j] - 1].lock[frq], rtk->x[IB(sat[j], frq, &rtk->opt)]); vflg[nv++] = (sat[i] << 16) | (sat[j] << 8) | ((code ? 1 : 0) << 4) | (frq); nb[b]++; } b++; } } /* end of system loop */ /* baseline length constraint for moving baseline 22、若为移动基站模式PMODE_MOVEB，计算移动基站限制并设置相应的数据有效标志*/ if (opt->mode == PMODE_MOVEB && constbl(rtk, x, P, v, H, Ri, Rj, nv)) { vflg[nv++] = 3 << 4; nb[b++]++; } //打印H阵 if (H) { trace(5, "H=\n"); tracemat(5, H, rtk->nx, nv, 7, 4); } /* double-differenced measurement error covariance 23、用Ri、Rj计算双差的测量误差协方差（计算双差量测噪）R*/ ddcov(nb, b, Ri, Rj, nv, R); //释放局部变量内存，并返回有效的观测量数目nv free(Ri); free(Rj); free(im); free(tropu); free(tropr); free(dtdxu); free(dtdxr); return nv; } /* time-interpolation of residuals (for post-mission) 基站数据插值函数 ------------------------*/ static 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], rs[MAXOBS * 6], dts[MAXOBS * 2], var[MAXOBS]; static double e[MAXOBS * 3], azel[MAXOBS * 2], freq[MAXOBS * NFREQ]; static int nb = 0, svh[MAXOBS * 2]; prcopt_t *opt = &rtk->opt; double tt = timediff(time, obs[0].time), ttb, *p, *q; int i, j, k, nf = NF(opt); trace(3, "intpres : n=%d tt=%.1f\n", n, tt); // 1检查前一历元基站卫星数目以及当前时间差是否小于门限，返回并记录当前历元信息 if (nb == 0 || fabs(tt) < DTTOL) { nb = n; for (i = 0; i < n; i++) obsb[i] = obs[i]; return tt; } // 2、检查与前一历元的时间差：大于2倍门限，返回 ttb = timediff(time, obsb[0].time); if (fabs(ttb) > opt->maxtdiff * 2.0 || ttb == tt) return tt; // 3、计算前一历元基站观测量下的卫星位置和钟差信息： satposs(time, obsb, nb, nav, opt->sateph, rs, dts, var, svh); // 4、计算前一历元基站观测量非差残差信息 if (!zdres(1, obsb, nb, rs, dts, var, svh, nav, rtk->rb, opt, 1, yb, e, azel, freq)) { return tt; } // 5、残差通过时间推算？ 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 || (obs[i].LLI[k % nf] & LLI_SLIP) || (obsb[j].LLI[k % nf] & LLI_SLIP)) *p = 0.0; else *p = (ttb * (*p) - tt * (*q)) / (ttb - tt); } } return fabs(ttb) > fabs(tt) ? ttb : tt; } /* index for single to double-difference transformation matrix (D') 创建将卡尔曼状态量从单差转到双差的转换矩阵D--------------------*/ static int ddidx(rtk_t *rtk, int *ix, int gps, int glo, int sbs) { int i, j, k, m, f, n, nb = 0, na = rtk->na, nf = NF(&rtk->opt), nofix; double fix[MAXSAT], ref[MAXSAT]; trace(3, "ddmat: gps=%d/%d glo=%d/%d sbs=%d\n", gps, rtk->opt.gpsmodear, glo, rtk->opt.glomodear, sbs); /* clear fix flag for all sats (1=float, 2=fix) 首先将所有卫星的fix标志都清空置0：rtk->ssat[i].fix[j]=0*/ for (i = 0; i < MAXSAT; i++) for (j = 0; j < NFREQ; j++) { rtk->ssat[i].fix[j] = 0; } /*对所有卫星系统进行循环，对所有频率进行循环，对所有卫星进行循环，如果该卫星没有被激活，则跳过。 如果激活了，则寻找第一颗满足要求的卫星作为参考星，找到了就break，跳出所有卫星这个循环。*/ for (m = 0; m < 6; m++) { /* m=0:GPS/SBS,1:GLO,2:GAL,3:BDS,4:QZS,5:IRN */ /* skip if ambiguity resolution turned off for this sys */ nofix = (m == 0 && gps == 0) || (m == 1 && glo == 0) || (m == 3 && rtk->opt.bdsmodear == 0); /* step through freqs */ for (f = 0, k = na; f < nf; f++, k += MAXSAT) { /* look for first valid sat (i=state index, i-k=sat index) */ for (i = k; i < k + MAXSAT; i++) { /* skip if sat not active */ if (rtk->x[i] == 0.0 || !test_sys(rtk->ssat[i - k].sys, m) || !rtk->ssat[i - k].vsat[f]) { continue; } /* set sat to use for fixing ambiguity if meets criteria */ 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; /* break out of loop if find good sat */ } /* else don't use this sat for fixing ambiguity */ else rtk->ssat[i - k].fix[f] = 1; } if (rtk->ssat[i - k].fix[f] != 2) continue; /* no good sat found */ /* step through all sats (j=state index, j-k=sat index, i-k=first good sat) 重新对所有卫星进行循环，检查每颗卫星是否符合要求，并根据该符合要求卫星和参考卫星的索引，对D矩阵进行赋值*/ for (n = 0, 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 (sbs == 0 && satsys(j - k + 1, NULL) == SYS_SBS) continue; if (rtk->ssat[j - k].lock[f] >= 0 && !(rtk->ssat[j - k].slip[f] & 2) && rtk->ssat[j - k].vsat[f] && rtk->ssat[j - k].azel[1] >= rtk->opt.elmaskar && !nofix) { /* set D coeffs to subtract sat j from sat i */ ix[nb * 2] = i; /* state index of ref bias */ ix[nb * 2 + 1] = j; /* state index of target bias */ /* inc # of sats used for fix */ ref[nb] = i - k + 1; fix[nb++] = j - k + 1; rtk->ssat[j - k].fix[f] = 2; /* fix */ n++; /* count # of sat pairs for this freq/constellation */ } /* else don't use this sat for fixing ambiguity */ else rtk->ssat[j - k].fix[f] = 1; } /* don't use ref sat if no sat pairs */ if (n == 0) rtk->ssat[i - k].fix[f] = 1; } } if (nb > 0) { trace(3, "refSats="); tracemat(3, ref, 1, nb, 7, 0); trace(3, "fixSats="); tracemat(3, fix, 1, nb, 7, 0); } return nb; } /* translate double diff fixed phase-bias values to single diff fix phase-bias values 根据lambda算法计算得到的双差整周模糊度，计算单差相位偏差 IO rtk_t *rtk: rtk solution structure I const double *bias 利用lambda算法计算得到的双差整周模糊度 I int nb 双差整周模糊度的个数 I double *xa fixed states*/ static void restamb(rtk_t *rtk, const double *bias, int nb, double *xa) { int i, n, m, f, index[MAXSAT], nv = 0, nf = NF(&rtk->opt); trace(3, "restamb :\n"); /*首先用将xa状态量的前na个元素赋值为rtk->xa[i]:for (i=0;ina;i++) xa[i]=rtk->xa[i]; 而其余元素（相位偏差状态量）则赋值为卡尔曼滤波浮点解：xa[i]=rtk->x [i]; 这里需要理解这几个变量的含义，具体看resamb_LAMBDA函数注意事项中的解释。*/ for (i = 0; i < rtk->nx; i++) xa[i] = rtk->x[i]; /* init all fixed states to float state values */ for (i = 0; i < rtk->na; i++) xa[i] = rtk->xa[i]; /* overwrite non phase-bias states with fixed values */ /*对所有卫星系统进行循环，对所有频率进行循环，对所有卫星进行循环，找到该卫星相位偏移在卡尔曼滤波状态量中的索引，放到index数组中。*/ for (m = 0; m < 5; 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(i + 1, f, &rtk->opt); } if (n < 2) continue; /*由于参考卫星是第一个符合要求的卫星，因此index[0]即为参考星的单差相位偏移， 由此根据lambda算法计算得到的双差整周模糊度，计算其他卫星的单差相位偏移。*/ xa[index[0]] = rtk->x[index[0]]; for (i = 1; i < n; i++) { xa[index[i]] = xa[index[0]] - bias[nv++]; } } } /* hold integer ambiguity ----------------------------------------------------*/ static void holdamb(rtk_t *rtk, const double *xa) { double *v, *H, *R; int i, j, n, m, f, info, index[MAXSAT], nb = rtk->nx - rtk->na, nv = 0, nf = NF(&rtk->opt); double dd, sum; trace(3, "holdamb :\n"); v = mat(nb, 1); H = zeros(nb, rtk->nx); for (m = 0; m < 5; 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(i + 1, f, &rtk->opt); rtk->ssat[i].fix[f] = 3; /* hold */ } /* use ambiguity resolution results to generate a set of pseudo-innovations to feed to kalman filter based on error between fixed and float solutions */ for (i = 1; i < n; i++) { /* phase-biases are single diff, so subtract errors to get double diff: v(nv)=err(i)-err(0) */ 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++; } } /* return if less than min sats for hold (skip if fix&hold for GLONASS only) */ if (rtk->opt.modear == ARMODE_FIXHOLD && nv < rtk->opt.minholdsats) { trace(3, "holdamb: not enough sats to hold ambiguity\n"); free(v); free(H); return; } rtk->holdamb = 1; /* set flag to indicate hold has occurred */ R = zeros(nv, nv); for (i = 0; i < nv; i++) R[i + i * nv] = rtk->opt.varholdamb; /* 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); /* skip glonass/sbs icbias update if not enabled */ if (rtk->opt.glomodear != GLO_ARMODE_FIXHOLD) return; /* Move fractional part of bias from phase-bias into ic bias for GLONASS sats (both in cycles) */ for (f = 0; f < nf; f++) { i = -1; sum = 0; for (j = nv = 0; j < MAXSAT; j++) { /* check if valid GLONASS sat */ if (test_sys(rtk->ssat[j].sys, 1) && rtk->ssat[j].vsat[f] && rtk->ssat[j].lock[f] >= 0) { if (i < 0) { i = j; /* use first valid sat for reference sat */ index[nv++] = j; } else { /* adjust the rest */ /* find phase-bias difference */ dd = rtk->x[IB(j + 1, f, &rtk->opt)] - rtk->x[IB(i + 1, f, &rtk->opt)]; dd = rtk->opt.gainholdamb * (dd - ROUND(dd)); /* throwout integer part of answer and multiply by filter gain */ rtk->x[IB(j + 1, f, &rtk->opt)] -= dd; /* remove fractional part from phase bias */ rtk->ssat[j].icbias[f] += dd; /* and move to IC bias */ sum += dd; index[nv++] = j; } } } } /* Move fractional part of bias from phase-bias into ic bias for SBAS sats (both in cycles) */ for (f = 0; f < nf; f++) { i = -1; sum = 0; for (j = nv = 0; j < MAXSAT; j++) { /* check if valid GPS/SBS sat */ if (test_sys(rtk->ssat[j].sys, 0) && rtk->ssat[j].vsat[f] && rtk->ssat[j].lock[f] >= 0) { if (i < 0) { i = j; /* use first valid GPS sat for reference sat */ index[nv++] = j; } else { /* adjust the SBS sats */ if (rtk->ssat[j].sys != SYS_SBS) continue; /* find phase-bias difference */ dd = rtk->x[IB(j + 1, f, &rtk->opt)] - rtk->x[IB(i + 1, f, &rtk->opt)]; dd = rtk->opt.gainholdamb * (dd - ROUND(dd)); /* throwout integer part of answer and multiply by filter gain */ rtk->x[IB(j + 1, f, &rtk->opt)] -= dd; /* remove fractional part from phase bias diff */ rtk->ssat[j].icbias[f] += dd; /* and move to IC bias */ sum += dd; index[nv++] = j; } } } } } /* resolve integer ambiguity by LAMBDA 通过LAMBDA算法求解整周模糊度 参数： IO rtk_t *rtk: rtk solution structure I double *bias 利用lambda算法计算得到的双差整周模糊度 IO double *xa fixed states（在注意事项中进行了更为详细的解释） 注意事项：这个函数中的变量比较多，容易混乱。rtk->x是卡尔曼滤波的状态量， rtk->xa是利用lambda算法得到的整数解修正后的、与相位偏差无关的状态量（例如：位置+速度+加速度+电离层+对流层……） rtk->P和rtk->Pa同理。xa这个变量（注意不是rtk->xa，这是两个不同的变量）则包含了两部分， 一部分是rtk->xa，另一部分则是利用lambda算法得到的双差整周模糊度计算得到的单差相位偏移。 nx=rtk->nx是指卡尔曼滤波状态量的个数，na=rtk->na是除了单差相位偏差B(手册E7.7)之外的所有状态量个数， 所以通常相位偏差状态量的索引是从na开始；nb则是双差整周模糊度的个数。 ---------------------------------------*/ static int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa, int gps, int glo, int sbs) { prcopt_t *opt = &rtk->opt; int i, j, nb, nb1, info, nx = rtk->nx, na = rtk->na; double *DP, *y, *b, *db, *Qb, *Qab, *QQ, s[2]; int *ix; double var = 0, coeff[3]; double QQb[MAXSAT]; trace(3, "resamb_LAMBDA : nx=%d\n", nx); /*1、整周模糊度ratio赋初值，并检测是否进行模糊度解算（定位模式，模糊度解算配置项以及模糊度验证门限值）*/ rtk->sol.ratio = 0.0; rtk->nb_ar = 0; /*检查配置中所设置的AR阈值（LAMBDA算法最优解和次优解的比值，通常取3.0），如果该阈值小于1，则返回0。*/ if (rtk->opt.mode <= PMODE_DGPS || rtk->opt.modear == ARMODE_OFF || rtk->opt.thresar[0] < 1.0) { return 0; } /* skip AR if position variance too high to avoid false fix 2、检查卡尔曼滤波中位置状态量的协方差阵，如果位置方差超过所设定的方差阈值，则返回0，避免由于方差过大造成false fix*/ for (i = 0; i < 3; i++) var += rtk->P[i + i * rtk->nx]; var = var / 3.0; /* maintain compatibility with previous code */ trace(3, "posvar=%.6f\n", var); if (var > rtk->opt.thresar[1]) { errmsg(rtk, "position variance too large: %.4f\n", var); return 0; } /* Create index of single to double-difference transformation matrix (D') used to translate phase biases to double difference 调用ddidx函数，创建将卡尔曼状态量从单差转到双差的转换矩阵D’。 主要是将单差相位偏移状态量转换为双差相位偏移，这里的D’阵实际就是manual 165页中的G阵*/ ix = imat(nx, 2); if ((nb = ddidx(rtk, ix, gps, glo, sbs)) < (rtk->opt.minfixsats - 1)) { /* nb is sat pairs */ errmsg(rtk, "not enough valid double-differences\n"); free(ix); return -1; /* flag abort */ } /*定义几个变量和矩阵，其中nx = na + nb，na实际就是之前卡尔曼滤波中除了单差相位偏移之外的所有状态量个数 （例如：位置 + 速度 + 加速度 + 电离层 + 对流层……）， nb则是双差相位偏移的个数（即需要解算的整周模糊度个数） */ rtk->nb_ar = nb; /* nx=# of float states, na=# of fixed states, nb=# of double-diff phase biases */ y = mat(nb, 1); DP = mat(nb, nx - na); b = mat(nb, 2); db = mat(nb, 1); Qb = mat(nb, nb); Qab = mat(na, nb); QQ = mat(na, nb); /* phase-bias covariance (Qb) and real-parameters to bias covariance (Qab) */ /* y=D*xc, Qb=D*Qc*D', Qab=Qac*D' 计算公式（E.7.16）中的Q_N和Q_NR矩阵*/ for (i = 0; i < nb; i++) { y[i] = rtk->x[ix[i * 2]] - rtk->x[ix[i * 2 + 1]]; } for (j = 0; j < nx - na; j++) for (i = 0; i < nb; i++) { DP[i + j * nb] = rtk->P[ix[i * 2] + (na + j) * nx] - rtk->P[ix[i * 2 + 1] + (na + j) * nx]; } for (j = 0; j < nb; j++) for (i = 0; i < nb; i++) { Qb[i + j * nb] = DP[i + (ix[j * 2] - na) * nb] - DP[i + (ix[j * 2 + 1] - na) * nb]; } for (j = 0; j < nb; j++) for (i = 0; i < na; i++) { Qab[i + j * na] = rtk->P[i + ix[j * 2] * nx] - rtk->P[i + ix[j * 2 + 1] * nx]; } for (i = 0; i < nb; i++) QQb[i] = 1000 * Qb[i + i * nb]; trace(3, "N(0)= "); tracemat(3, y, 1, nb, 7, 2); trace(3, "Qb*1000= "); tracemat(3, QQb, 1, nb, 7, 4); /* lambda/mlambda integer least-square estimation */ /* return best integer solutions */ /* b are best integer solutions, s are residuals */ /*调用lambda函数计算双差整周模糊度最优解以及残差*/ #ifdef PAR if (!(info = parlambda(rtk, nb, 2, y, Qb, b, s))) { #else if (!(info = lambda(rtk, nb, 2, y, Qb, b, s))) { #endif trace(3, "N(1)= "); tracemat(3, b, 1, nb, 7, 2); trace(3, "N(2)= "); tracemat(3, b + nb, 1, nb, 7, 2); rtk->sol.ratio = s[0] > 0 ? (float)(s[1] / s[0]) : 0.0f; if (rtk->sol.ratio > 999.9) rtk->sol.ratio = 999.9f; /* adjust AR ratio based on # of sats, unless minAR==maxAR */ if (opt->thresar[5] != opt->thresar[6]) { nb1 = nb < 50 ? nb : 50; /* poly only fitted for upto 50 sat pairs */ /* generate poly coeffs based on nominal AR ratio */ for ((i = 0); i < 3; i++) { coeff[i] = ar_poly_coeffs[i][0]; for ((j = 1); j < 5; j++) coeff[i] = coeff[i] * opt->thresar[0] + ar_poly_coeffs[i][j]; } /* generate adjusted AR ratio based on # of sat pairs */ rtk->sol.thres = coeff[0]; for (i = 1; i < 3; i++) { rtk->sol.thres = rtk->sol.thres * 1 / (nb1 + 1) + coeff[i]; } rtk->sol.thres = MIN(MAX(rtk->sol.thres, opt->thresar[5]), opt->thresar[6]); } else rtk->sol.thres = (float)opt->thresar[0]; /* validation by popular ratio-test of residuals 如果最优解和次优解的比值大于阈值，则利用公式（E.7.19），根据lambda算法得到的双差整周模糊度b， 计算除了单差相位偏移之外的所有状态量（例如：位置+速度+加速度+电离层+对流层……），存入rtk->xa中。 这一步实际就是利用lambda算法得到的整数的整周模糊度对其他状态量进行修正。*/ if (s[0] <= 0.0 || s[1] / s[0] >= rtk->sol.thres) { /* init non phase-bias states and covariances with float solution values */ /* 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]; } /* y = differences between float and fixed dd phase-biases bias = fixed dd phase-biases */ for (i = 0; i < nb; i++) { bias[i] = b[i]; y[i] -= b[i]; } /* adjust non phase-bias states and covariances using fixed solution values */ if (!matinv(Qb, nb)) { /* returns 0 if inverse successful */ /* rtk->xa = rtk->x-Qab*Qb^-1*(b0-b) */ matmul("NN", nb, 1, nb, 1.0, Qb, y, 0.0, db); /* db = Qb^-1*(b0-b) */ matmul("NN", na, 1, nb, -1.0, Qab, db, 1.0, rtk->xa); /* rtk->xa = rtk->x-Qab*db */ /* rtk->Pa=rtk->P-Qab*Qb^-1*Qab') */ /* covariance of fixed solution (Qa=Qa-Qab*Qb^-1*Qab') */ matmul("NN", na, nb, nb, 1.0, Qab, Qb, 0.0, QQ); /* QQ = Qab*Qb^-1 */ matmul("NT", na, na, nb, -1.0, QQ, Qab, 1.0, rtk->Pa); /* rtk->Pa = rtk->P-QQ*Qab' */ trace(3, "resamb : validation ok (nb=%d ratio=%.2f thresh=%.2f s=%.2f/%.2f)\n", nb, s[0] == 0.0 ? 0.0 : s[1] / s[0], rtk->sol.thres, s[0], s[1]); /* translate double diff fixed phase-bias values to single diff fix phase-bias values, result in xa 调用restamb函数，利用lambda算法计算得到的双差整周模糊度，重新计算单差相位偏移，并存入xa中， 同时将上一注释中中得到的其他状态量也存入xa中。*/ restamb(rtk, bias, nb, xa); } else nb = 0; } else { /* validation failed */ errmsg(rtk, "ambiguity validation failed (nb=%d ratio=%.2f thresh=%.2f s=%.2f/%.2f)\n", nb, s[1] / s[0], rtk->sol.thres, s[0], s[1]); nb = 0; } } else { errmsg(rtk, "lambda error (info=%d)\n", info); nb = 0; } free(ix); free(y); free(DP); free(b); free(db); free(Qb); free(Qab); free(QQ); return nb; /* number of ambiguities */ } /* resolve integer ambiguity by LAMBDA using partial fix techniques and multiple attempts 通过LAMBDA使用部分修复技术和多次尝试来求解整周模糊度-----------------------*/ static int manage_amb_LAMBDA(rtk_t *rtk, double *bias, double *xa, const int *sat, int nf, int ns) { int i, f, lockc[NFREQ], ar = 0, excflag = 0, arsats[MAXOBS] = {0}; int gps1 = -1, glo1 = -1, sbas1 = -1, gps2, glo2, sbas2, nb, rerun, dly; float ratio1; trace(3, "prevRatios= %.3f %.3f\n", rtk->sol.prev_ratio1, rtk->sol.prev_ratio2); /* if no fix on previous sample and enough sats, exclude next sat in list */ trace(3, "num ambiguities used last AR: %d\n", rtk->nb_ar); if (rtk->sol.prev_ratio2 < rtk->sol.thres && rtk->nb_ar >= rtk->opt.mindropsats) { /* find and count sats used last time for AR */ for (f = 0; f < nf; f++) for (i = 0; i < ns; i++) if (rtk->ssat[sat[i] - 1].vsat[f] && rtk->ssat[sat[i] - 1].lock[f] >= 0 && rtk->ssat[sat[i] - 1].azel[1] >= rtk->opt.elmin) { arsats[ar++] = i; } if (rtk->excsat < ar) { i = sat[arsats[rtk->excsat]]; for (f = 0; f < nf; f++) { lockc[f] = rtk->ssat[i - 1].lock[f]; /* save lock count */ /* remove sat from AR long enough to enable hold if stays fixed */ rtk->ssat[i - 1].lock[f] = -rtk->nb_ar; } trace(3, "AR: exclude sat %d\n", i); excflag = 1; } else rtk->excsat = 0; /* exclude none and reset to beginning of list */ } /* for inital ambiguity resolution attempt, include all enabled sats */ gps1 = 1; /* always enable gps for initial pass */ glo1 = (rtk->opt.navsys & SYS_GLO) ? (((rtk->opt.glomodear == GLO_ARMODE_FIXHOLD) && !rtk->holdamb) ? 0 : 1) : 0; sbas1 = (rtk->opt.navsys & SYS_GLO) ? glo1 : ((rtk->opt.navsys & SYS_SBS) ? 1 : 0); /* first attempt to resolve ambiguities */ nb = resamb_LAMBDA(rtk, bias, xa, gps1, glo1, sbas1); ratio1 = rtk->sol.ratio; /* reject bad satellites if AR filtering enabled */ if (rtk->opt.arfilter) { rerun = 0; /* if results are much poorer than previous epoch or dropped below ar ratio thresh, remove new sats */ if (nb >= 0 && rtk->sol.prev_ratio2 >= rtk->sol.thres && ((rtk->sol.ratio < rtk->sol.thres) || (rtk->sol.ratio < rtk->opt.thresar[0] * 1.1 && rtk->sol.ratio < rtk->sol.prev_ratio1 / 2.0))) { trace(3, "low ratio: check for new sat\n"); dly = 2; for (i = 0; i < ns; i++) for (f = 0; f < nf; f++) { if (rtk->ssat[sat[i] - 1].fix[f] != 2) continue; /* check for new sats */ if (rtk->ssat[sat[i] - 1].lock[f] == 0) { trace(3, "remove sat %d:%d lock=%d\n", sat[i], f, rtk->ssat[sat[i] - 1].lock[f]); rtk->ssat[sat[i] - 1].lock[f] = -rtk->opt.minlock - dly; /* delay use of this sat with stagger */ dly += 2; /* stagger next try of new sats */ rerun = 1; } } } /* rerun if filter removed any sats */ if (rerun) { trace(3, "rerun AR with new sat removed\n"); /* try again with new sats removed */ nb = resamb_LAMBDA(rtk, bias, xa, gps1, glo1, sbas1); } } rtk->sol.prev_ratio1 = ratio1; /* if fix-and-hold gloarmode enabled, re-run AR with final gps/glo settings if differ from above */ if ((rtk->opt.navsys & SYS_GLO) && rtk->opt.glomodear == GLO_ARMODE_FIXHOLD && rtk->sol.ratio < rtk->sol.thres) { glo2 = sbas2 = 0; /* turn off gpsmode if not enabled and got good fix (used for debug and eval only) */ gps2 = rtk->opt.gpsmodear == 0 && rtk->sol.ratio >= rtk->sol.thres ? 0 : 1; /* if modes changed since initial AR run or haven't run yet,re-run with new modes */ if (glo1 != glo2 || gps1 != gps2) nb = resamb_LAMBDA(rtk, bias, xa, gps2, glo2, sbas2); } /* restore excluded sat if still no fix or significant increase in ar ratio */ if (excflag && (rtk->sol.ratio < rtk->sol.thres) && (rtk->sol.ratio < (1.5 * rtk->sol.prev_ratio2))) { i = sat[arsats[rtk->excsat++]]; for (f = 0; f < nf; f++) rtk->ssat[i - 1].lock[f] = lockc[f]; trace(3, "AR: restore sat %d\n", i); } rtk->sol.prev_ratio1 = ratio1 > 0 ? ratio1 : rtk->sol.ratio; rtk->sol.prev_ratio2 = rtk->sol.ratio; return nb; } /* validation of solution ----------------------------------------------------*/ static int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg, int nv, double thres) { double fact = thres * thres; int i, stat = 1, sat1, sat2, type, 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 ? "P" : "C"); errmsg(rtk, "large residual (sat=%2d-%2d %s%d v=%6.3f sig=%.3f)\n", sat1, sat2, stype, freq + 1, v[i], SQRT(R[i + i * nv])); } return stat; } /* relpos()relative positioning ------------------------------------------------------ * args: rtk IO gps solution structure obs I satellite observations nu I # of user observations (rover) nr I # of ref observations (base) nav I satellite navigation data 相对定位 函数参数，5个： rtk_t *rtk IO rtk控制结构体 obsd_t *obs I 观测数据 int nu I 接收机观测数据的数量 int nr I 基站观测数据的数量 nav_t *nav I 导航数据 返回类型: int O (1:ok,0:error) */ static 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, *dts, *var, *y, *e, *azel, *freq, *v, *H, *R, *xp, *Pp, *xa, *bias, dt; int i, j, f, n = nu + nr, ns, ny, nv, sat[MAXSAT], iu[MAXSAT], ir[MAXSAT], niter; int info, vflg[MAXOBS * NFREQ * 2 + 1], 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); /* time diff between base and rover observations (usually zero) 计算流动站和参考站时间差*/ dt = timediff(time, obs[nu].time); /* define local matrices, n=total observations, base + rover */ rs = mat(6, n); /* range to satellites 卫星位置和速度，长度为6*n，{x,y,z,vx,vy,vz}(ecef)(m,m/s)*/ dts = mat(2, n); /* satellite clock biases 卫星钟差，长度为2*n， {bias,drift} (s|s/s)*/ var = mat(1, n); y = mat(nf * 2, n); /*残差*/ e = mat(3, n); azel = zeros(2, n); /* [az, el] */ freq = zeros(nf, n); /* init satellite status arrays 一系列值、状态、中间变量的初始化*/ for (i = 0; i < MAXSAT; i++) { rtk->ssat[i].sys = satsys(i + 1, NULL); /* gps system */ for (j = 0; j < NFREQ; j++) { rtk->ssat[i].vsat[j] = 0; /* valid satellite */ rtk->ssat[i].snr_rover[j] = 0; rtk->ssat[i].snr_base[j] = 0; } } /* compute satellite positions, velocities and clocks 调用 satposs 计算卫星们的位置、速度和钟差。*/ /*返回值rs是卫星的位置速度数组，rs = mat(6, n)，（X1 Y1 Z1 Vx1 Vy1 Vz1 ....） 返回值dts是卫星的钟差钟漂，dts=mat(2,n),(dt1 dt1˙......)*/ satposs(time, obs, n, nav, opt->sateph, rs, dts, var, svh); /* calculate [range - measured pseudorange] for base station (phase and code) output is in y[nu:nu+nr], see call for rover below for more details */ trace(3, "processing for the base station:\n"); /*参考站非差残差---调用 zdres 计算参考站的没有差分的相位/码残差，若出错则返回0*/ /*rtklib中浮点解是用ekf算法计算的，ekf的量测更新要有量测向量和线性化的量测矩阵， 从无差观测值开始，量测向量的计算开始了。*/ if (!zdres(1, obs + nu, nr, rs + nu * 6, dts + nu * 2, var + nu, svh + nu, nav, rtk->rb, opt, 1, y + nu * nf * 2, e + nu * 3, azel + nu * 2, freq + nu * nf)) { errmsg(rtk, "initial base station position error\n"); free(rs); free(dts); free(var); free(y); free(e); free(azel); free(freq); return 0; } /* time-interpolation of residuals (for post-processing) - defaults to off 若为后处理，需要插值的，调用 intpref 进行插值。基站信息插值，通过配置项进行设置*/ if (opt->intpref) { dt = intpres(time, obs + nu, nr, nav, rtk, y + nu * nf * 2); } /* select common satellites between rover and base-station 调用 selsat 选择接收机与基站共同观测的卫星，返回共同观测的卫星个数， 输出卫星号列表sat、在接收机观测值中的index值列表 iu 和在基站观测值中的index值列表 ir*/ if ((ns = selsat(obs, azel, nu, nr, opt, sat, iu, ir)) <= 0) { errmsg(rtk, "no common satellite\n"); free(rs); free(dts); free(var); free(y); free(e); free(azel); free(freq); return 0; } /* update kalman filter states (pos,vel,acc,ionosp, troposp, sat phase biases) 调用 udstate 更新状态值 rtk->x 及其误差协方差 rtk->P*/ udstate(rtk, obs, sat, iu, ir, ns, nav); trace(4, "x(0)="); tracemat(4, rtk->x, 1, NR(opt), 13, 4); for (i = 0; i < ns; i++) for (j = 0; j < nf; j++) { /* snr of base and rover receiver */ rtk->ssat[sat[i] - 1].snr_rover[j] = obs[iu[i]].SNR[j]; rtk->ssat[sat[i] - 1].snr_base[j] = obs[ir[i]].SNR[j]; } /* initialize Pp,xa to zero, xp to rtk->x */ 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 (else default niter=1) 设置迭代次数（设置值为1， 动基线，增加2次迭代次数）*/ niter = opt->niter + (opt->mode == PMODE_MOVEB && opt->baseline[0] > 0.0 ? 2 : 0); //按迭代次数循环以下3步 for (i = 0; i < niter; i++) { /* calculate zero diff residuals [range - measured pseudorange] for rover (phase and code) output is in y[0:nu-1], only shared input with base is nav obs = sat observations nu = # of sats rs = range to sats dts = sat clock biases (rover) svh = sat health flags nav = sat nav data xp = kalman states opt = options y = zero diff residuals (code and phase) e = line of sight unit vectors to sats azel = [az, el] to sats */ trace(3, "processing for the rover station:\n"); //调用 zdres 计算接收机的没有差分的相位/码残差 if (!zdres(0, obs, nu, rs, dts, var, svh, nav, xp, opt, 0, y, e, azel, freq)) { errmsg(rtk, "rover initial position error\n"); stat = SOLQ_NONE; break; } /* calculate double-differenced residuals and create state matrix from sat angles O rtk->ssat[i].resp[j] = residual pseudorange error O rtk->ssat[i].resc[j] = residual carrier phase error I dt = time diff between base and rover observations (usually 0) I Pp = covariance matrix of float solution I sat = list of common sats I iu,ir = user and ref indices to sats I ns = # of sats O v = double diff residuals (phase and code) O H = partial derivatives O R = double diff measurement error covariances O vflg = list of sats used for dd */ //调用 ddres 计算双差相位/码残差 if ((nv = ddres(rtk, nav, obs, dt, xp, Pp, sat, y, e, azel, freq, iu, ir, ns, v, H, R, vflg)) < 1) { errmsg(rtk, "no double-differenced residual\n"); stat = SOLQ_NONE; break; } /* kalman filter measurement update, updates x,y,z,sat phase biases, etc K=P*H*(H'*P*H+R)^-1 xp=x+K*v Pp=(I-K*H')*P */ //调用 filter 进行 KF量测更新实现 --计算浮点解 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(opt), 13, 4); } /*再次调用 zdres 和 ddres 计算双差相位/码残差， 调用 valpos 进行验证，若通过则更新 rtk->x 以及 rtk->P，并更新模糊度控制结构体。*/ /* calc zero diff residuals again after kalman filter update 量测更新完成，检测是否完成，并利用更新以后的结果计算残差*/ if (stat != SOLQ_NONE && zdres(0, obs, nu, rs, dts, var, svh, nav, xp, opt, 0, y, e, azel, freq)) { /* calc double diff residuals again after kalman filter update for float solution 利用浮点结果计算双差残差以及量测噪声*/ nv = ddres(rtk, nav, obs, dt, xp, Pp, sat, y, e, azel, freq, iu, ir, ns, v, NULL, R, vflg); /* validation of float solution, always returns 1, msg to trace file if large residual 通过方差和量测进行校验，检测结果是否有效*/ if (valpos(rtk, v, R, vflg, nv, 4.0)) { /* update state and covariance matrix from kalman filter update 存储浮点结果*/ matcpy(rtk->x, xp, rtk->nx, 1); matcpy(rtk->P, Pp, rtk->nx, rtk->nx); /* update valid satellite status for ambiguity control 存储模糊度相关的信息，统计有效卫星数目*/ 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].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; stat = SOLQ_DGPS; } else stat = SOLQ_NONE; } /* resolve integer ambiguity by LAMBDA lAMBDA固定整周模糊度*/ if (stat != SOLQ_NONE) { /* if valid fixed solution, process it */ if (manage_amb_LAMBDA(rtk, bias, xa, sat, nf, ns) > 1) { //模糊度解算成功，根据固定结果计算双差残差和协方差，并进行校验 /* find zero-diff residuals for fixed solution */ if (zdres(0, obs, nu, rs, dts, var, svh, nav, xa, opt, 0, y, e, azel, freq)) { /* post-fit residuals for fixed solution (xa includes fixed phase biases, rtk->xa does not) */ nv = ddres(rtk, nav, obs, dt, xa, NULL, sat, y, e, azel, freq, iu, ir, ns, v, NULL, R, vflg); /* validation of fixed solution, always returns valid */ if (valpos(rtk, v, R, vflg, nv, 4.0)) { /* hold integer ambiguity if meet minfix count 固定解验证有效，若配置为hold模式，需要存储模糊度信息*/ if (++rtk->nfix >= rtk->opt.minfix) { if (rtk->opt.modear == ARMODE_FIXHOLD || rtk->opt.glomodear == GLO_ARMODE_FIXHOLD) holdamb(rtk, xa); /* switch to kinematic after qualify for hold if in static-start mode */ if (rtk->opt.mode == PMODE_STATIC_START) { rtk->opt.mode = PMODE_KINEMA; trace(3, "Fix and hold complete: switch to kinematic mode\n"); } } stat = SOLQ_FIX; } } } } /* save solution status (fixed or float) 保存solution状态 位置和速度以及方差信息（若状态为固定解，存储固定解结果）*/ if (stat == SOLQ_FIX) { for (i = 0; i < 3; i++) { rtk->sol.rr[i] = rtk->xa[i]; rtk->sol.qr[i] = (float)rtk->Pa[i + i * rtk->na]; } rtk->sol.qr[3] = (float)rtk->Pa[1]; rtk->sol.qr[4] = (float)rtk->Pa[1 + 2 * rtk->na]; rtk->sol.qr[5] = (float)rtk->Pa[2]; if (rtk->opt.dynamics) { /* velocity and covariance */ for (i = 3; i < 6; i++) { rtk->sol.rr[i] = rtk->xa[i]; rtk->sol.qv[i - 3] = (float)rtk->Pa[i + i * rtk->na]; } rtk->sol.qv[3] = (float)rtk->Pa[4 + 3 * rtk->na]; rtk->sol.qv[4] = (float)rtk->Pa[5 + 4 * rtk->na]; rtk->sol.qv[5] = (float)rtk->Pa[5 + 3 * rtk->na]; } } else { /* float solution */ for (i = 0; i < 3; i++) { rtk->sol.rr[i] = rtk->x[i]; rtk->sol.qr[i] = (float)rtk->P[i + i * rtk->nx]; } rtk->sol.qr[3] = (float)rtk->P[1]; rtk->sol.qr[4] = (float)rtk->P[1 + 2 * rtk->nx]; rtk->sol.qr[5] = (float)rtk->P[2]; if (rtk->opt.dynamics) { /* velocity and covariance */ for (i = 3; i < 6; i++) { rtk->sol.rr[i] = rtk->x[i]; rtk->sol.qv[i - 3] = (float)rtk->P[i + i * rtk->nx]; } rtk->sol.qv[3] = (float)rtk->P[4 + 3 * rtk->nx]; rtk->sol.qv[4] = (float)rtk->P[5 + 4 * rtk->nx]; rtk->sol.qv[5] = (float)rtk->P[5 + 3 * rtk->nx]; } 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]; } //更新卫星的fix信息以及周跳信息 for (i = 0; i < MAXSAT; i++) for (j = 0; j < nf; j++) { /* Don't lose track of which sats were used to try and resolve the ambiguities */ /* 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]++; /* inc lock count if this sat used for good fix */ if (!rtk->ssat[i].vsat[j]) continue; if (rtk->ssat[i].lock[j] < 0 || (rtk->nfix > 0 && rtk->ssat[i].fix[j] >= 2)) rtk->ssat[i].lock[j]++; } //释放局部变量，返回定位状态 free(rs); free(dts); free(var); free(y); free(e); free(azel); free(freq); 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 TKk control/result struct * prcopt_t *opt I positioning options (see rtklib.h) * return : none *-----------------------------------------------------------------------------*/ extern void rtkinit(rtk_t *rtk, const prcopt_t *opt) { sol_t sol0 = {{0}}; ambc_t ambc0 = {{{0}}}; ssat_t ssat0 = {0}; int i; trace(3, "rtkinit :\n"); rtk->sol = sol0; for (i = 0; i < 6; i++) rtk->rb[i] = 0.0; rtk->nx = NX(opt); rtk->na = NR(opt); rtk->tt = 0.0; #ifdef STATIC /* GPS+BDS NX = 78 NR =9 USING 60K */ extern double rtk_x[]; /* NX*8 = B */ extern double rtk_P[]; /* NX*NX*8 = B */ extern double rtk_xa[]; /* NR*8 = B */ extern double rtk_Pa[]; /* NR*NR*8 = B */ rtk->x = rtk_x; rtk->P = rtk_P; rtk->xa = rtk_xa; rtk->Pa = rtk_Pa; #else rtk->x = zeros(rtk->nx, 1); /* 41*8 = 328 B */ rtk->P = zeros(rtk->nx, rtk->nx); /* 41*41*8 = 13448 B */ rtk->xa = zeros(rtk->na, 1); /* 9*8 = 72 B */ rtk->Pa = zeros(rtk->na, rtk->na); /* 9*9*8 = 648 B */ #endif rtk->nfix = rtk->neb = 0; for (i = 0; i < MAXSAT; i++) { rtk->ambc[i] = ambc0; rtk->ssat[i] = ssat0; } rtk->holdamb = 0; rtk->excsat = 0; rtk->nb_ar = 0; // for (i=0;ierrbuf[i]=0; rtk->opt = *opt; rtk->initial_mode = rtk->opt.mode; rtk->com_bias = 0; rtk->sol.thres = (float)opt->thresar[0]; } /* free rtk control ------------------------------------------------------------ * free memory for rtk control struct * args : rtk_t *rtk IO rtk control/result struct * return : none *-----------------------------------------------------------------------------*/ extern void rtkfree(rtk_t *rtk) { trace(3, "rtkfree :\n"); rtk->nx = rtk->na = 0; #ifdef STATIC for (int i = 0; i < rtk->nx; i++) { rtk->x[i] = 0; rtk->P[i] = 0; } for (int i = 0; i < rtk->nx * rtk->nx; i++) { rtk->xa[i] = 0; rtk->Pa[i] = 0; } #else free(rtk->x); rtk->x = NULL; free(rtk->P); rtk->P = NULL; free(rtk->xa); rtk->xa = NULL; free(rtk->Pa); rtk->Pa = NULL; #endif } /* 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-5] O receiver GLO/GAL/BDS/IRN/QZS-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 satellites in use * 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 satellite {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 vaild (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) cycle 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 or 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 *-----------------------------------------------------------------------------*/ extern int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) { prcopt_t *opt = &rtk->opt; sol_t solb = {{0}}; /* 199 B */ gtime_t time; int i, nu, 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); /* 获取基准站nr/移动站nu 的观测值数量 感觉可以优化 */ 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 */ LOG_D("nr:%d nu:%d \r\n", nr, nu); /* STEP-1 对移动站进行单点定位 求解初始化迭代坐标*/ /* bug--若没设置动态模式会直接return 0 只做单点定位*/ if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, NULL, rtk->ssat, msg)) { errmsg(rtk, "point pos error (%s)\n", msg); if (!rtk->opt.dynamics) { outsolstat(rtk, nav); return 0; } } if (time.time != 0) rtk->tt = timediff(rtk->sol.time, time); /* return to static start if long delay without rover data */ // if (fabs(rtk->tt) > 300 && rtk->initial_mode == PMODE_STATIC_START) // { // rtk->opt.mode = PMODE_STATIC_START; // for (i = 0; i < 3; i++) // initx(rtk, rtk->sol.rr[i], VAR_POS, i); // if (rtk->opt.dynamics) // { // for (i = 3; i < 6; i++) // initx(rtk, 1E-6, VAR_VEL, i); // for (i = 6; i < 9; i++) // initx(rtk, 1E-6, VAR_ACC, i); // } // trace(3, "No data for > 5 min: switch back to static mode:\n"); // } /* single point positioning */ if (opt->mode == PMODE_SINGLE) { outsolstat(rtk, nav); return 1; } /* STEP-2 判断基站卫星数量 是否满足RTK需求 */ if (nr == 0) { errmsg(rtk, "no base station observation data for rtk\n"); outsolstat(rtk, nav); return 1; } /* STEP-3 判断基站电文龄期是否满足RTK需求 */ rtk->sol.age = (float)timediff(obs[0].time, obs[nu].time); if (fabs(rtk->sol.age) > opt->maxtdiff) { errmsg(rtk, "age of differential error (age=%.1f)\n", rtk->sol.age); outsolstat(rtk, nav); return 1; } /* STEP-4 满足以上条件 开始相对定位 */ relpos(rtk, obs, nu, nr, nav); outsolstat(rtk, nav); return 1; }