diff --git a/src/algorithms/libs/rtklib/rtklib_preceph.cc b/src/algorithms/libs/rtklib/rtklib_preceph.cc index f64654773..38525afd4 100644 --- a/src/algorithms/libs/rtklib/rtklib_preceph.cc +++ b/src/algorithms/libs/rtklib/rtklib_preceph.cc @@ -355,7 +355,8 @@ void readsp3(const char *file, nav_t *nav, int opt) *-----------------------------------------------------------------------------*/ int readsap(const char *file, gtime_t time, nav_t *nav) { - pcvs_t pcvs = {0, 0}; + pcv_t aux = { 0, {}, {}, {0,0}, {0,0}, {{},{}}, {{},{}} }; + pcvs_t pcvs = {0, 0, &aux }; pcv_t pcv0 = { 0, {}, {}, {0,0}, {0,0}, {{},{}}, {{},{}} }, *pcv; int i; diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index 8af9a11e3..3371b24fa 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -129,23 +129,23 @@ *-----------------------------------------------------------------------------*/ int rtkopenstat(const char *file, int level) { - gtime_t time=utc2gpst(timeget()); + gtime_t time = utc2gpst(timeget()); char path[1024]; - trace(3,"rtkopenstat: file=%s level=%d\n",file,level); + trace(3, "rtkopenstat: file=%s level=%d\n", file, level); - if (level<=0) return 0; + if (level <= 0) return 0; - reppath(file,path,time,"",""); + reppath(file, path, time, "",""); - if (!(fp_stat=fopen(path,"w"))) + if (!(fp_stat = fopen(path, "w"))) { - trace(1,"rtkopenstat: file open error path=%s\n",path); + trace(1, "rtkopenstat: file open error path=%s\n", path); return 0; } - strcpy(file_stat,file); - time_stat=time; - statlevel=level; + strcpy(file_stat, file); + time_stat = time; + statlevel = level; return 1; } @@ -157,109 +157,109 @@ int rtkopenstat(const char *file, int level) *-----------------------------------------------------------------------------*/ void rtkclosestat(void) { - trace(3,"rtkclosestat:\n"); + trace(3, "rtkclosestat:\n"); if (fp_stat) fclose(fp_stat); - fp_stat=NULL; - file_stat[0]='\0'; - statlevel=0; + fp_stat = NULL; + file_stat[0] = '\0'; + statlevel = 0; } /* write solution status to buffer -------------------------------------------*/ -int rtkoutstat(rtk_t *rtk, char *buff) +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(&rtk->opt); - char id[32],*p=buff; + 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(&rtk->opt); + char id[32], *p = buff; - if (rtk->sol.stat<=SOLQ_NONE) + if (rtk->sol.stat <= SOLQ_NONE) { return 0; } /* write ppp solution status to buffer */ - if (rtk->opt.mode>=PMODE_PPP_KINEMA) + if (rtk->opt.mode >= PMODE_PPP_KINEMA) { - return pppoutstat(rtk,buff); + return pppoutstat(rtk, buff); } - est=rtk->opt.mode>=PMODE_DGPS; - nfreq=est?nf:1; - tow=time2gpst(rtk->sol.time,&week); + 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]=ina?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], + for (i = 0;i<3;i++) xa[i] = ina ? 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); + 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) + 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]); + 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); + 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\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); + p += sprintf(p, "$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) + if (est && rtk->opt.ionoopt == IONOOPT_EST) { - for (i=0;issat+i; + ssat = rtk->ssat+i; if (!ssat->vs) continue; - satno2id(i+1,id); - j=II_RTK(i+1,&rtk->opt); - xa[0]=jna?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]); + satno2id(i+1, id); + j = II_RTK(i+1, &rtk->opt); + xa[0] = jna ? 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)) + 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]=jna?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]); + for (i = 0;i<2;i++) { + j = IT_RTK(i, &rtk->opt); + xa[0] = jna ? 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==2) + if (est && rtk->opt.glomodear == 2) { - for (i=0;iopt); - xa[0]=jna?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]); + j = IL_RTK(i, &rtk->opt); + xa[0] = jna ? 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); @@ -269,28 +269,28 @@ int rtkoutstat(rtk_t *rtk, char *buff) /* swap solution status file -------------------------------------------------*/ void swapsolstat(void) { - gtime_t time=utc2gpst(timeget()); + gtime_t time = utc2gpst(timeget()); char path[1024]; - if ((int)(time2gpst(time ,NULL)/INT_SWAP_STAT)== - (int)(time2gpst(time_stat,NULL)/INT_SWAP_STAT)) + if ((int)(time2gpst(time , NULL)/INT_SWAP_STAT) == + (int)(time2gpst(time_stat, NULL)/INT_SWAP_STAT)) { return; } - time_stat=time; + time_stat = time; - if (!reppath(file_stat,path,time,"","")) + if (!reppath(file_stat, path, time, "","")) { return; } if (fp_stat) fclose(fp_stat); - if (!(fp_stat=fopen(path,"w"))) + if (!(fp_stat = fopen(path, "w"))) { - trace(2,"swapsolstat: file open error path=%s\n",path); + trace(2, "swapsolstat: file open error path=%s\n", path); return; } - trace(3,"swapsolstat: path=%s\n",path); + trace(3, "swapsolstat: path=%s\n", path); } @@ -299,39 +299,39 @@ void outsolstat(rtk_t *rtk) { ssat_t *ssat; double tow; - char buff[MAXSOLMSG+1],id[32]; - int i,j,n,week,nfreq,nf=NF_RTK(&rtk->opt); + char buff[MAXSOLMSG+1], id[32]; + int i, j, n, week, nfreq, nf = NF_RTK(&rtk->opt); - if (statlevel<=0||!fp_stat||!rtk->sol.stat) return; + if (statlevel <= 0 || !fp_stat || !rtk->sol.stat) return; - trace(3,"outsolstat:\n"); + trace(3, "outsolstat:\n"); /* swap solution status file */ swapsolstat(); /* write solution status */ - n=rtkoutstat(rtk,buff); buff[n]='\0'; + n = rtkoutstat(rtk, buff); buff[n] = '\0'; - fputs(buff,fp_stat); + fputs(buff, fp_stat); - if (rtk->sol.stat==SOLQ_NONE||statlevel<=1) return; + if (rtk->sol.stat == SOLQ_NONE || statlevel <= 1) return; - tow=time2gpst(rtk->sol.time,&week); - nfreq=rtk->opt.mode>=PMODE_DGPS?nf:1; + tow = time2gpst(rtk->sol.time, &week); + nfreq = rtk->opt.mode >= PMODE_DGPS ? nf : 1; /* write residuals and status */ - for (i=0;issat+i; + ssat = rtk->ssat+i; if (!ssat->vs) continue; - satno2id(i+1,id); - for (j=0;jazel[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]); + fprintf(fp_stat, "$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\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[j]*0.25, + ssat->fix[j], ssat->slip[j]&3, ssat->lock[j], ssat->outc[j], + ssat->slipc[j], ssat->rejc[j]); } } } @@ -340,42 +340,42 @@ void outsolstat(rtk_t *rtk) /* save error message --------------------------------------------------------*/ void errmsg(rtk_t *rtk, const char *format, ...) { - char buff[256],tstr[32]; + 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); + 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=nneb?n:MAXERRMSG-rtk->neb; - memcpy(rtk->errbuf+rtk->neb,buff,n); - rtk->neb+=n; - trace(2,"%s",buff); + n = nneb ? 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=ferr[3]*bl/1E4,d=SPEED_OF_LIGHT*opt->sclkstab*dt,fact=1.0; - double sinel=sin(el); - int i=sys==SYS_GLO?1:(sys==SYS_GAL?2:0),nf=NF_RTK(opt); + double a, b, c = opt->err[3]*bl/1E4, d = SPEED_OF_LIGHT*opt->sclkstab*dt, fact = 1.0; + double sinel = sin(el); + int i = sys == SYS_GLO ? 1 : (sys == SYS_GAL ? 2 : 0), nf = NF_RTK(opt); /* extended error model */ - if (f>=nf&&opt->exterr.ena[0]) + 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;} + 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 (fexterr.ena[1]) + else if (fexterr.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;} + 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]; + 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; + return 2.0*(opt->ionoopt == IONOOPT_IFLC ? 3.0 : 1.0)*(a*a+b*b/sinel/sinel+c*c)+d*d; } @@ -416,8 +416,8 @@ double varerr(int sat, int sys, double el, double bl, double dt, int f, 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); + for (i = 0;i<3;i++) dr[i] = ru[i]-rb[i]; + return norm(dr, 3); } @@ -425,10 +425,10 @@ double baseline(const double *ru, const double *rb, double *dr) void initx(rtk_t *rtk, double xi, double var, int i) { int j; - rtk->x[i]=xi; - for (j=0;jnx;j++) + rtk->x[i] = xi; + for (j = 0;jnx;j++) { - rtk->P[i+j*rtk->nx]=rtk->P[j+i*rtk->nx]=i==j?var:0.0; + rtk->P[i+j*rtk->nx] = rtk->P[j+i*rtk->nx] = i == j ? var : 0.0; } } @@ -437,17 +437,17 @@ void initx(rtk_t *rtk, double xi, double var, int i) 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; + int i, j, k = 0; - trace(3,"selsat : nu=%d nr=%d\n",nu,nr); + trace(3, "selsat : nu=%d nr=%d\n", nu, nr); - for (i=0,j=nu;iobs[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); + 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; @@ -457,68 +457,68 @@ int selsat(const obsd_t *obs, double *azel, int nu, int nr, /* temporal update of position/velocity/acceleration -------------------------*/ void udpos(rtk_t *rtk, double tt) { - double *F,*FP,*xp,pos[3],Q[9]={0},Qv[9],var=0.0; - int i,j; + double *F, *FP, *xp, pos[3], Q[9] = {0}, Qv[9], var = 0.0; + int i, j; - trace(3,"udpos : tt=%.3f\n",tt); + trace(3, "udpos : tt=%.3f\n", tt); /* fixed mode */ - if (rtk->opt.mode==PMODE_FIXED) + if (rtk->opt.mode == PMODE_FIXED) { - for (i=0;i<3;i++) initx(rtk,rtk->opt.ru[i],1E-8,i); + for (i = 0;i<3;i++) initx(rtk, rtk->opt.ru[i], 1E-8, i); return; } /* initialize position for first epoch */ - if (norm(rtk->x,3)<=0.0) + if (norm(rtk->x, 3) <= 0.0) { - for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i); + 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); + 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 */ - if (rtk->opt.mode==PMODE_STATIC) return; + if (rtk->opt.mode == PMODE_STATIC) return; /* kinmatic mode without dynamics */ if (!rtk->opt.dynamics) { - for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i); + for (i = 0;i<3;i++) initx(rtk, rtk->sol.rr[i], VAR_POS, i); return; } /* check variance of estimated postion */ - for (i=0;i<3;i++) var+=rtk->P[i+i*rtk->nx]; var/=3.0; + 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->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); + 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; } /* state transition of position/velocity/acceleration */ - F=eye(rtk->nx); FP=mat(rtk->nx,rtk->nx); xp=mat(rtk->nx,1); + F = eye(rtk->nx); FP = mat(rtk->nx, rtk->nx); xp = mat(rtk->nx, 1); - for (i=0;i<6;i++) + for (i = 0;i<6;i++) { - F[i+(i+3)*rtk->nx]=tt; + 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); + 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++) + 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]; + rtk->P[i+6+(j+6)*rtk->nx] += Qv[i+j*3]; } free(F); free(FP); free(xp); } @@ -527,32 +527,32 @@ void udpos(rtk_t *rtk, double tt) /* temporal update of ionospheric parameters ---------------------------------*/ void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns) { - double el,fact; - int i,j; + double el, fact; + int i, j; - trace(3,"udion : tt=%.1f bl=%.0f ns=%d\n",tt,bl,ns); + trace(3, "udion : tt=%.1f bl=%.0f ns=%d\n", tt, bl, ns); - for (i=1;i<=MAXSAT;i++) + 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; + 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;iopt); + j = II_RTK(sat[i], &rtk->opt); - if (rtk->x[j]==0.0) + if (rtk->x[j] == 0.0) { - initx(rtk,1E-6,std::pow(rtk->opt.std[1]*bl/1e4, 2.0),j); + initx(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; + 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; } } } @@ -561,32 +561,32 @@ void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns) /* temporal update of tropospheric parameters --------------------------------*/ void udtrop(rtk_t *rtk, double tt, double bl) { - int i,j,k; + int i, j, k; - trace(3,"udtrop : tt=%.1f\n",tt); + trace(3, "udtrop : tt=%.1f\n", tt); - for (i=0;i<2;i++) + for (i = 0;i<2;i++) { - j=IT_RTK(i,&rtk->opt); + j = IT_RTK(i, &rtk->opt); - if (rtk->x[j]==0.0) + if (rtk->x[j] == 0.0) { - initx(rtk,INIT_ZWD,std::pow(rtk->opt.std[2], 2.0),j); /* initial zwd */ + initx(rtk, INIT_ZWD, std::pow(rtk->opt.std[2], 2.0), j); /* initial zwd */ - if (rtk->opt.tropopt>=TROPOPT_ESTG) + if (rtk->opt.tropopt >= TROPOPT_ESTG) { - for (k=0;k<2;k++) initx(rtk,1E-6,VAR_GRA,++j); + for (k = 0;k<2;k++) initx(rtk, 1E-6, VAR_GRA, ++j); } } else { - rtk->P[j+j*rtk->nx]+=std::pow(rtk->opt.prn[2], 2.0)*tt; + rtk->P[j+j*rtk->nx]+=std::pow(rtk->opt.prn[2], 2.0)*tt; - if (rtk->opt.tropopt>=TROPOPT_ESTG) + if (rtk->opt.tropopt >= TROPOPT_ESTG) { - for (k=0;k<2;k++) + 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); + rtk->P[++j*(1+rtk->nx)]+=std::pow(rtk->opt.prn[2]*0.3, 2.0)*fabs(rtk->tt); } } } @@ -597,26 +597,26 @@ void udtrop(rtk_t *rtk, double tt, double bl) /* temporal update of receiver h/w biases ------------------------------------*/ void udrcvbias(rtk_t *rtk, double tt) { - int i,j; + int i, j; - trace(3,"udrcvbias: tt=%.1f\n",tt); + trace(3, "udrcvbias: tt=%.1f\n", tt); - for (i=0;iopt); + j = IL_RTK(i, &rtk->opt); - if (rtk->x[j]==0.0) + if (rtk->x[j] == 0.0) { - initx(rtk,1E-6,VAR_HWBIAS,j); + initx(rtk, 1E-6, VAR_HWBIAS, j); } /* hold to fixed solution */ - else if (rtk->nfix>=rtk->opt.minfix&&rtk->sol.ratio>rtk->opt.thresar[0]) + else if (rtk->nfix >= rtk->opt.minfix && rtk->sol.ratio>rtk->opt.thresar[0]) { - initx(rtk,rtk->xa[j],rtk->Pa[j+j*rtk->na],j); + initx(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; + rtk->P[j+j*rtk->nx]+=std::pow(PRN_HWBIAS, 2.0)*tt; } } } @@ -625,52 +625,52 @@ void udrcvbias(rtk_t *rtk, double tt) /* detect cycle slip by LLI --------------------------------------------------*/ void detslp_ll(rtk_t *rtk, const obsd_t *obs, int i, int rcv) { - unsigned int slip,LLI; - int f,sat=obs[i].sat; + unsigned int slip, LLI; + int f, sat = obs[i].sat; - trace(3,"detslp_ll: i=%d rcv=%d\n",i,rcv); + trace(3, "detslp_ll: i=%d rcv=%d\n", i, rcv); - for (f=0;fopt.nf;f++) + for (f = 0;fopt.nf;f++) { - if (obs[i].L[f]==0.0) continue; + 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 */ + 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 (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]); + 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]; + 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); + errmsg(rtk, "slip detected backward (sat=%2d rcv=%d F=%d LLI=%x)\n", + sat, rcv, f+1, LLI); } - slip=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))) + 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]); + 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]); + 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]|=(unsigned char)slip; - rtk->ssat[sat-1].half[f]=(obs[i].LLI[f]&2)?0:1; + rtk->ssat[sat-1].half[f] = (obs[i].LLI[f]&2) ? 0 : 1; } } @@ -679,22 +679,22 @@ void detslp_ll(rtk_t *rtk, const obsd_t *obs, int i, int rcv) 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,g1; + int sat = obs[i].sat; + double g0, g1; - trace(3,"detslp_gf_L1L2: i=%d j=%d\n",i,j); + 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; + 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; + g0 = rtk->ssat[sat-1].gf; rtk->ssat[sat-1].gf = g1; - if (g0!=0.0&&fabs(g1-g0)>rtk->opt.thresslip) + 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); + errmsg(rtk, "slip detected (sat=%2d GF_L1_L2=%.3f %.3f)\n", sat, g0, g1); } } @@ -703,21 +703,21 @@ void detslp_gf_L1L2(rtk_t *rtk, const obsd_t *obs, int i, int j, 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,g1; + int sat = obs[i].sat; + double g0, g1; - trace(3,"detslp_gf_L1L5: i=%d j=%d\n",i,j); + 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; + 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; + g0 = rtk->ssat[sat-1].gf2; rtk->ssat[sat-1].gf2 = g1; - if (g0!=0.0&&fabs(g1-g0)>rtk->opt.thresslip) + 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); + errmsg(rtk, "slip detected (sat=%2d GF_L1_L5=%.3f %.3f)\n", sat, g0, g1); } } @@ -764,114 +764,114 @@ void detslp_dop(rtk_t *rtk, const obsd_t *obs, int i, int rcv, 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,lami,lam1,lam2,C1,C2; - int i,j,f,slip,reset,nf=NF_RTK(&rtk->opt); + double cp, pr, cp1, cp2, pr1, pr2, *bias, offset, lami, lam1, lam2, C1, C2; + int i, j, f, slip, reset, nf = NF_RTK(&rtk->opt); - trace(3,"udbias : tt=%.1f ns=%d\n",tt,ns); + trace(3, "udbias : tt=%.1f ns=%d\n", tt, ns); - for (i=0;iopt.nf;f++) rtk->ssat[sat[i]-1].slip[f]&=0xFC; - detslp_ll(rtk,obs,iu[i],1); - detslp_ll(rtk,obs,ir[i],2); + for (f = 0;fopt.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); + 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); + detslp_dop(rtk, obs, iu[i], 1, nav); + detslp_dop(rtk, obs, ir[i], 2, nav); /* update half-cycle valid flag */ - for (f=0;fssat[sat[i]-1].half[f]= - !((obs[iu[i]].LLI[f]&2)||(obs[ir[i]].LLI[f]&2)); + rtk->ssat[sat[i]-1].half[f] = + !((obs[iu[i]].LLI[f]&2) || (obs[ir[i]].LLI[f]&2)); } } - for (f=0;fssat[i-1].outc[f]>(unsigned int)rtk->opt.maxout; + reset = ++rtk->ssat[i-1].outc[f]>(unsigned int)rtk->opt.maxout; - if (rtk->opt.modear==ARMODE_INST&&rtk->x[IB_RTK(i,f,&rtk->opt)]!=0.0) + if (rtk->opt.modear == ARMODE_INST && rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0) { - initx(rtk,0.0,0.0,IB_RTK(i,f,&rtk->opt)); + initx(rtk, 0.0, 0.0, IB_RTK(i, f, &rtk->opt)); } - else if (reset&&rtk->x[IB_RTK(i,f,&rtk->opt)]!=0.0) + else if (reset && rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0) { - initx(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]); + initx(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) + if (rtk->opt.modear != ARMODE_INST && reset) { - rtk->ssat[i-1].lock[f]=-rtk->opt.minlock; + rtk->ssat[i-1].lock[f] = -rtk->opt.minlock; } } /* reset phase-bias if detecting cycle slip */ - for (i=0;iopt); + 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; + 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); + bias = zeros(ns, 1); /* estimate approximate phase-bias by phase - code */ - for (i=j=0,offset=0.0;iopt.ionoopt!=IONOOPT_IFLC) + 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; + 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; + 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; + 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); + 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) + if (rtk->x[IB_RTK(sat[i], f, &rtk->opt)] != 0.0) { - offset+=bias[i]-rtk->x[IB_RTK(sat[i],f,&rtk->opt)]; + 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++) + 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; + 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;ix[IB_RTK(sat[i],f,&rtk->opt)]!=0.0) continue; - initx(rtk,bias[i],std::pow(rtk->opt.std[0], 2.0),IB_RTK(sat[i],f,&rtk->opt)); + if (bias[i] == 0.0 || rtk->x[IB_RTK(sat[i], f, &rtk->opt)] != 0.0) continue; + initx(rtk, bias[i], std::pow(rtk->opt.std[0], 2.0), IB_RTK(sat[i], f, &rtk->opt)); } free(bias); } @@ -882,33 +882,33 @@ void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat, 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),bl,dr[3]; + double tt = fabs(rtk->tt), bl = 0.0, dr[3]; - trace(3,"udstate : ns=%d\n",ns); + trace(3, "udstate : ns=%d\n", ns); /* temporal update of position/velocity/acceleration */ - udpos(rtk,tt); + udpos(rtk, tt); /* temporal update of ionospheric parameters */ - if (rtk->opt.ionoopt>=IONOOPT_EST) + if (rtk->opt.ionoopt >= IONOOPT_EST) { - bl=baseline(rtk->x,rtk->rb,dr); - udion(rtk,tt,bl,sat,ns); + bl = baseline(rtk->x, rtk->rb, dr); + udion(rtk, tt, bl, sat, ns); } /* temporal update of tropospheric parameters */ - if (rtk->opt.tropopt>=TROPOPT_EST) + if (rtk->opt.tropopt >= TROPOPT_EST) { - udtrop(rtk,tt,bl); + udtrop(rtk, tt, bl); } /* temporal update of eceiver h/w bias */ - if (rtk->opt.glomodear==2&&(rtk->opt.navsys&SYS_GLO)) + if (rtk->opt.glomodear == 2 && (rtk->opt.navsys&SYS_GLO)) { - udrcvbias(rtk,tt); + udrcvbias(rtk, tt); } /* temporal update of phase-bias */ if (rtk->opt.mode>PMODE_DGPS) { - udbias(rtk,tt,obs,sat,iu,ir,ns,nav); + udbias(rtk, tt, obs, sat, iu, ir, ns, nav); } } @@ -918,46 +918,46 @@ 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,f2,C1,C2,dant_if; - int i,nf=NF_RTK(opt); + const double *lam = nav->lam[obs->sat-1]; + double f1, f2, C1, C2, dant_if; + int i, nf = NF_RTK(opt); - if (opt->ionoopt==IONOOPT_IFLC) + if (opt->ionoopt == IONOOPT_IFLC) { /* iono-free linear combination */ - if (lam[0]==0.0||lam[1]==0.0) return; + 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; + 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/lam[0]; - f2=SPEED_OF_LIGHT/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]; + f1 = SPEED_OF_LIGHT/lam[0]; + f2 = SPEED_OF_LIGHT/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) + 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; + 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) + if (obs->P[0] != 0.0 && obs->P[1] != 0.0) { - y[1]=C1*obs->P[0]+C2*obs->P[1]-r-dant_if; + y[1] = C1*obs->P[0]+C2*obs->P[1]-r-dant_if; } } else { - for (i=0;iSNR[i]*0.25,&opt->snrmask)) + 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]; + 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]; } } } @@ -969,59 +969,59 @@ int zdres(int base, const obsd_t *obs, int n, const double *rs, const double *rr, const prcopt_t *opt, int index, double *y, double *e, double *azel) { - double r,rr_[3],pos[3],dant[NFREQ]={0},disp[3]; - double zhd,zazel[]={0.0,90.0*D2R}; - int i,nf=NF_RTK(opt); + double r, rr_[3], pos[3], dant[NFREQ] = {0}, disp[3]; + double zhd, zazel[] = {0.0, 90.0*D2R}; + int i, nf = NF_RTK(opt); - trace(3,"zdres : n=%d\n",n); + trace(3, "zdres : n=%d\n", n); - for (i=0;itidecorr) { - tidedisp(gpst2utc(obs[0].time),rr_,opt->tidecorr,&nav->erp, - opt->odisp[base],disp); - for (i=0;i<3;i++) rr_[i]+=disp[i]; + 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); + ecef2pos(rr_, pos); - for (i=0;ielmin) continue; + if ((r = geodist(rs+i*6, rr_, e+i*3)) <= 0.0) continue; + if (satazel(pos, e+i*3, azel+i*2)elmin) continue; /* excluded satellite? */ - if (satexclude(obs[i].sat,svh[i],opt)) continue; + if (satexclude(obs[i].sat, svh[i], opt)) continue; /* satellite clock-bias */ r+=-SPEED_OF_LIGHT*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,NULL)*zhd; + zhd = tropmodel(obs[0].time, pos, zazel, 0.0); + r+=tropmapf(obs[i].time, pos, azel+i*2, NULL)*zhd; /* receiver antenna phase center correction */ - antmodel(opt->pcvr+index,opt->antdel[index],azel+i*2,opt->posopt[1], + 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); + 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;iopt.baseline[0]<=0.0) return 0; + if (rtk->opt.baseline[0] <= 0.0) return 0; /* time-adjusted baseline vector and length */ - for (i=0;i<3;i++) + for (i = 0;i<3;i++) { - xb[i]=rtk->rb[i]+rtk->rb[i+3]*rtk->sol.age; - b[i]=x[i]-xb[i]; + xb[i] = rtk->rb[i]+rtk->rb[i+3]*rtk->sol.age; + b[i] = x[i]-xb[i]; } - bb=norm(b,3); + bb = norm(b, 3); /* approximate variance of solution */ if (P) { - for (i=0;i<3;i++) var+=P[i+i*rtk->nx]; + 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); + 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; + v[index] = rtk->opt.baseline[0]-bb; if (H) { - for (i=0;i<3;i++) H[i+index*rtk->nx]=b[i]/bb; + 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); + 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]); + trace(4, "baseline len v=%13.3f R=%8.6f %8.6f\n", v[index], Ri[index], Rj[index]); return 1; } @@ -1110,24 +1110,24 @@ 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_RTK(r,opt); + double m_w = 0.0, cotz, grad_n, grad_e; + int i = IT_RTK(r, opt); /* wet mapping function */ - tropmapf(time,pos,azel,&m_w); + tropmapf(time, pos, azel, &m_w); - if (opt->tropopt>=TROPOPT_ESTG&&azel[1]>0.0) + 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]); + 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]; + dtdx[1] = grad_n*x[i]; + dtdx[2] = grad_e*x[i]; } - else dtdx[1]=dtdx[2]=0.0; - dtdx[0]=m_w; + else dtdx[1] = dtdx[2] = 0.0; + dtdx[0] = m_w; return m_w*x[i]; } @@ -1138,9 +1138,9 @@ double gloicbcorr(int sat1, int sat2, const prcopt_t *opt, double lam1, { double dfreq; - if (f>=NFREQGLO||f>=opt->nf||!opt->exterr.ena[2]) return 0.0; + if (f>=NFREQGLO || f >= opt->nf || !opt->exterr.ena[2]) return 0.0; - dfreq=(SPEED_OF_LIGHT/lam1-SPEED_OF_LIGHT/lam2)/(f==0?DFRQ1_GLO:DFRQ2_GLO); + dfreq = (SPEED_OF_LIGHT/lam1-SPEED_OF_LIGHT/lam2)/(f == 0 ? DFRQ1_GLO : DFRQ2_GLO); return opt->exterr.gloicb[f]*0.01*dfreq; /* (m) */ } @@ -1151,12 +1151,12 @@ 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; + 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; } @@ -1168,172 +1168,172 @@ int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x, 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,dr[3],posu[3],posr[3],didxi=0.0,didxj=0.0,*im; - double *tropr,*tropu,*dtdxr,*dtdxu,*Ri,*Rj,lami,lamj,fi,fj,df,*Hi=NULL; - int i,j,k,m,f,ff,nv=0,nb[NFREQ*4*2+2]={0},b=0,sysi,sysj,nf=NF_RTK(opt); + prcopt_t *opt = &rtk->opt; + double bl, dr[3], posu[3], posr[3], didxi = 0.0, didxj = 0.0, *im; + double *tropr, *tropu, *dtdxr, *dtdxu, *Ri, *Rj, lami, lamj, fi, fj, df, *Hi = NULL; + int i, j, k, m, f, ff, nv = 0, nb[NFREQ*4*2+2] = {0}, b = 0, sysi, sysj, nf = NF_RTK(opt); - trace(3,"ddres : dt=%.1f nx=%d ns=%d\n",dt,rtk->nx,ns); + 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); + 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); + 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;issat[i].resp[j]=rtk->ssat[i].resc[j]=0.0; + rtk->ssat[i].resp[j] = rtk->ssat[i].resc[j] = 0.0; } /* compute factors of ionospheric and tropospheric delay */ - for (i=0;iionoopt>=IONOOPT_EST) + if (opt->ionoopt >= IONOOPT_EST) { - im[i]=(ionmapf(posu,azel+iu[i]*2)+ionmapf(posr,azel+ir[i]*2))/2.0; + im[i] = (ionmapf(posu, azel+iu[i]*2)+ionmapf(posr, azel+ir[i]*2))/2.0; } - if (opt->tropopt>=TROPOPT_EST) + 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); + 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 (m = 0;m<4;m++) /* m=0:gps/qzs/sbs, 1:glo, 2:gal, 3:bds */ - for (f=opt->mode>PMODE_DGPS?0:nf;fmode>PMODE_DGPS ? 0 : nf;fssat[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; + 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;jssat[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; + 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; + 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;knx;k++) Hi[k]=0.0; + Hi = H+nv*rtk->nx; + for (k = 0;knx;k++) Hi[k] = 0.0; } /* double-differenced residual */ - v[nv]=(y[f+iu[i]*nf*2]-y[f+ir[i]*nf*2])- + 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++) + for (k = 0;k<3;k++) { - Hi[k]=-e[k+iu[i]*3]+e[k+iu[j]*3]; + Hi[k] = -e[k+iu[i]*3]+e[k+iu[j]*3]; } } /* double-differenced ionospheric delay term */ - if (opt->ionoopt==IONOOPT_EST) + if (opt->ionoopt == IONOOPT_EST) { - fi=lami/lam_carr[0]; fj=lamj/lam_carr[0]; - didxi=(ftropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) + if (opt->tropopt == TROPOPT_EST || opt->tropopt == TROPOPT_ESTG) { - v[nv]-=(tropu[i]-tropu[j])-(tropr[i]-tropr[j]); - for (k=0;k<(opt->tropopttropoptionoopt!=IONOOPT_IFLC) + if (opt->ionoopt != IONOOPT_IFLC) { - v[nv]-=lami*x[IB_RTK(sat[i],f,opt)]-lamj*x[IB_RTK(sat[j],f,opt)]; + 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; + 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)]; + 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; + 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&&ffopt.glomodear == 2 && sysi == SYS_GLO && sysj == SYS_GLO && ffopt,lami,lamj,f); + v[nv] -= gloicbcorr(sat[i], sat[j], &rtk->opt, lami, lamj, f); } - if (fssat[sat[j]-1].resc[f ]=v[nv]; - else rtk->ssat[sat[j]-1].resp[f-nf]=v[nv]; + if (fssat[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 (opt->maxinno>0.0 && fabs(v[nv])>opt->maxinno) { if (fssat[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],fmode>PMODE_DGPS) { - if (fssat[sat[i]-1].vsat[f]=rtk->ssat[sat[j]-1].vsat[f]=1; + if (fssat[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; + 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],fssat[j].resc[f]!=0.0) rtk->ssat[j].resc[f]-=s; + if (j == sat[i]-1||rtk->ssat[j].resc[f]!=0.0) rtk->ssat[j].resc[f]-=s; } } else @@ -1353,7 +1353,7 @@ int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x, s/=nb[b]+1; for (j=0;jssat[j].resp[f-nf]!=0.0) + if (j == sat[i]-1||rtk->ssat[j].resp[f-nf]!=0.0) rtk->ssat[j].resp[f-nf]-=s; } } @@ -1363,15 +1363,15 @@ int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x, /* end of system loop */ /* baseline length constraint for moving baseline */ - if (opt->mode==PMODE_MOVEB&&constbl(rtk,x,P,v,H,Ri,Rj,nv)) + if (opt->mode == PMODE_MOVEB && constbl(rtk, x, P, v, H, Ri, Rj, nv)) { - vflg[nv++]=3<<4; + vflg[nv++] = 3<<4; nb[b++]++; } - if (H) {trace(5,"H=\n"); tracemat(5,H,rtk->nx,nv,7,4);} + 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); + ddcov(nb, b, Ri, Rj, nv, R); free(Ri); free(Rj); free(im); free(tropu); free(tropr); free(dtdxu); free(dtdxr); @@ -1385,105 +1385,105 @@ 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]; - 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_RTK(opt); + static double yb[MAXOBS*NFREQ*2], rs[MAXOBS*6], dts[MAXOBS*2], var[MAXOBS]; + static double e[MAXOBS*3], azel[MAXOBS*2]; + 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_RTK(opt); - trace(3,"intpres : n=%d tt=%.1f\n",n,tt); + trace(3, "intpres : n=%d tt=%.1f\n", n, tt); - if (nb==0||fabs(tt)opt->maxtdiff*2.0||ttb==tt) 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); + 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)) + if (!zdres(1, obsb, nb, rs, dts, svh, nav, rtk->rb, opt, 1, yb, e, azel)) { return tt; } - for (i=0;i=nb) continue; - for (k=0,p=y+i*nf*2,q=yb+j*nf*2;k= nb) continue; + for (k = 0, p = y+i*nf*2, q = yb+j*nf*2;kfabs(tt)?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,j,k,m,f,nb=0,nx=rtk->nx,na=rtk->na,nf=NF_RTK(&rtk->opt),nofix; + int i, j, k, m, f, nb = 0, nx = rtk->nx, na = rtk->na, nf = NF_RTK(&rtk->opt), nofix; - trace(3,"ddmat :\n"); + trace(3, "ddmat :\n"); - for (i=0;issat[i].fix[j]=0; + rtk->ssat[i].fix[j] = 0; } - for (i=0;iopt.glomodear==0)||(m==3&&rtk->opt.bdsmodear==0); + nofix = (m == 1 && rtk->opt.glomodear == 0) || (m == 3 && rtk->opt.bdsmodear == 0); - for (f=0,k=na;fx[i]==0.0||!test_sys(rtk->ssat[i-k].sys,m)|| + if (rtk->x[i] == 0.0||!test_sys(rtk->ssat[i-k].sys,m)|| !rtk->ssat[i-k].vsat[f]) { #else - 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]) + 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]) { #endif 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) + 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 */ + rtk->ssat[i-k].fix[f] = 2; /* fix */ break; } - else rtk->ssat[i-k].fix[f]=1; + else rtk->ssat[i-k].fix[f] = 1; } - for (j=k;jx[j]==0.0||!test_sys(rtk->ssat[j-k].sys,m)|| + 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) + 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; + D[i+(na+nb)*nx] = 1.0; + D[j+(na+nb)*nx] = -1.0; nb++; - rtk->ssat[j-k].fix[f]=2; /* fix */ + rtk->ssat[j-k].fix[f] = 2; /* fix */ } - else rtk->ssat[j-k].fix[f]=1; + else rtk->ssat[j-k].fix[f] = 1; } } } - trace(5,"D=\n"); tracemat(5,D,nx,na+nb,2,0); + trace(5, "D=\n"); tracemat(5, D, nx, na+nb, 2, 0); return nb; } @@ -1491,31 +1491,31 @@ int ddmat(rtk_t *rtk, double *D) /* restore single-differenced ambiguity --------------------------------------*/ void restamb(rtk_t *rtk, const double *bias, int nb, double *xa) { - int i,n,m,f,index[MAXSAT],nv=0,nf=NF_RTK(&rtk->opt); + int i, n, m, f, index[MAXSAT], nv = 0, nf = NF_RTK(&rtk->opt); - trace(3,"restamb :\n"); + trace(3, "restamb :\n"); - for (i=0;inx;i++) xa[i]=rtk->x [i]; - for (i=0;ina;i++) xa[i]=rtk->xa[i]; + for (i = 0;inx;i++) xa[i] = rtk->x [i]; + for (i = 0;ina;i++) xa[i] = rtk->xa[i]; - for (m=0;m<4;m++) for (f=0;fssat[i].sys,m)||rtk->ssat[i].fix[f]!=2) + if (!test_sys(rtk->ssat[i].sys, m) || rtk->ssat[i].fix[f] != 2) { continue; } - index[n++]=IB_RTK(i+1,f,&rtk->opt); + index[n++] = IB_RTK(i+1, f, &rtk->opt); } if (n<2) continue; - xa[index[0]]=rtk->x[index[0]]; + xa[index[0]] = rtk->x[index[0]]; - for (i=1;inx-rtk->na,nv=0,nf=NF_RTK(&rtk->opt); + double *v, *H, *R; + int i, n, m, f, info, index[MAXSAT], nb = rtk->nx-rtk->na, nv = 0, nf = NF_RTK(&rtk->opt); - trace(3,"holdamb :\n"); + trace(3, "holdamb :\n"); - v=mat(nb,1); H=zeros(nb,rtk->nx); + v = mat(nb, 1); H = zeros(nb, rtk->nx); - for (m=0;m<4;m++) for (f=0;fssat[i].sys,m)||rtk->ssat[i].fix[f]!=2|| + if (!test_sys(rtk->ssat[i].sys, m) || rtk->ssat[i].fix[f] != 2 || rtk->ssat[i].azel[1]opt.elmaskhold) { continue; } - index[n++]=IB_RTK(i+1,f,&rtk->opt); - rtk->ssat[i].fix[f]=3; /* hold */ + index[n++] = IB_RTK(i+1, f, &rtk->opt); + rtk->ssat[i].fix[f] = 3; /* hold */ } /* constraint to fixed ambiguity */ - for (i=1;ix[index[0]]-rtk->x[index[i]]); + for (i = 1;ix[index[0]]-rtk->x[index[i]]); - H[index[0]+nv*rtk->nx]= 1.0; - H[index[i]+nv*rtk->nx]=-1.0; + 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;ix,rtk->P,H,v,R,rtk->nx,nv))) + if ((info = filter(rtk->x, rtk->P, H, v, R, rtk->nx, nv))) { - errmsg(rtk,"filter error (info=%d)\n",info); + errmsg(rtk, "filter error (info=%d)\n", info); } free(R); } @@ -1572,92 +1572,92 @@ int ddmat(rtk_t *rtk, double *D) /* resolve integer ambiguity by LAMBDA ---------------------------------------*/ int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa) { - prcopt_t *opt=&rtk->opt; - int i,j,ny,nb,info,nx=rtk->nx,na=rtk->na; - double *D,*DP,*y,*Qy,*b,*db,*Qb,*Qab,*QQ,s[2]; + prcopt_t *opt = &rtk->opt; + int i, j, ny, nb, info, nx = rtk->nx, na = rtk->na; + double *D, *DP, *y, *Qy, *b, *db, *Qb, *Qab, *QQ, s[2]; - trace(3,"resamb_LAMBDA : nx=%d\n",nx); + trace(3, "resamb_LAMBDA : nx=%d\n", nx); - rtk->sol.ratio=0.0; + rtk->sol.ratio = 0.0; - if (rtk->opt.mode<=PMODE_DGPS||rtk->opt.modear==ARMODE_OFF|| + 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) + D = zeros(nx, nx); + if ((nb = ddmat(rtk, D)) <= 0) { - errmsg(rtk,"no valid double-difference\n"); + 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); + 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); + 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;ixa); + 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); + 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]); + 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); + restamb(rtk, bias, nb, xa); } - else nb=0; + 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; + 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); + 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); @@ -1667,30 +1667,30 @@ int ddmat(rtk_t *rtk, double *D) /* validation of solution ----------------------------------------------------*/ - int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg, + 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,stat=1,sat1,sat2,type,freq; + double fact = thres*thres; + int i, stat = 1, sat1, sat2, type, freq; char stype; - trace(3,"valpos : nv=%d thres=%.1f\n",nv,thres); + trace(3, "valpos : nv=%d thres=%.1f\n", nv, thres); /* post-fit residual test */ - for (i=0;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 %s%d v=%6.3f sig=%.3f)\n", - sat1,sat2,stype,freq+1,v[i],std::sqrt(R[i+i*nv])); + 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 %s%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)) @@ -1720,34 +1720,34 @@ int ddmat(rtk_t *rtk, double *D) 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,*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; + prcopt_t *opt = &rtk->opt; + gtime_t time = obs[0].time; + double *rs, *dts, *var, *y, *e, *azel, *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); + trace(3, "relpos : nx=%d nu=%d nr=%d\n", rtk->nx, nu, nr); - dt=timediff(time,obs[nu].time); + 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); + 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;issat[i].sys=satsys(i+1,NULL); - for (j=0;jssat[i].vsat[j]=rtk->ssat[i].snr[j]=0; + rtk->ssat[i].sys = satsys(i+1, NULL); + for (j = 0;jssat[i].vsat[j] = rtk->ssat[i].snr[j] = 0; } /* satellite positions/clocks */ - satposs(time,obs,n,nav,opt->sateph,rs,dts,var,svh); + satposs(time, obs, n, nav, opt->sateph, rs, dts, var, svh); /* 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)) + 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"); + errmsg(rtk, "initial base station position error\n"); free(rs); free(dts); free(var); free(y); free(e); free(azel); return 0; @@ -1755,171 +1755,171 @@ int ddmat(rtk_t *rtk, double *D) /* time-interpolation of residuals (for post-processing) */ if (opt->intpref) { - dt=intpres(time,obs+nu,nr,nav,rtk,y+nu*nf*2); + 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,iu,ir))<=0) + if ((ns = selsat(obs, azel, nu, nr, opt, sat, iu, ir)) <= 0) { - errmsg(rtk,"no common satellite\n"); + 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,iu,ir,ns,nav); + udstate(rtk, obs, sat, iu, ir, ns, nav); - trace(4,"x(0)="); tracemat(4,rtk->x,1,NR_RTK(opt),13,4); + 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); + 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); + 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); + niter = opt->niter+(opt->mode == PMODE_MOVEB && opt->baseline[0]>0.0 ? 2 : 0); - for (i=0;iP,rtk->nx,rtk->nx); - if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) + 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; + 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); + 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,nav,xp,opt,0,y,e,azel)) + if (stat != SOLQ_NONE && zdres(0, obs, nu, rs, dts, svh, nav, xp, opt, 0, y, e, azel)) { /* post-fit residuals for float solution */ - nv=ddres(rtk,nav,dt,xp,Pp,sat,y,e,azel,iu,ir,ns,v,NULL,R,vflg); + nv = ddres(rtk, nav, dt, xp, Pp, sat, y, e, azel, iu, ir, ns, v, NULL, R, vflg); /* validation of float solution */ - if (valpos(rtk,v,R,vflg,nv,4.0)) + if (valpos(rtk, v, R, vflg, nv, 4.0)) { /* update state and covariance matrix */ - matcpy(rtk->x,xp,rtk->nx,1); - matcpy(rtk->P,Pp,rtk->nx,rtk->nx); + 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;isol.ns = 0; + for (i = 0;issat[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 */ + 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; + if (rtk->sol.ns<4) stat = SOLQ_NONE; } - else stat=SOLQ_NONE; + else stat = SOLQ_NONE; } /* resolve integer ambiguity by WL-NL */ - if (stat!=SOLQ_NONE&&rtk->opt.modear==ARMODE_WLNL) + if (stat != SOLQ_NONE && rtk->opt.modear == ARMODE_WLNL) { - if (resamb_WLNL(rtk,obs,sat,iu,ir,ns,nav,azel)) + if (resamb_WLNL(rtk, obs, sat, iu, ir, ns, nav, azel)) { - stat=SOLQ_FIX; + stat = SOLQ_FIX; } } /* resolve integer ambiguity by TCAR */ - else if (stat!=SOLQ_NONE&&rtk->opt.modear==ARMODE_TCAR) + else if (stat != SOLQ_NONE && rtk->opt.modear == ARMODE_TCAR) { - if (resamb_TCAR(rtk,obs,sat,iu,ir,ns,nav,azel)) + if (resamb_TCAR(rtk, obs, sat, iu, ir, ns, nav, azel)) { - stat=SOLQ_FIX; + stat = SOLQ_FIX; } } /* resolve integer ambiguity by LAMBDA */ - else if (stat!=SOLQ_NONE&&resamb_LAMBDA(rtk,bias,xa)>1) + else if (stat != SOLQ_NONE && resamb_LAMBDA(rtk, bias, xa)>1) { - if (zdres(0,obs,nu,rs,dts,svh,nav,xa,opt,0,y,e,azel)) + if (zdres(0, obs, nu, rs, dts, svh, nav, xa, opt, 0, y, e, azel)) { /* post-fit reisiduals for fixed solution */ - nv=ddres(rtk,nav,dt,xa,NULL,sat,y,e,azel,iu,ir,ns,v,NULL,R,vflg); + nv = ddres(rtk, nav, dt, xa, NULL, sat, y, e, azel, iu, ir, ns, v, NULL, R, vflg); /* validation of fixed solution */ - if (valpos(rtk,v,R,vflg,nv,4.0)) + if (valpos(rtk, v, R, vflg, nv, 4.0)) { /* hold integer ambiguity */ - if (++rtk->nfix>=rtk->opt.minfix&& - rtk->opt.modear==ARMODE_FIXHOLD) + if (++rtk->nfix >= rtk->opt.minfix && + rtk->opt.modear == ARMODE_FIXHOLD) { - holdamb(rtk,xa); + holdamb(rtk, xa); } - stat=SOLQ_FIX; + stat = SOLQ_FIX; } } } /* save solution status */ - if (stat==SOLQ_FIX) + if (stat == SOLQ_FIX) { - for (i=0;i<3;i++) + 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.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]; + 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]; } else { - for (i=0;i<3;i++) + 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.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]; - rtk->nfix=0; + 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]; + rtk->nfix = 0; } - for (i=0;issat[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]; + 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;issat[sat[i]-1].snr[j]=obs[iu[i]].SNR[j]; + rtk->ssat[sat[i]-1].snr[j] = obs[iu[i]].SNR[j]; } - for (i=0;issat[i].fix[j]==2&&stat!=SOLQ_FIX) rtk->ssat[i].fix[j]=1; + 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; + if (stat != SOLQ_NONE) rtk->sol.stat = stat; - return stat!=SOLQ_NONE; + return stat != SOLQ_NONE; } @@ -1929,32 +1929,32 @@ int ddmat(rtk_t *rtk, double *D) * prcopt_t *opt I positioning options (see rtklib.h) * return : none *-----------------------------------------------------------------------------*/ - void rtkinit(rtk_t *rtk, const prcopt_t *opt) + 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}; + 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"); + 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;isol = 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;iambc[i]=ambc0; - rtk->ssat[i]=ssat0; + rtk->ambc[i] = ambc0; + rtk->ssat[i] = ssat0; } - for (i=0;ierrbuf[i]=0; - rtk->opt=*opt; + for (i = 0;ierrbuf[i] = 0; + rtk->opt = *opt; } @@ -1965,13 +1965,13 @@ int ddmat(rtk_t *rtk, double *D) *-----------------------------------------------------------------------------*/ void rtkfree(rtk_t *rtk) { - trace(3,"rtkfree :\n"); + trace(3, "rtkfree :\n"); - rtk->nx=rtk->na=0; - free(rtk->x ); rtk->x =NULL; - free(rtk->P ); rtk->P =NULL; - free(rtk->xa); rtk->xa=NULL; - free(rtk->Pa); rtk->Pa=NULL; + rtk->nx = rtk->na = 0; + free(rtk->x ); rtk->x = NULL; + free(rtk->P ); rtk->P = NULL; + free(rtk->xa); rtk->xa = NULL; + free(rtk->Pa); rtk->Pa = NULL; } @@ -2035,32 +2035,32 @@ int ddmat(rtk_t *rtk, double *D) *-----------------------------------------------------------------------------*/ 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}; + prcopt_t *opt = &rtk->opt; + sol_t solb = { {0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0}; gtime_t time; - int i,nu,nr; - char msg[128]=""; + 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); + 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 staion position */ - if (opt->refpos<=POSOPT_RINEX&&opt->mode!=PMODE_SINGLE&& - opt->mode!=PMODE_MOVEB) + 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; + 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 sol.time; /* previous epoch */ + time = rtk->sol.time; /* previous epoch */ /* rover position by single point positioning */ - if (!pntpos(obs,nu,nav,&rtk->opt,&rtk->sol,NULL,rtk->ssat,msg)) + if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, NULL, rtk->ssat, msg)) { - errmsg(rtk,"point pos error (%s)\n",msg); + errmsg(rtk, "point pos error (%s)\n", msg); if (!rtk->opt.dynamics) { @@ -2068,10 +2068,10 @@ int ddmat(rtk_t *rtk, double *D) return 0; } } - if (time.time!=0) rtk->tt=timediff(rtk->sol.time,time); + if (time.time != 0) rtk->tt=timediff(rtk->sol.time, time); /* single point positioning */ - if (opt->mode==PMODE_SINGLE) + if (opt->mode == PMODE_SINGLE) { outsolstat(rtk); return 1; @@ -2079,56 +2079,56 @@ int ddmat(rtk_t *rtk, double *D) /* suppress output of single solution */ if (!opt->outsingle) { - rtk->sol.stat=SOLQ_NONE; + rtk->sol.stat = SOLQ_NONE; } /* precise point positioning */ - if (opt->mode>=PMODE_PPP_KINEMA) + if (opt->mode >= PMODE_PPP_KINEMA) { - pppos(rtk,obs,nu,nav); + pppos(rtk, obs, nu, nav); outsolstat(rtk); return 1; } /* check number of data of base station and age of differential */ - if (nr==0) + if (nr == 0) { - errmsg(rtk,"no base station observation data for rtk\n"); + errmsg(rtk, "no base station observation data for rtk\n"); outsolstat(rtk); return 1; } - if (opt->mode==PMODE_MOVEB) + if (opt->mode == PMODE_MOVEB) { /* moving baseline */ /* estimate position/velocity of base station */ - if (!pntpos(obs+nu,nr,nav,&rtk->opt,&solb,NULL,NULL,msg)) + if (!pntpos(obs+nu, nr, nav, &rtk->opt, &solb, NULL, NULL, msg)) { - errmsg(rtk,"base station position error (%s)\n",msg); + errmsg(rtk, "base station position error (%s)\n", msg); return 0; } - rtk->sol.age=(float)timediff(rtk->sol.time,solb.time); + rtk->sol.age = (float)timediff(rtk->sol.time, solb.time); if (fabs(rtk->sol.age)>TTOL_MOVEB) { - errmsg(rtk,"time sync error for moving-base (age=%.1f)\n",rtk->sol.age); + 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]; + 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; + for (i = 0;i<3;i++) rtk->rb[i]+=rtk->rb[i+3]*rtk->sol.age; } else { - rtk->sol.age=(float)timediff(obs[0].time,obs[nu].time); + 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); + errmsg(rtk, "age of differential error (age=%.1f)\n", rtk->sol.age); outsolstat(rtk); return 1; } } /* relative potitioning */ - relpos(rtk,obs,nu,nr,nav); + relpos(rtk, obs, nu, nr, nav); outsolstat(rtk); return 1;