From f49e5ae3b081c5ecd2f1256f64ffafaf265f03a4 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 6 May 2017 12:34:09 +0200 Subject: [PATCH] Make code more readable --- src/algorithms/libs/rtklib/rtklib_ppp.cc | 2182 ++++++++++++---------- src/algorithms/libs/rtklib/rtklib_ppp.h | 100 +- 2 files changed, 1221 insertions(+), 1061 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_ppp.cc b/src/algorithms/libs/rtklib/rtklib_ppp.cc index 91b1aa626..211bf263e 100644 --- a/src/algorithms/libs/rtklib/rtklib_ppp.cc +++ b/src/algorithms/libs/rtklib/rtklib_ppp.cc @@ -59,432 +59,489 @@ #include "rtklib_lambda.h" /* wave length of LC (m) -----------------------------------------------------*/ - double lam_LC(int i, int j, int k) +double lam_LC(int i, int j, int k) { - const double f1=FREQ1,f2=FREQ2,f5=FREQ5; + const double f1 = FREQ1, f2 = FREQ2, f5 = FREQ5; return SPEED_OF_LIGHT/(i*f1+j*f2+k*f5); } -/* carrier-phase LC (m) ------------------------------------------------------*/ - double L_LC(int i, int j, int k, const double *L) -{ - const double f1=FREQ1,f2=FREQ2,f5=FREQ5; - double L1,L2,L5; - if ((i&&!L[0])||(j&&!L[1])||(k&&!L[2])) { - return 0.0; - } - L1=SPEED_OF_LIGHT/f1*L[0]; - L2=SPEED_OF_LIGHT/f2*L[1]; - L5=SPEED_OF_LIGHT/f5*L[2]; + +/* carrier-phase LC (m) ------------------------------------------------------*/ +double L_LC(int i, int j, int k, const double *L) +{ + const double f1 = FREQ1, f2 = FREQ2, f5 = FREQ5; + double L1, L2, L5; + + if ((i && !L[0]) || (j && !L[1]) || (k && !L[2])) + { + return 0.0; + } + L1 = SPEED_OF_LIGHT/f1*L[0]; + L2 = SPEED_OF_LIGHT/f2*L[1]; + L5 = SPEED_OF_LIGHT/f5*L[2]; return (i*f1*L1+j*f2*L2+k*f5*L5)/(i*f1+j*f2+k*f5); } -/* pseudorange LC (m) --------------------------------------------------------*/ - double P_LC(int i, int j, int k, const double *P) -{ - const double f1=FREQ1,f2=FREQ2,f5=FREQ5; - double P1,P2,P5; - if ((i&&!P[0])||(j&&!P[1])||(k&&!P[2])) { - return 0.0; - } - P1=P[0]; - P2=P[1]; - P5=P[2]; + +/* pseudorange LC (m) --------------------------------------------------------*/ +double P_LC(int i, int j, int k, const double *P) +{ + const double f1 = FREQ1, f2 = FREQ2, f5 = FREQ5; + double P1, P2, P5; + + if ((i && !P[0]) || (j && !P[1]) || (k && !P[2])) + { + return 0.0; + } + P1 = P[0]; + P2 = P[1]; + P5 = P[2]; return (i*f1*P1+j*f2*P2+k*f5*P5)/(i*f1+j*f2+k*f5); } + + /* noise variance of LC (m) --------------------------------------------------*/ - double var_LC(int i, int j, int k, double sig) +double var_LC(int i, int j, int k, double sig) { - const double f1=FREQ1,f2=FREQ2,f5=FREQ5; + const double f1 = FREQ1, f2 = FREQ2, f5 = FREQ5; - return (SQR(i*f1)+SQR(j*f2)+SQR(k*f5))/SQR(i*f1+j*f2+k*f5)*SQR(sig); + return (std::pow(i*f1, 2.0)+std::pow(j*f2, 2.0)+std::pow(k*f5, 2.0))/std::pow(i*f1+j*f2+k*f5, 2.0)*std::pow(sig, 2.0); } + + /* complementaty error function (ref [1] p.227-229) --------------------------*/ - - double p_gamma(double a, double x, double log_gamma_a) +double p_gamma(double a, double x, double log_gamma_a) { - double y,w; + double y, w; int i; - if (x==0.0) return 0.0; - if (x>=a+1.0) return 1.0-q_gamma(a,x,log_gamma_a); + if (x == 0.0) return 0.0; + if (x >= a+1.0) return 1.0-q_gamma(a, x, log_gamma_a); - y=w=exp(a*log(x)-x-log_gamma_a)/a; + y = w = exp(a*log(x)-x-log_gamma_a)/a; - for (i=1;i<100;i++) { - w*=x/(a+i); - y+=w; - if (fabs(w)<1E-15) break; - } + for (i = 1;i<100;i++) + { + w *= x/(a+i); + y += w; + if (fabs(w)<1E-15) break; + } return y; } - double q_gamma(double a, double x, double log_gamma_a) + + +double q_gamma(double a, double x, double log_gamma_a) { - double y,w,la=1.0,lb=x+1.0-a,lc; + double y, w, la = 1.0, lb = x+1.0-a, lc; int i; - if (x=0.0?q_gamma(0.5,x*x,LOG_PI/2.0):1.0+p_gamma(0.5,x*x,LOG_PI/2.0); + return x >= 0.0?q_gamma(0.5, x*x, LOG_PI/2.0):1.0+p_gamma(0.5, x*x, LOG_PI/2.0); } + + /* confidence function of integer ambiguity ----------------------------------*/ - double conffunc(int N, double B, double sig) +double conffunc(int N, double B, double sig) { - double x,p=1.0; + double x, p = 1.0; int i; - x=fabs(B-N); - for (i=1;i<8;i++) { - p-=f_erfc((i-x)/(SQRT2*sig))-f_erfc((i+x)/(SQRT2*sig)); - } + x = fabs(B-N); + for (i = 1;i<8;i++) + { + p -= f_erfc((i-x)/(SQRT2*sig))-f_erfc((i+x)/(SQRT2*sig)); + } return p; } + + /* average LC ----------------------------------------------------------------*/ - void average_LC(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, - const double *azel) +void average_LC(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, + const double *azel) { ambc_t *amb; - double LC1,LC2,LC3,var1,var2,var3,sig; - int i,j,sat; + double LC1, LC2, LC3, var1, var2, var3, sig; + int i, j, sat; - for (i=0;iopt.elmin) continue; + if (azel[1+2*i]opt.elmin) continue; - if (satsys(sat,NULL)!=SYS_GPS) continue; + if (satsys(sat, NULL) != SYS_GPS) continue; - /* triple-freq carrier and code LC (m) */ - LC1=L_LC(1,-1, 0,obs[i].L)-P_LC(1,1,0,obs[i].P); - LC2=L_LC(0, 1,-1,obs[i].L)-P_LC(0,1,1,obs[i].P); - LC3=L_LC(1,-6, 5,obs[i].L)-P_LC(1,1,0,obs[i].P); + /* triple-freq carrier and code LC (m) */ + LC1 = L_LC(1, -1, 0, obs[i].L)-P_LC(1, 1, 0, obs[i].P); + LC2 = L_LC(0, 1, -1, obs[i].L)-P_LC(0, 1, 1, obs[i].P); + LC3 = L_LC(1, -6, 5, obs[i].L)-P_LC(1, 1, 0, obs[i].P); - sig=sqrt(SQR(rtk->opt.err[1])+SQR(rtk->opt.err[2]/sin(azel[1+2*i]))); + sig = std::sqrt(std::pow(rtk->opt.err[1], 2.0)+std::pow(rtk->opt.err[2]/sin(azel[1+2*i]), 2.0)); - /* measurement noise variance (m) */ - var1=var_LC(1,1,0,sig*rtk->opt.eratio[0]); - var2=var_LC(0,1,1,sig*rtk->opt.eratio[0]); - var3=var_LC(1,1,0,sig*rtk->opt.eratio[0]); + /* measurement noise variance (m) */ + var1 = var_LC(1, 1, 0, sig*rtk->opt.eratio[0]); + var2 = var_LC(0, 1, 1, sig*rtk->opt.eratio[0]); + var3 = var_LC(1, 1, 0, sig*rtk->opt.eratio[0]); - amb=rtk->ambc+sat-1; + amb = rtk->ambc+sat-1; - if (rtk->ssat[sat-1].slip[0]||rtk->ssat[sat-1].slip[1]|| - rtk->ssat[sat-1].slip[2]||amb->n[0]==0.0|| - fabs(timediff(amb->epoch[0],obs[0].time))>MIN_ARC_GAP) { + if (rtk->ssat[sat-1].slip[0] || rtk->ssat[sat-1].slip[1] || + rtk->ssat[sat-1].slip[2] || amb->n[0] == 0.0 || + fabs(timediff(amb->epoch[0], obs[0].time))>MIN_ARC_GAP) + { - amb->n[0]=amb->n[1]=amb->n[2]=0.0; - amb->LC[0]=amb->LC[1]=amb->LC[2]=0.0; - amb->LCv[0]=amb->LCv[1]=amb->LCv[2]=0.0; - amb->fixcnt=0; - for (j=0;jflags[j]=0; + amb->n[0] = amb->n[1] = amb->n[2] = 0.0; + amb->LC[0] = amb->LC[1] = amb->LC[2] = 0.0; + amb->LCv[0] = amb->LCv[1] = amb->LCv[2] = 0.0; + amb->fixcnt = 0; + for (j = 0;jflags[j] = 0; + } + /* averaging */ + if (LC1) + { + amb->n[0] += 1.0; + amb->LC [0] += (LC1 -amb->LC [0])/amb->n[0]; + amb->LCv[0] += (var1-amb->LCv[0])/amb->n[0]; + } + if (LC2) + { + amb->n[1] += 1.0; + amb->LC [1] += (LC2 -amb->LC [1])/amb->n[1]; + amb->LCv[1] += (var2-amb->LCv[1])/amb->n[1]; + } + if (LC3) + { + amb->n[2] += 1.0; + amb->LC [2] += (LC3 -amb->LC [2])/amb->n[2]; + amb->LCv[2] += (var3-amb->LCv[2])/amb->n[2]; + } + amb->epoch[0] = obs[0].time; } - /* averaging */ - if (LC1) { - amb->n[0]+=1.0; - amb->LC [0]+=(LC1 -amb->LC [0])/amb->n[0]; - amb->LCv[0]+=(var1-amb->LCv[0])/amb->n[0]; - } - if (LC2) { - amb->n[1]+=1.0; - amb->LC [1]+=(LC2 -amb->LC [1])/amb->n[1]; - amb->LCv[1]+=(var2-amb->LCv[1])/amb->n[1]; - } - if (LC3) { - amb->n[2]+=1.0; - amb->LC [2]+=(LC3 -amb->LC [2])/amb->n[2]; - amb->LCv[2]+=(var3-amb->LCv[2])/amb->n[2]; - } - amb->epoch[0]=obs[0].time; - } } -/* fix wide-lane ambiguity ---------------------------------------------------*/ - int fix_amb_WL(rtk_t *rtk, const nav_t *nav, int sat1, int sat2, int *NW) -{ - ambc_t *amb1,*amb2; - double BW,vW,lam_WL=lam_LC(1,-1,0); - amb1=rtk->ambc+sat1-1; - amb2=rtk->ambc+sat2-1; - if (!amb1->n[0]||!amb2->n[0]) return 0; + +/* fix wide-lane ambiguity ---------------------------------------------------*/ +int fix_amb_WL(rtk_t *rtk, const nav_t *nav, int sat1, int sat2, int *NW) +{ + ambc_t *amb1, *amb2; + double BW, vW, lam_WL = lam_LC(1, -1, 0); + + amb1 = rtk->ambc+sat1-1; + amb2 = rtk->ambc+sat2-1; + if (!amb1->n[0] || !amb2->n[0]) return 0; /* wide-lane ambiguity */ #ifndef REV_WL_FCB - BW=(amb1->LC[0]-amb2->LC[0])/lam_WL+nav->wlbias[sat1-1]-nav->wlbias[sat2-1]; + BW = (amb1->LC[0]-amb2->LC[0])/lam_WL+nav->wlbias[sat1-1]-nav->wlbias[sat2-1]; #else - BW=(amb1->LC[0]-amb2->LC[0])/lam_WL-nav->wlbias[sat1-1]+nav->wlbias[sat2-1]; + BW = (amb1->LC[0]-amb2->LC[0])/lam_WL-nav->wlbias[sat1-1]+nav->wlbias[sat2-1]; #endif - *NW=ROUND(BW); + *NW = ROUND_PPP(BW); /* variance of wide-lane ambiguity */ - vW=(amb1->LCv[0]/amb1->n[0]+amb2->LCv[0]/amb2->n[0])/SQR(lam_WL); + vW = (amb1->LCv[0]/amb1->n[0]+amb2->LCv[0]/amb2->n[0])/std::pow(lam_WL, 2.0); /* validation of integer wide-lane ambigyity */ - return fabs(*NW-BW)<=rtk->opt.thresar[2]&& - conffunc(*NW,BW,sqrt(vW))>=rtk->opt.thresar[1]; + return fabs(*NW-BW) <= rtk->opt.thresar[2] && + conffunc(*NW, BW, sqrt(vW)) >= rtk->opt.thresar[1]; } + + /* linear dependency check ---------------------------------------------------*/ - int is_depend(int sat1, int sat2, int *flgs, int *max_flg) +int is_depend(int sat1, int sat2, int *flgs, int *max_flg) { int i; - if (flgs[sat1-1]==0&&flgs[sat2-1]==0) { - flgs[sat1-1]=flgs[sat2-1]=++(*max_flg); - } - else if (flgs[sat1-1]==0&&flgs[sat2-1]!=0) { - flgs[sat1-1]=flgs[sat2-1]; - } - else if (flgs[sat1-1]!=0&&flgs[sat2-1]==0) { - flgs[sat2-1]=flgs[sat1-1]; - } - else if (flgs[sat1-1]>flgs[sat2-1]) { - for (i=0;iflgs[sat2-1]) + { + for (i = 0;i=var[j-1]) continue; - SWAP_I(sat1[j],sat1[j-1]); - SWAP_I(sat2[j],sat2[j-1]); - SWAP_D(N[j],N[j-1]); - SWAP_D(var[j],var[j-1]); - } + for (i = 0;i= var[j-1]) continue; + SWAP_I(sat1[j], sat1[j-1]); + SWAP_I(sat2[j], sat2[j-1]); + SWAP_D(N[j], N[j-1]); + SWAP_D(var[j], var[j-1]); + } /* select linearly independent satellite pair */ - for (i=j=0;inx,n); R=zeros(n,n); + v = zeros(n, 1); H = zeros(rtk->nx, n); R = zeros(n, n); /* constraints to fixed ambiguities */ - for (i=0;iopt); - k=IB(sat2[i],&rtk->opt); - v[i]=NC[i]-(rtk->x[j]-rtk->x[k]); - H[j+i*rtk->nx]= 1.0; - H[k+i*rtk->nx]=-1.0; - R[i+i*n]=SQR(CONST_AMB); - } - /* update states with constraints */ - if ((info=filter(rtk->x,rtk->P,H,v,R,rtk->nx,n))) { - trace(1,"filter error (info=%d)\n",info); - free(v); free(H); free(R); - return 0; - } - /* set solution */ - for (i=0;ina;i++) { - rtk->xa[i]=rtk->x[i]; - for (j=0;jna;j++) { - rtk->Pa[i+j*rtk->na]=rtk->Pa[j+i*rtk->na]=rtk->P[i+j*rtk->nx]; + for (i = 0;iopt); + k = IB_PPP(sat2[i], &rtk->opt); + v[i] = NC[i]-(rtk->x[j]-rtk->x[k]); + H[j+i*rtk->nx] = 1.0; + H[k+i*rtk->nx] = -1.0; + R[i+i*n] = std::pow(CONST_AMB, 2.0); + } + /* update states with constraints */ + if ((info = filter(rtk->x, rtk->P, H, v, R, rtk->nx, n))) + { + trace(1, "filter error (info=%d)\n", info); + free(v); free(H); free(R); + return 0; + } + /* set solution */ + for (i = 0;ina;i++) + { + rtk->xa[i] = rtk->x[i]; + for (j = 0;jna;j++) + { + rtk->Pa[i+j*rtk->na] = rtk->Pa[j+i*rtk->na] = rtk->P[i+j*rtk->nx]; + } } - } /* set flags */ - for (i=0;iambc[sat1[i]-1].flags[sat2[i]-1]=1; - rtk->ambc[sat2[i]-1].flags[sat1[i]-1]=1; - } + for (i = 0;iambc[sat1[i]-1].flags[sat2[i]-1] = 1; + rtk->ambc[sat2[i]-1].flags[sat1[i]-1] = 1; + } free(v); free(H); free(R); return 1; } + + /* fix narrow-lane ambiguity by rounding -------------------------------------*/ - int fix_amb_ROUND(rtk_t *rtk, int *sat1, int *sat2, const int *NW, int n) +int fix_amb_ROUND(rtk_t *rtk, int *sat1, int *sat2, const int *NW, int n) { - double C1,C2,B1,v1,BC,v,vc,*NC,*var,lam_NL=lam_LC(1,1,0),lam1,lam2; - int i,j,k,m=0,N1,stat; + double C1, C2, B1, v1, BC, v, vc, *NC, *var, lam_NL = lam_LC(1, 1, 0), lam1, lam2; + int i, j, k, m = 0, N1, stat; - lam1=lam_carr[0]; lam2=lam_carr[1]; + lam1 = lam_carr[0]; lam2 = lam_carr[1]; - C1= SQR(lam2)/(SQR(lam2)-SQR(lam1)); - C2=-SQR(lam1)/(SQR(lam2)-SQR(lam1)); + 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)); - NC=zeros(n,1); var=zeros(n,1); + NC = zeros(n, 1); var = zeros(n, 1); - for (i=0;iopt); - k=IB(sat2[i],&rtk->opt); + for (i = 0;iopt); + k = IB_PPP(sat2[i], &rtk->opt); - /* narrow-lane ambiguity */ - B1=(rtk->x[j]-rtk->x[k]+C2*lam2*NW[i])/lam_NL; - N1=ROUND(B1); + /* narrow-lane ambiguity */ + B1 = (rtk->x[j]-rtk->x[k]+C2*lam2*NW[i])/lam_NL; + N1 = ROUND_PPP(B1); - /* variance of narrow-lane ambiguity */ - var[m]=rtk->P[j+j*rtk->nx]+rtk->P[k+k*rtk->nx]-2.0*rtk->P[j+k*rtk->nx]; - v1=var[m]/SQR(lam_NL); + /* variance of narrow-lane ambiguity */ + var[m] = rtk->P[j+j*rtk->nx]+rtk->P[k+k*rtk->nx]-2.0*rtk->P[j+k*rtk->nx]; + v1 = var[m]/std::pow(lam_NL, 2.0); - /* validation of narrow-lane ambiguity */ - if (fabs(N1-B1)>rtk->opt.thresar[2]|| - conffunc(N1,B1,sqrt(v1))opt.thresar[1]) { - continue; + /* validation of narrow-lane ambiguity */ + if (fabs(N1-B1)>rtk->opt.thresar[2] || + conffunc(N1, B1, sqrt(v1))opt.thresar[1]) + { + continue; + } + /* iono-free ambiguity (m) */ + BC = C1*lam1*N1+C2*lam2*(N1-NW[i]); + + /* check residuals */ + v = rtk->ssat[sat1[i]-1].resc[0]-rtk->ssat[sat2[i]-1].resc[0]; + vc = v+(BC-(rtk->x[j]-rtk->x[k])); + if (fabs(vc)>THRES_RES) continue; + + sat1[m] = sat1[i]; + sat2[m] = sat2[i]; + NC[m++] = BC; } - /* iono-free ambiguity (m) */ - BC=C1*lam1*N1+C2*lam2*(N1-NW[i]); - - /* check residuals */ - v=rtk->ssat[sat1[i]-1].resc[0]-rtk->ssat[sat2[i]-1].resc[0]; - vc=v+(BC-(rtk->x[j]-rtk->x[k])); - if (fabs(vc)>THRES_RES) continue; - - sat1[m]=sat1[i]; - sat2[m]=sat2[i]; - NC[m++]=BC; - } /* select fixed ambiguities by dependancy check */ - m=sel_amb(sat1,sat2,NC,var,m); + m = sel_amb(sat1, sat2, NC, var, m); /* fixed solution */ - stat=fix_sol(rtk,sat1,sat2,NC,m); + stat = fix_sol(rtk, sat1, sat2, NC, m); free(NC); free(var); - return stat&&m>=3; + return stat && m >= 3; } + + /* fix narrow-lane ambiguity by ILS ------------------------------------------*/ - int fix_amb_ILS(rtk_t *rtk, int *sat1, int *sat2, int *NW, int n) +int fix_amb_ILS(rtk_t *rtk, int *sat1, int *sat2, int *NW, int n) { - double C1,C2,*B1,*N1,*NC,*D,*E,*Q,s[2],lam_NL=lam_LC(1,1,0),lam1,lam2; - int i,j,k,m=0,info,stat,flgs[MAXSAT]={0},max_flg=0; + double C1, C2, *B1, *N1, *NC, *D, *E, *Q, s[2], lam_NL = lam_LC(1, 1, 0), lam1, lam2; + int i, j, k, m = 0, info, stat, flgs[MAXSAT] = {0}, max_flg = 0; - lam1=lam_carr[0]; lam2=lam_carr[1]; + lam1 = lam_carr[0]; lam2 = lam_carr[1]; - C1= SQR(lam2)/(SQR(lam2)-SQR(lam1)); - C2=-SQR(lam1)/(SQR(lam2)-SQR(lam1)); + 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)); - B1=zeros(n,1); N1=zeros(n,2); D=zeros(rtk->nx,n); E=mat(n,rtk->nx); - Q=mat(n,n); NC=mat(n,1); + B1 = zeros(n, 1); N1 = zeros(n, 2); D = zeros(rtk->nx, n); E = mat(n, rtk->nx); + Q = mat(n, n); NC = mat(n, 1); - for (i=0;iopt); + k = IB_PPP(sat2[i], &rtk->opt); - j=IB(sat1[i],&rtk->opt); - k=IB(sat2[i],&rtk->opt); + /* float narrow-lane ambiguity (cycle) */ + B1[m] = (rtk->x[j]-rtk->x[k]+C2*lam2*NW[i])/lam_NL; + N1[m] = ROUND_PPP(B1[m]); - /* float narrow-lane ambiguity (cycle) */ - B1[m]=(rtk->x[j]-rtk->x[k]+C2*lam2*NW[i])/lam_NL; - N1[m]=ROUND(B1[m]); + /* validation of narrow-lane ambiguity */ + if (fabs(N1[m]-B1[m])>rtk->opt.thresar[2]) continue; - /* validation of narrow-lane ambiguity */ - if (fabs(N1[m]-B1[m])>rtk->opt.thresar[2]) continue; + /* narrow-lane ambiguity transformation matrix */ + D[j+m*rtk->nx] = 1.0/lam_NL; + D[k+m*rtk->nx] = -1.0/lam_NL; - /* narrow-lane ambiguity transformation matrix */ - D[j+m*rtk->nx]= 1.0/lam_NL; - D[k+m*rtk->nx]=-1.0/lam_NL; - - sat1[m]=sat1[i]; - sat2[m]=sat2[i]; - NW[m++]=NW[i]; - } + sat1[m] = sat1[i]; + sat2[m] = sat2[i]; + NW[m++] = NW[i]; + } if (m<3) return 0; /* covariance of narrow-lane ambiguities */ - matmul("TN",m,rtk->nx,rtk->nx,1.0,D,rtk->P,0.0,E); - matmul("NN",m,m,rtk->nx,1.0,E,D,0.0,Q); + matmul("TN", m, rtk->nx, rtk->nx, 1.0, D, rtk->P, 0.0, E); + matmul("NN", m, m, rtk->nx, 1.0, E, D, 0.0, Q); /* integer least square */ - if ((info=lambda(m,2,B1,Q,N1,s))) { - trace(2,"lambda error: info=%d\n",info); - return 0; - } - if (s[0]<=0.0) return 0; + if ((info = lambda(m, 2, B1, Q, N1, s))) + { + trace(2, "lambda error: info=%d\n", info); + return 0; + } + if (s[0] <= 0.0) return 0; - rtk->sol.ratio=(float)(MIN(s[1]/s[0],999.9)); + rtk->sol.ratio = (float)(MIN_PPP(s[1]/s[0], 999.9)); /* varidation by ratio-test */ - if (rtk->opt.thresar[0]>0.0&&rtk->sol.ratioopt.thresar[0]) { - trace(2,"varidation error: n=%2d ratio=%8.3f\n",m,rtk->sol.ratio); - return 0; - } - trace(2,"varidation ok: %s n=%2d ratio=%8.3f\n",time_str(rtk->sol.time,0),m, - rtk->sol.ratio); + if (rtk->opt.thresar[0]>0.0 && rtk->sol.ratioopt.thresar[0]) + { + trace(2, "varidation error: n=%2d ratio=%8.3f\n", m, rtk->sol.ratio); + return 0; + } + trace(2, "varidation ok: %s n=%2d ratio=%8.3f\n", time_str(rtk->sol.time, 0), m, + rtk->sol.ratio); /* narrow-lane to iono-free ambiguity */ - for (i=0;iopt.ionoopt!=IONOOPT_IFLC||rtk->opt.nf<2) return 0; + if (n <= 0 || rtk->opt.ionoopt != IONOOPT_IFLC || rtk->opt.nf<2) return 0; - trace(3,"pppamb: time=%s n=%d\n",time_str(obs[0].time,0),n); + trace(3, "pppamb: time=%s n=%d\n", time_str(obs[0].time, 0), n); - elmask=rtk->opt.elmaskar>0.0?rtk->opt.elmaskar:rtk->opt.elmin; + elmask = rtk->opt.elmaskar>0.0?rtk->opt.elmaskar:rtk->opt.elmin; - sat1=imat(n*n,1); sat2=imat(n*n,1); NW=imat(n*n,1); + sat1 = imat(n*n, 1); sat2 = imat(n*n, 1); NW = imat(n*n, 1); /* average LC */ - average_LC(rtk,obs,n,nav,azel); + average_LC(rtk, obs, n, nav, azel); /* fix wide-lane ambiguity */ - for (i=0;issat[obs[i].sat-1].vsat[0]|| - !rtk->ssat[obs[j].sat-1].vsat[0]|| - azel[1+i*2]ssat[obs[i].sat-1].vsat[0] || + !rtk->ssat[obs[j].sat-1].vsat[0] || + azel[1+i*2]ambc[obs[i].sat-1].flags[obs[j].sat-1]&& - rtk->ambc[obs[j].sat-1].flags[obs[i].sat-1]) continue; + /* test already fixed */ + if (rtk->ambc[obs[i].sat-1].flags[obs[j].sat-1] && + rtk->ambc[obs[j].sat-1].flags[obs[i].sat-1]) continue; #endif - sat1[m]=obs[i].sat; - sat2[m]=obs[j].sat; - if (fix_amb_WL(rtk,nav,sat1[m],sat2[m],NW+m)) m++; - } + sat1[m] = obs[i].sat; + sat2[m] = obs[j].sat; + if (fix_amb_WL(rtk, nav, sat1[m], sat2[m], NW+m)) m++; + } /* fix narrow-lane ambiguity */ - if (rtk->opt.modear==ARMODE_PPPAR) { - stat=fix_amb_ROUND(rtk,sat1,sat2,NW,m); - } - else if (rtk->opt.modear==ARMODE_PPPAR_ILS) { - stat=fix_amb_ILS(rtk,sat1,sat2,NW,m); - } + if (rtk->opt.modear == ARMODE_PPPAR) + { + stat = fix_amb_ROUND(rtk, sat1, sat2, NW, m); + } + else if (rtk->opt.modear == ARMODE_PPPAR_ILS) + { + stat = fix_amb_ILS(rtk, sat1, sat2, NW, m); + } free(sat1); free(sat2); free(NW); return stat; @@ -494,737 +551,831 @@ int pppamb(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, void pppoutsolstat(rtk_t *rtk, int level, FILE *fp) { ssat_t *ssat; - double tow,pos[3],vel[3],acc[3]; - int i,j,week,nfreq=1; + double tow, pos[3], vel[3], acc[3]; + int i, j, week, nfreq = 1; char id[32]; - if (level<=0||!fp) return; + if (level <= 0 || !fp) return; - trace(3,"pppoutsolstat:\n"); + trace(3, "pppoutsolstat:\n"); - tow=time2gpst(rtk->sol.time,&week); + tow = time2gpst(rtk->sol.time, &week); /* receiver position */ - fprintf(fp,"$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],0.0,0.0,0.0); + fprintf(fp, "$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], 0.0, 0.0, 0.0); /* receiver velocity and acceleration */ - if (rtk->opt.dynamics) { - ecef2pos(rtk->sol.rr,pos); - ecef2enu(pos,rtk->x+3,vel); - ecef2enu(pos,rtk->x+6,acc); - fprintf(fp,"$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], - 0.0,0.0,0.0,0.0,0.0,0.0); - } + if (rtk->opt.dynamics) + { + ecef2pos(rtk->sol.rr, pos); + ecef2enu(pos, rtk->x+3, vel); + ecef2enu(pos, rtk->x+6, acc); + fprintf(fp, "$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], + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + } /* receiver clocks */ - i=IC(0,&rtk->opt); - fprintf(fp,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", - week,tow,rtk->sol.stat,1,rtk->x[i]*1E9/SPEED_OF_LIGHT,rtk->x[i+1]*1E9/SPEED_OF_LIGHT, - 0.0,0.0); + i = IC_PPP(0, &rtk->opt); + fprintf(fp, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", + week, tow, rtk->sol.stat, 1, rtk->x[i]*1E9/SPEED_OF_LIGHT, rtk->x[i+1]*1E9/SPEED_OF_LIGHT, + 0.0, 0.0); /* tropospheric parameters */ - if (rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG) { - i=IT_PPP(&rtk->opt); - fprintf(fp,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat, - 1,rtk->x[i],0.0); - } - if (rtk->opt.tropopt==TROPOPT_ESTG) { - i=IT_PPP(&rtk->opt); - fprintf(fp,"$TRPG,%d,%.3f,%d,%d,%.5f,%.5f,%.5f,%.5f\n",week,tow, - rtk->sol.stat,1,rtk->x[i+1],rtk->x[i+2],0.0,0.0); - } - if (rtk->sol.stat==SOLQ_NONE||level<=1) return; + if (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG) + { + i = IT_PPP(&rtk->opt); + fprintf(fp, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, + 1, rtk->x[i], 0.0); + } + if (rtk->opt.tropopt == TROPOPT_ESTG) + { + i = IT_PPP(&rtk->opt); + fprintf(fp, "$TRPG,%d,%.3f,%d,%d,%.5f,%.5f,%.5f,%.5f\n", week, tow, + rtk->sol.stat, 1, rtk->x[i+1], rtk->x[i+2], 0.0, 0.0); + } + if (rtk->sol.stat == SOLQ_NONE || level <= 1) return; /* residuals and status */ - for (i=0;issat+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]); + for (i = 0;issat+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]); + } } - } } + + /* solar/lunar tides (ref [2] 7) ---------------------------------------------*/ - void tide_pl(const double *eu, const double *rp, double GMp, - const double *pos, double *dr) +void tide_pl(const double *eu, const double *rp, double GMp, + const double *pos, double *dr) { - const double H3=0.292,L3=0.015; - double r,ep[3],latp,lonp,p,K2,K3,a,H2,L2,dp,du,cosp,sinl,cosl; + const double H3 = 0.292, L3 = 0.015; + double r, ep[3], latp, lonp, p, K2, K3, a, H2, L2, dp, du, cosp, sinl, cosl; int i; - trace(4,"tide_pl : pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D); + trace(4, "tide_pl : pos=%.3f %.3f\n", pos[0]*R2D, pos[1]*R2D); - if ((r=norm_rtk(rp,3))<=0.0) return; + if ((r = norm_rtk(rp, 3)) <= 0.0) return; - for (i=0;i<3;i++) ep[i]=rp[i]/r; + for (i = 0;i<3;i++) ep[i] = rp[i]/r; - K2=GMp/GME*SQR(RE_WGS84)*SQR(RE_WGS84)/(r*r*r); - K3=K2*RE_WGS84/r; - latp=asin(ep[2]); lonp=atan2(ep[1],ep[0]); - cosp=cos(latp); sinl=sin(pos[0]); cosl=cos(pos[0]); + K2 = GMp / GME * std::pow(RE_WGS84, 2.0) * std::pow(RE_WGS84, 2.0) / (r*r*r); + K3 = K2*RE_WGS84/r; + latp = asin(ep[2]); lonp = atan2(ep[1], ep[0]); + cosp = cos(latp); sinl = sin(pos[0]); cosl = cos(pos[0]); /* step1 in phase (degree 2) */ - p=(3.0*sinl*sinl-1.0)/2.0; - H2=0.6078-0.0006*p; - L2=0.0847+0.0002*p; - a=dot(ep,eu,3); - dp=K2*3.0*L2*a; - du=K2*(H2*(1.5*a*a-0.5)-3.0*L2*a*a); + p = (3.0*sinl*sinl-1.0)/2.0; + H2 = 0.6078-0.0006*p; + L2 = 0.0847+0.0002*p; + a = dot(ep, eu, 3); + dp = K2*3.0*L2*a; + du = K2*(H2*(1.5*a*a-0.5)-3.0*L2*a*a); /* step1 in phase (degree 3) */ - dp+=K3*L3*(7.5*a*a-1.5); - du+=K3*(H3*(2.5*a*a*a-1.5*a)-L3*(7.5*a*a-1.5)*a); + dp += K3*L3*(7.5*a*a-1.5); + du += K3*(H3*(2.5*a*a*a-1.5*a)-L3*(7.5*a*a-1.5)*a); /* step1 out-of-phase (only radial) */ - du+=3.0/4.0*0.0025*K2*sin(2.0*latp)*sin(2.0*pos[0])*sin(pos[1]-lonp); - du+=3.0/4.0*0.0022*K2*cosp*cosp*cosl*cosl*sin(2.0*(pos[1]-lonp)); + du += 3.0/4.0*0.0025*K2*sin(2.0*latp)*sin(2.0*pos[0])*sin(pos[1]-lonp); + du += 3.0/4.0*0.0022*K2*cosp*cosp*cosl*cosl*sin(2.0*(pos[1]-lonp)); - dr[0]=dp*ep[0]+du*eu[0]; - dr[1]=dp*ep[1]+du*eu[1]; - dr[2]=dp*ep[2]+du*eu[2]; + dr[0] = dp*ep[0]+du*eu[0]; + dr[1] = dp*ep[1]+du*eu[1]; + dr[2] = dp*ep[2]+du*eu[2]; - trace(5,"tide_pl : dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]); + trace(5, "tide_pl : dr=%.3f %.3f %.3f\n", dr[0], dr[1], dr[2]); } -/* displacement by solid earth tide (ref [2] 7) ------------------------------*/ - void tide_solid(const double *rsun, const double *rmoon, - const double *pos, const double *E, double gmst, int opt, - double *dr) -{ - double dr1[3],dr2[3],eu[3],du,dn,sinl,sin2l; - trace(3,"tide_solid: pos=%.3f %.3f opt=%d\n",pos[0]*R2D,pos[1]*R2D,opt); + +/* displacement by solid earth tide (ref [2] 7) ------------------------------*/ +void tide_solid(const double *rsun, const double *rmoon, + const double *pos, const double *E, double gmst, int opt, + double *dr) +{ + double dr1[3], dr2[3], eu[3], du, dn, sinl, sin2l; + + trace(3, "tide_solid: pos=%.3f %.3f opt=%d\n", pos[0]*R2D, pos[1]*R2D, opt); /* step1: time domain */ - eu[0]=E[2]; eu[1]=E[5]; eu[2]=E[8]; - tide_pl(eu,rsun, GMS,pos,dr1); - tide_pl(eu,rmoon,GMM,pos,dr2); + eu[0] = E[2]; eu[1] = E[5]; eu[2] = E[8]; + tide_pl(eu, rsun, GMS, pos, dr1); + tide_pl(eu, rmoon, GMM, pos, dr2); /* step2: frequency domain, only K1 radial */ - sin2l=sin(2.0*pos[0]); - du=-0.012*sin2l*sin(gmst+pos[1]); + sin2l = sin(2.0*pos[0]); + du = -0.012*sin2l*sin(gmst+pos[1]); - dr[0]=dr1[0]+dr2[0]+du*E[2]; - dr[1]=dr1[1]+dr2[1]+du*E[5]; - dr[2]=dr1[2]+dr2[2]+du*E[8]; + dr[0] = dr1[0]+dr2[0]+du*E[2]; + dr[1] = dr1[1]+dr2[1]+du*E[5]; + dr[2] = dr1[2]+dr2[2]+du*E[8]; /* eliminate permanent deformation */ - if (opt&8) { - sinl=sin(pos[0]); - du=0.1196*(1.5*sinl*sinl-0.5); - dn=0.0247*sin2l; - dr[0]+=du*E[2]+dn*E[1]; - dr[1]+=du*E[5]+dn*E[4]; - dr[2]+=du*E[8]+dn*E[7]; - } - trace(5,"tide_solid: dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]); + if (opt&8) + { + sinl = sin(pos[0]); + du = 0.1196*(1.5*sinl*sinl-0.5); + dn = 0.0247*sin2l; + dr[0] += du*E[2]+dn*E[1]; + dr[1] += du*E[5]+dn*E[4]; + dr[2] += du*E[8]+dn*E[7]; + } + trace(5, "tide_solid: dr=%.3f %.3f %.3f\n", dr[0], dr[1], dr[2]); } -/* displacement by ocean tide loading (ref [2] 7) ----------------------------*/ - void tide_oload(gtime_t tut, const double *odisp, double *denu) -{ - const double args[][5]={ - {1.40519E-4, 2.0,-2.0, 0.0, 0.00}, /* M2 */ - {1.45444E-4, 0.0, 0.0, 0.0, 0.00}, /* S2 */ - {1.37880E-4, 2.0,-3.0, 1.0, 0.00}, /* N2 */ - {1.45842E-4, 2.0, 0.0, 0.0, 0.00}, /* K2 */ - {0.72921E-4, 1.0, 0.0, 0.0, 0.25}, /* K1 */ - {0.67598E-4, 1.0,-2.0, 0.0,-0.25}, /* O1 */ - {0.72523E-4,-1.0, 0.0, 0.0,-0.25}, /* P1 */ - {0.64959E-4, 1.0,-3.0, 1.0,-0.25}, /* Q1 */ - {0.53234E-5, 0.0, 2.0, 0.0, 0.00}, /* Mf */ - {0.26392E-5, 0.0, 1.0,-1.0, 0.00}, /* Mm */ - {0.03982E-5, 2.0, 0.0, 0.0, 0.00} /* Ssa */ - }; - const double ep1975[]={1975,1,1,0,0,0}; - double ep[6],fday,days,t,t2,t3,a[5],ang,dp[3]={0}; - int i,j; - trace(3,"tide_oload:\n"); + +/* displacement by ocean tide loading (ref [2] 7) ----------------------------*/ +void tide_oload(gtime_t tut, const double *odisp, double *denu) +{ + const double args[][5] = { + {1.40519E-4, 2.0,-2.0, 0.0, 0.00}, /* M2 */ + {1.45444E-4, 0.0, 0.0, 0.0, 0.00}, /* S2 */ + {1.37880E-4, 2.0,-3.0, 1.0, 0.00}, /* N2 */ + {1.45842E-4, 2.0, 0.0, 0.0, 0.00}, /* K2 */ + {0.72921E-4, 1.0, 0.0, 0.0, 0.25}, /* K1 */ + {0.67598E-4, 1.0,-2.0, 0.0,-0.25}, /* O1 */ + {0.72523E-4,-1.0, 0.0, 0.0,-0.25}, /* P1 */ + {0.64959E-4, 1.0,-3.0, 1.0,-0.25}, /* Q1 */ + {0.53234E-5, 0.0, 2.0, 0.0, 0.00}, /* Mf */ + {0.26392E-5, 0.0, 1.0,-1.0, 0.00}, /* Mm */ + {0.03982E-5, 2.0, 0.0, 0.0, 0.00} /* Ssa */ + }; + const double ep1975[] = {1975, 1, 1, 0, 0, 0}; + double ep[6], fday, days, t, t2, t3, a[5], ang, dp[3] = {0}; + int i, j; + + trace(3, "tide_oload:\n"); /* angular argument: see subroutine arg.f for reference [1] */ - time2epoch(tut,ep); - fday=ep[3]*3600.0+ep[4]*60.0+ep[5]; - ep[3]=ep[4]=ep[5]=0.0; - days=timediff(epoch2time(ep),epoch2time(ep1975))/86400.0; - t=(27392.500528+1.000000035*days)/36525.0; - t2=t*t; t3=t2*t; + time2epoch(tut, ep); + fday = ep[3]*3600.0+ep[4]*60.0+ep[5]; + ep[3] = ep[4] = ep[5] = 0.0; + days = timediff(epoch2time(ep), epoch2time(ep1975))/86400.0; + t = (27392.500528+1.000000035*days)/36525.0; + t2 = t*t; t3 = t2*t; - a[0]=fday; - a[1]=(279.69668+36000.768930485*t+3.03E-4*t2)*D2R; /* H0 */ - a[2]=(270.434358+481267.88314137*t-0.001133*t2+1.9E-6*t3)*D2R; /* S0 */ - a[3]=(334.329653+4069.0340329577*t-0.010325*t2-1.2E-5*t3)*D2R; /* P0 */ - a[4]=2.0*PI; + a[0] = fday; + a[1] = (279.69668+36000.768930485*t+3.03E-4*t2)*D2R; /* H0 */ + a[2] = (270.434358+481267.88314137*t-0.001133*t2+1.9E-6*t3)*D2R; /* S0 */ + a[3] = (334.329653+4069.0340329577*t-0.010325*t2-1.2E-5*t3)*D2R; /* P0 */ + a[4] = 2.0*PI; /* displacements by 11 constituents */ - for (i=0;i<11;i++) { - ang=0.0; - for (j=0;j<5;j++) ang+=a[j]*args[i][j]; - for (j=0;j<3;j++) dp[j]+=odisp[j+i*6]*cos(ang-odisp[j+3+i*6]*D2R); - } - denu[0]=-dp[1]; - denu[1]=-dp[2]; - denu[2]= dp[0]; + for (i = 0;i<11;i++) + { + ang = 0.0; + for (j = 0;j<5;j++) ang += a[j]*args[i][j]; + for (j = 0;j<3;j++) dp[j] += odisp[j+i*6]*cos(ang-odisp[j+3+i*6]*D2R); + } + denu[0] = -dp[1]; + denu[1] = -dp[2]; + denu[2] = dp[0]; - trace(5,"tide_oload: denu=%.3f %.3f %.3f\n",denu[0],denu[1],denu[2]); + trace(5, "tide_oload: denu=%.3f %.3f %.3f\n", denu[0], denu[1], denu[2]); } + + /* iers mean pole (ref [7] eq.7.25) ------------------------------------------*/ - void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar) +void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar) { - const double ep2000[]={2000,1,1,0,0,0}; - double y,y2,y3; + const double ep2000[] = {2000, 1, 1, 0, 0, 0}; + double y, y2, y3; - y=timediff(tut,epoch2time(ep2000))/86400.0/365.25; + y = timediff(tut, epoch2time(ep2000))/86400.0/365.25; - if (y<3653.0/365.25) { /* until 2010.0 */ - y2=y*y; y3=y2*y; - *xp_bar= 55.974+1.8243*y+0.18413*y2+0.007024*y3; /* (mas) */ - *yp_bar=346.346+1.7896*y-0.10729*y2-0.000908*y3; - } - else { /* after 2010.0 */ - *xp_bar= 23.513+7.6141*y; /* (mas) */ - *yp_bar=358.891-0.6287*y; - } + if (y<3653.0/365.25) + { /* until 2010.0 */ + y2 = y*y; y3 = y2*y; + *xp_bar = 55.974+1.8243*y+0.18413*y2+0.007024*y3; /* (mas) */ + *yp_bar = 346.346+1.7896*y-0.10729*y2-0.000908*y3; + } + else + { /* after 2010.0 */ + *xp_bar = 23.513+7.6141*y; /* (mas) */ + *yp_bar = 358.891-0.6287*y; + } } -/* displacement by pole tide (ref [7] eq.7.26) --------------------------------*/ - void tide_pole(gtime_t tut, const double *pos, const double *erpv, - double *denu) -{ - double xp_bar,yp_bar,m1,m2,cosl,sinl; - trace(3,"tide_pole: pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D); + +/* displacement by pole tide (ref [7] eq.7.26) --------------------------------*/ +void tide_pole(gtime_t tut, const double *pos, const double *erpv, + double *denu) +{ + double xp_bar, yp_bar, m1, m2, cosl, sinl; + + trace(3, "tide_pole: pos=%.3f %.3f\n", pos[0]*R2D, pos[1]*R2D); /* iers mean pole (mas) */ - iers_mean_pole(tut,&xp_bar,&yp_bar); + iers_mean_pole(tut, &xp_bar, &yp_bar); /* ref [7] eq.7.24 */ - m1= erpv[0]/AS2R-xp_bar*1E-3; /* (as) */ - m2=-erpv[1]/AS2R+yp_bar*1E-3; + m1 = erpv[0]/AS2R-xp_bar*1E-3; /* (as) */ + m2 = -erpv[1]/AS2R+yp_bar*1E-3; /* sin(2*theta) = sin(2*phi), cos(2*theta)=-cos(2*phi) */ - cosl=cos(pos[1]); - sinl=sin(pos[1]); - denu[0]= 9E-3*sin(pos[0]) *(m1*sinl-m2*cosl); /* de= Slambda (m) */ - denu[1]= -9E-3*cos(2.0*pos[0])*(m1*cosl+m2*sinl); /* dn=-Stheta (m) */ - denu[2]=-33E-3*sin(2.0*pos[0])*(m1*cosl+m2*sinl); /* du= Sr (m) */ + cosl = cos(pos[1]); + sinl = sin(pos[1]); + denu[0] = 9E-3*sin(pos[0]) *(m1*sinl-m2*cosl); /* de= Slambda (m) */ + denu[1] = -9E-3*cos(2.0*pos[0])*(m1*cosl+m2*sinl); /* dn=-Stheta (m) */ + denu[2] = -33E-3*sin(2.0*pos[0])*(m1*cosl+m2*sinl); /* du= Sr (m) */ - trace(5,"tide_pole : denu=%.3f %.3f %.3f\n",denu[0],denu[1],denu[2]); + trace(5, "tide_pole : denu=%.3f %.3f %.3f\n", denu[0], denu[1], denu[2]); } + + /* tidal displacement ---------------------------------------------------------- -* displacements by earth tides -* args : gtime_t tutc I time in utc -* double *rr I site position (ecef) (m) -* int opt I options (or of the followings) -* 1: solid earth tide -* 2: ocean tide loading -* 4: pole tide -* 8: elimate permanent deformation -* double *erp I earth rotation parameters (NULL: not used) -* double *odisp I ocean loading parameters (NULL: not used) -* odisp[0+i*6]: consituent i amplitude radial(m) -* odisp[1+i*6]: consituent i amplitude west (m) -* odisp[2+i*6]: consituent i amplitude south (m) -* odisp[3+i*6]: consituent i phase radial (deg) -* odisp[4+i*6]: consituent i phase west (deg) -* odisp[5+i*6]: consituent i phase south (deg) -* (i=0:M2,1:S2,2:N2,3:K2,4:K1,5:O1,6:P1,7:Q1, -* 8:Mf,9:Mm,10:Ssa) -* double *dr O displacement by earth tides (ecef) (m) -* return : none -* notes : see ref [1], [2] chap 7 -* see ref [4] 5.2.1, 5.2.2, 5.2.3 -* ver.2.4.0 does not use ocean loading and pole tide corrections -*-----------------------------------------------------------------------------*/ - void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp, - const double *odisp, double *dr) + * displacements by earth tides + * args : gtime_t tutc I time in utc + * double *rr I site position (ecef) (m) + * int opt I options (or of the followings) + * 1: solid earth tide + * 2: ocean tide loading + * 4: pole tide + * 8: elimate permanent deformation + * double *erp I earth rotation parameters (NULL: not used) + * double *odisp I ocean loading parameters (NULL: not used) + * odisp[0+i*6]: consituent i amplitude radial(m) + * odisp[1+i*6]: consituent i amplitude west (m) + * odisp[2+i*6]: consituent i amplitude south (m) + * odisp[3+i*6]: consituent i phase radial (deg) + * odisp[4+i*6]: consituent i phase west (deg) + * odisp[5+i*6]: consituent i phase south (deg) + * (i=0:M2,1:S2,2:N2,3:K2,4:K1,5:O1,6:P1,7:Q1, + * 8:Mf,9:Mm,10:Ssa) + * double *dr O displacement by earth tides (ecef) (m) + * return : none + * notes : see ref [1], [2] chap 7 + * see ref [4] 5.2.1, 5.2.2, 5.2.3 + * ver.2.4.0 does not use ocean loading and pole tide corrections + *-----------------------------------------------------------------------------*/ +void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp, + const double *odisp, double *dr) { gtime_t tut; - double pos[2],E[9],drt[3],denu[3],rs[3],rm[3],gmst,erpv[5]={0}; + double pos[2], E[9], drt[3], denu[3], rs[3], rm[3], gmst, erpv[5] = {0}; int i; #ifdef IERS_MODEL - double ep[6],fhr; - int year,mon,day; + double ep[6], fhr; + int year, mon, day; #endif - trace(3,"tidedisp: tutc=%s\n",time_str(tutc,0)); + trace(3, "tidedisp: tutc=%s\n", time_str(tutc, 0)); - if (erp) geterp(erp,tutc,erpv); + if (erp) geterp(erp, tutc, erpv); - tut=timeadd(tutc,erpv[2]); + tut = timeadd(tutc, erpv[2]); - dr[0]=dr[1]=dr[2]=0.0; + dr[0] = dr[1] = dr[2] = 0.0; - if (norm_rtk(rr,3)<=0.0) return; + if (norm_rtk(rr, 3) <= 0.0) return; - pos[0]=asin(rr[2]/norm_rtk(rr,3)); - pos[1]=atan2(rr[1],rr[0]); - xyz2enu(pos,E); + pos[0] = asin(rr[2]/norm_rtk(rr, 3)); + pos[1] = atan2(rr[1], rr[0]); + xyz2enu(pos, E); - if (opt&1) { /* solid earth tides */ + if (opt&1) + { /* solid earth tides */ - /* sun and moon position in ecef */ - sunmoonpos(tutc,erpv,rs,rm,&gmst); + /* sun and moon position in ecef */ + sunmoonpos(tutc, erpv, rs, rm, &gmst); #ifdef IERS_MODEL - time2epoch(tutc,ep); - year=(int)ep[0]; - mon =(int)ep[1]; - day =(int)ep[2]; - fhr =ep[3]+ep[4]/60.0+ep[5]/3600.0; + time2epoch(tutc, ep); + year = (int)ep[0]; + mon = (int)ep[1]; + day = (int)ep[2]; + fhr = ep[3]+ep[4]/60.0+ep[5]/3600.0; - /* call DEHANTTIDEINEL */ - dehanttideinel_((double *)rr,&year,&mon,&day,&fhr,rs,rm,drt); + /* call DEHANTTIDEINEL */ + dehanttideinel_((double *)rr, &year, &mon, &day, &fhr, rs, rm, drt); #else - tide_solid(rs,rm,pos,E,gmst,opt,drt); + tide_solid(rs, rm, pos, E, gmst, opt, drt); #endif - for (i=0;i<3;i++) dr[i]+=drt[i]; - } - if ((opt&2)&&odisp) { /* ocean tide loading */ - tide_oload(tut,odisp,denu); - matmul("TN",3,1,3,1.0,E,denu,0.0,drt); - for (i=0;i<3;i++) dr[i]+=drt[i]; - } - if ((opt&4)&&erp) { /* pole tide */ - tide_pole(tut,pos,erpv,denu); - matmul("TN",3,1,3,1.0,E,denu,0.0,drt); - for (i=0;i<3;i++) dr[i]+=drt[i]; - } - trace(5,"tidedisp: dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]); + for (i = 0;i<3;i++) dr[i] += drt[i]; + } + if ((opt&2) && odisp) + { /* ocean tide loading */ + tide_oload(tut, odisp, denu); + matmul("TN", 3, 1, 3, 1.0, E, denu, 0.0, drt); + for (i = 0;i<3;i++) dr[i] += drt[i]; + } + if ((opt&4) && erp) + { /* pole tide */ + tide_pole(tut, pos, erpv, denu); + matmul("TN", 3, 1, 3, 1.0, E, denu, 0.0, drt); + for (i = 0;i<3;i++) dr[i] += drt[i]; + } + trace(5, "tidedisp: dr=%.3f %.3f %.3f\n", dr[0], dr[1], dr[2]); } + + /* exclude meas of eclipsing satellite (block IIA) ---------------------------*/ - void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs) +void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs) { - double rsun[3],esun[3],r,ang,erpv[5]={0},cosa; - int i,j; + double rsun[3], esun[3], r, ang, erpv[5] = {0}, cosa; + int i, j; const char *type; - trace(3,"testeclipse:\n"); + trace(3, "testeclipse:\n"); /* unit vector of sun direction (ecef) */ - sunmoonpos(gpst2utc(obs[0].time),erpv,rsun,NULL,NULL); - normv3(rsun,esun); + sunmoonpos(gpst2utc(obs[0].time), erpv, rsun, NULL, NULL); + normv3(rsun, esun); - for (i=0;ipcvs[obs[i].sat-1].type; + for (i = 0;ipcvs[obs[i].sat-1].type; - if ((r=norm_rtk(rs+i*6,3))<=0.0) continue; + if ((r = norm_rtk(rs+i*6, 3)) <= 0.0) continue; #if 1 - /* only block IIA */ - if (*type&&!strstr(type,"BLOCK IIA")) continue; + /* only block IIA */ + if (*type && !strstr(type, "BLOCK IIA")) continue; #endif - /* sun-earth-satellite angle */ - cosa=dot(rs+i*6,esun,3)/r; - cosa=cosa<-1.0?-1.0:(cosa>1.0?1.0:cosa); - ang=acos(cosa); + /* sun-earth-satellite angle */ + cosa = dot(rs+i*6, esun, 3)/r; + cosa = cosa<-1.0?-1.0:(cosa>1.0?1.0:cosa); + ang = acos(cosa); - /* test eclipse */ - if (angRE_WGS84) continue; + /* test eclipse */ + if (angRE_WGS84) continue; - trace(2,"eclipsing sat excluded %s sat=%2d\n",time_str(obs[0].time,0), - obs[i].sat); + trace(2, "eclipsing sat excluded %s sat=%2d\n", time_str(obs[0].time, 0), + obs[i].sat); - for (j=0;j<3;j++) rs[j+i*6]=0.0; - } + for (j = 0;j<3;j++) rs[j+i*6] = 0.0; + } } + + /* measurement error variance ------------------------------------------------*/ - double varerr(int sat, int sys, double el, int type, const prcopt_t *opt) +double varerr(int sat, int sys, double el, int type, const prcopt_t *opt) { - double a,b,a2,b2,fact=1.0; - double sinel=sin(el); - int i=sys==SYS_GLO?1:(sys==SYS_GAL?2:0); + double a, b, a2, b2, fact = 1.0; + double sinel = sin(el); + int i = sys == SYS_GLO?1:(sys == SYS_GAL?2:0); /* extended error model */ - if (type==1&&opt->exterr.ena[0]) { /* code */ - a=opt->exterr.cerr[i][0]; - b=opt->exterr.cerr[i][1]; - if (opt->ionoopt==IONOOPT_IFLC) { - a2=opt->exterr.cerr[i][2]; - b2=opt->exterr.cerr[i][3]; - a=sqrt(SQR(2.55)*a*a+SQR(1.55)*a2*a2); - b=sqrt(SQR(2.55)*b*b+SQR(1.55)*b2*b2); + if (type == 1 && opt->exterr.ena[0]) + { /* code */ + a = opt->exterr.cerr[i][0]; + b = opt->exterr.cerr[i][1]; + if (opt->ionoopt == IONOOPT_IFLC) + { + a2 = opt->exterr.cerr[i][2]; + b2 = opt->exterr.cerr[i][3]; + a = std::sqrt(std::pow(2.55, 2.0)*a*a+std::pow(1.55, 2.0)*a2*a2); + b = std::sqrt(std::pow(2.55, 2.0)*b*b+std::pow(1.55, 2.0)*b2*b2); + } } - } - else if (type==0&&opt->exterr.ena[1]) { /* phase */ - a=opt->exterr.perr[i][0]; - b=opt->exterr.perr[i][1]; - if (opt->ionoopt==IONOOPT_IFLC) { - a2=opt->exterr.perr[i][2]; - b2=opt->exterr.perr[i][3]; - a=sqrt(SQR(2.55)*a*a+SQR(1.55)*a2*a2); - b=sqrt(SQR(2.55)*b*b+SQR(1.55)*b2*b2); + else if (type == 0 && opt->exterr.ena[1]) + { /* phase */ + a = opt->exterr.perr[i][0]; + b = opt->exterr.perr[i][1]; + if (opt->ionoopt == IONOOPT_IFLC) + { + a2 = opt->exterr.perr[i][2]; + b2 = opt->exterr.perr[i][3]; + a = std::sqrt(std::pow(2.55, 2.0)*a*a+std::pow(1.55, 2.0)*a2*a2); + b = std::sqrt(std::pow(2.55, 2.0)*b*b+std::pow(1.55, 2.0)*b2*b2); + } + } + else + { /* normal error model */ + if (type == 1) fact *= opt->eratio[0]; + fact *= sys == SYS_GLO?EFACT_GLO:(sys == SYS_SBS?EFACT_SBS:EFACT_GPS); + if (opt->ionoopt == IONOOPT_IFLC) fact *= 3.0; + a = fact*opt->err[1]; + b = fact*opt->err[2]; } - } - else { /* normal error model */ - if (type==1) fact*=opt->eratio[0]; - fact*=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS); - if (opt->ionoopt==IONOOPT_IFLC) fact*=3.0; - a=fact*opt->err[1]; - b=fact*opt->err[2]; - } return a*a+b*b/sinel/sinel; } + + /* initialize state and covariance -------------------------------------------*/ - void initx(rtk_t *rtk, double xi, double var, int i) +void initx(rtk_t *rtk, double xi, double var, int i) { int 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->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; + } } -/* dual-frequency iono-free measurements -------------------------------------*/ - int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel, - const prcopt_t *opt, const double *dantr, const double *dants, - double phw, double *meas, double *var) -{ - const double *lam=nav->lam[obs->sat-1]; - double c1,c2,L1,L2,P1,P2,P1_C1,P2_C2,gamma; - int i=0,j=1,k; - trace(4,"ifmeas :\n"); + +/* dual-frequency iono-free measurements -------------------------------------*/ +int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel, + const prcopt_t *opt, const double *dantr, const double *dants, + double phw, double *meas, double *var) +{ + const double *lam = nav->lam[obs->sat-1]; + double c1, c2, L1, L2, P1, P2, P1_C1, P2_C2, gamma; + int i = 0, j = 1, k; + + trace(4, "ifmeas :\n"); /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ - if (NFREQ>=3&&(satsys(obs->sat,NULL)&(SYS_GAL|SYS_SBS))) j=2; + if (NFREQ >= 3 && (satsys(obs->sat, NULL)&(SYS_GAL|SYS_SBS))) j = 2; - if (NFREQ<2||lam[i]==0.0||lam[j]==0.0) return 0; + if (NFREQ<2 || lam[i] == 0.0 || lam[j] == 0.0) return 0; /* test snr mask */ - if (testsnr(0,i,azel[1],obs->SNR[i]*0.25,&opt->snrmask)|| - testsnr(0,j,azel[1],obs->SNR[j]*0.25,&opt->snrmask)) { - return 0; - } - gamma=SQR(lam[j])/SQR(lam[i]); /* f1^2/f2^2 */ - c1=gamma/(gamma-1.0); /* f1^2/(f1^2-f2^2) */ - c2=-1.0 /(gamma-1.0); /* -f2^2/(f1^2-f2^2) */ + if (testsnr(0, i, azel[1], obs->SNR[i]*0.25, &opt->snrmask) || + testsnr(0, j, azel[1], obs->SNR[j]*0.25, &opt->snrmask)) + { + return 0; + } + gamma = std::pow(lam[j], 2.0)/std::pow(lam[i], 2.0); /* f1^2/f2^2 */ + c1 = gamma/(gamma-1.0); /* f1^2/(f1^2-f2^2) */ + c2 = -1.0 /(gamma-1.0); /* -f2^2/(f1^2-f2^2) */ - L1=obs->L[i]*lam[i]; /* cycle -> m */ - L2=obs->L[j]*lam[j]; - P1=obs->P[i]; - P2=obs->P[j]; - P1_C1=nav->cbias[obs->sat-1][1]; - P2_C2=nav->cbias[obs->sat-1][2]; - if (opt->sateph==EPHOPT_LEX) { - P1_C1=nav->lexeph[obs->sat-1].isc[0]*SPEED_OF_LIGHT; /* ISC_L1C/A */ - } - if (L1==0.0||L2==0.0||P1==0.0||P2==0.0) return 0; + L1 = obs->L[i]*lam[i]; /* cycle -> m */ + L2 = obs->L[j]*lam[j]; + P1 = obs->P[i]; + P2 = obs->P[j]; + P1_C1 = nav->cbias[obs->sat-1][1]; + P2_C2 = nav->cbias[obs->sat-1][2]; + if (opt->sateph == EPHOPT_LEX) + { + P1_C1 = nav->lexeph[obs->sat-1].isc[0]*SPEED_OF_LIGHT; /* ISC_L1C/A */ + } + if (L1 == 0.0 || L2 == 0.0 || P1 == 0.0 || P2 == 0.0) return 0; /* iono-free phase with windup correction */ - meas[0]=c1*L1+c2*L2-(c1*lam[i]+c2*lam[j])*phw; + meas[0] = c1*L1+c2*L2-(c1*lam[i]+c2*lam[j])*phw; /* iono-free code with dcb correction */ - if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */ - if (obs->code[j]==CODE_L2C) P2+=P2_C2; /* C2->P2 */ - meas[1]=c1*P1+c2*P2; - var[1]=SQR(ERR_CBIAS); + if (obs->code[i] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ + if (obs->code[j] == CODE_L2C) P2 += P2_C2; /* C2->P2 */ + meas[1] = c1*P1+c2*P2; + var[1] = std::pow(ERR_CBIAS, 2.0); - if (opt->sateph==EPHOPT_SBAS) meas[1]-=P1_C1; /* sbas clock based C1 */ + if (opt->sateph == EPHOPT_SBAS) meas[1] -= P1_C1; /* sbas clock based C1 */ /* gps-glonass h/w bias correction for code */ - if (opt->exterr.ena[3]&&satsys(obs->sat,NULL)==SYS_GLO) { - meas[1]+=c1*opt->exterr.gpsglob[0]+c2*opt->exterr.gpsglob[1]; - } + if (opt->exterr.ena[3] && satsys(obs->sat, NULL) == SYS_GLO) + { + meas[1] += c1*opt->exterr.gpsglob[0]+c2*opt->exterr.gpsglob[1]; + } /* antenna phase center variation correction */ - for (k=0;k<2;k++) { - if (dants) meas[k]-=c1*dants[i]+c2*dants[j]; - if (dantr) meas[k]-=c1*dantr[i]+c2*dantr[j]; - } + for (k = 0;k<2;k++) + { + if (dants) meas[k] -= c1*dants[i]+c2*dants[j]; + if (dantr) meas[k] -= c1*dantr[i]+c2*dantr[j]; + } return 1; } + + /* get tgd parameter (m) -----------------------------------------------------*/ - double gettgd_ppp(int sat, const nav_t *nav) +double gettgd_ppp(int sat, const nav_t *nav) { int i; - for (i=0;in;i++) { - if (nav->eph[i].sat!=sat) continue; - return SPEED_OF_LIGHT*nav->eph[i].tgd[0]; - } + for (i = 0;in;i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT*nav->eph[i].tgd[0]; + } return 0.0; } + + /* slant ionospheric delay ---------------------------------------------------*/ - int corr_ion(gtime_t time, const nav_t *nav, int sat, const double *pos, - const double *azel, int ionoopt, double *ion, double *var, - int *brk) +int corr_ion(gtime_t time, const nav_t *nav, int sat, const double *pos, + const double *azel, int ionoopt, double *ion, double *var, + int *brk) { #ifdef EXTSTEC double rate; #endif /* sbas ionosphere model */ - if (ionoopt==IONOOPT_SBAS) { - return sbsioncorr(time,nav,pos,azel,ion,var); - } + if (ionoopt == IONOOPT_SBAS) + { + return sbsioncorr(time, nav, pos, azel, ion, var); + } /* ionex tec model */ - if (ionoopt==IONOOPT_TEC) { - return iontec(time,nav,pos,azel,1,ion,var); - } + if (ionoopt == IONOOPT_TEC) + { + return iontec(time, nav, pos, azel, 1, ion, var); + } #ifdef EXTSTEC /* slant tec model */ - if (ionoopt==IONOOPT_STEC) { - return stec_ion(time,nav,sat,pos,azel,ion,&rate,var,brk); - } + if (ionoopt == IONOOPT_STEC) + { + return stec_ion(time, nav, sat, pos, azel, ion, &rate, var, brk); + } #endif /* broadcast model */ - if (ionoopt==IONOOPT_BRDC) { - *ion=ionmodel(time,nav->ion_gps,pos,azel); - *var=SQR(*ion*ERR_BRDCI); - return 1; - } + if (ionoopt == IONOOPT_BRDC) + { + *ion = ionmodel(time, nav->ion_gps, pos, azel); + *var = std::pow(*ion*ERR_BRDCI, 2.0); + return 1; + } /* ionosphere model off */ - *ion=0.0; - *var=VAR_IONO_OFF; + *ion = 0.0; + *var = VAR_IONO_OFF; return 1; } + + /* ionosphere and antenna corrected measurements -----------------------------*/ - int corrmeas(const obsd_t *obs, const nav_t *nav, const double *pos, - const double *azel, const prcopt_t *opt, - const double *dantr, const double *dants, double phw, - double *meas, double *var, int *brk) +int corrmeas(const obsd_t *obs, const nav_t *nav, const double *pos, + const double *azel, const prcopt_t *opt, + const double *dantr, const double *dants, double phw, + double *meas, double *var, int *brk) { - const double *lam=nav->lam[obs->sat-1]; - double ion=0.0,L1,P1,PC,P1_P2,P1_C1,vari,gamma; + const double *lam = nav->lam[obs->sat-1]; + double ion = 0.0, L1, P1, PC, P1_P2, P1_C1, vari, gamma; int i; - trace(4,"corrmeas:\n"); + trace(4, "corrmeas:\n"); - meas[0]=meas[1]=var[0]=var[1]=0.0; + meas[0] = meas[1] = var[0] = var[1] = 0.0; /* iono-free LC */ - if (opt->ionoopt==IONOOPT_IFLC) { - return ifmeas(obs,nav,azel,opt,dantr,dants,phw,meas,var); - } - if (lam[0]==0.0||obs->L[0]==0.0||obs->P[0]==0.0) return 0; + if (opt->ionoopt == IONOOPT_IFLC) + { + return ifmeas(obs, nav, azel, opt, dantr, dants, phw, meas, var); + } + if (lam[0] == 0.0 || obs->L[0] == 0.0 || obs->P[0] == 0.0) return 0; - if (testsnr(0,0,azel[1],obs->SNR[0]*0.25,&opt->snrmask)) return 0; + if (testsnr(0, 0, azel[1], obs->SNR[0]*0.25, &opt->snrmask)) return 0; - L1=obs->L[0]*lam[0]; - P1=obs->P[0]; + L1 = obs->L[0]*lam[0]; + P1 = obs->P[0]; /* dcb correction */ - gamma=SQR(lam[1]/lam[0]); /* f1^2/f2^2 */ - P1_P2=nav->cbias[obs->sat-1][0]; - P1_C1=nav->cbias[obs->sat-1][1]; - if (P1_P2==0.0&&(satsys(obs->sat,NULL)&(SYS_GPS|SYS_GAL|SYS_QZS))) { - P1_P2=(1.0-gamma)*gettgd_ppp(obs->sat,nav); - } - if (obs->code[0]==CODE_L1C) P1+=P1_C1; /* C1->P1 */ - PC=P1-P1_P2/(1.0-gamma); /* P1->PC */ + gamma = std::pow(lam[1]/lam[0], 2.0); /* f1^2/f2^2 */ + P1_P2 = nav->cbias[obs->sat-1][0]; + P1_C1 = nav->cbias[obs->sat-1][1]; + if (P1_P2 == 0.0 && (satsys(obs->sat, NULL)&(SYS_GPS|SYS_GAL|SYS_QZS))) + { + P1_P2 = (1.0-gamma)*gettgd_ppp(obs->sat, nav); + } + if (obs->code[0] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ + PC = P1-P1_P2/(1.0-gamma); /* P1->PC */ /* slant ionospheric delay L1 (m) */ - if (!corr_ion(obs->time,nav,obs->sat,pos,azel,opt->ionoopt,&ion,&vari,brk)) { - - trace(2,"iono correction error: time=%s sat=%2d ionoopt=%d\n", - time_str(obs->time,2),obs->sat,opt->ionoopt); - return 0; - } + if (!corr_ion(obs->time, nav, obs->sat, pos, azel, opt->ionoopt, &ion, &vari, brk)) + { + trace(2, "iono correction error: time=%s sat=%2d ionoopt=%d\n", + time_str(obs->time, 2), obs->sat, opt->ionoopt); + return 0; + } /* ionosphere and windup corrected phase and code */ - meas[0]=L1+ion-lam[0]*phw; - meas[1]=PC-ion; + meas[0] = L1+ion-lam[0]*phw; + meas[1] = PC-ion; - var[0]+=vari; - var[1]+=vari+SQR(ERR_CBIAS); + var[0] += vari; + var[1] += vari+std::pow(ERR_CBIAS, 2.0); /* antenna phase center variation correction */ - for (i=0;i<2;i++) { - if (dants) meas[i]-=dants[0]; - if (dantr) meas[i]-=dantr[0]; - } + for (i = 0;i<2;i++) + { + if (dants) meas[i] -= dants[0]; + if (dantr) meas[i] -= dantr[0]; + } return 1; } -/* L1/L2 geometry-free phase measurement -------------------------------------*/ - double gfmeas(const obsd_t *obs, const nav_t *nav) -{ - const double *lam=nav->lam[obs->sat-1]; - if (lam[0]==0.0||lam[1]==0.0||obs->L[0]==0.0||obs->L[1]==0.0) return 0.0; + +/* L1/L2 geometry-free phase measurement -------------------------------------*/ +double gfmeas(const obsd_t *obs, const nav_t *nav) +{ + const double *lam = nav->lam[obs->sat-1]; + + if (lam[0] == 0.0 || lam[1] == 0.0 || obs->L[0] == 0.0 || obs->L[1] == 0.0) return 0.0; return lam[0]*obs->L[0]-lam[1]*obs->L[1]; } + + /* temporal update of position -----------------------------------------------*/ - void udpos_ppp(rtk_t *rtk) +void udpos_ppp(rtk_t *rtk) { int i; - trace(3,"udpos_ppp:\n"); + trace(3, "udpos_ppp:\n"); /* fixed mode */ - if (rtk->opt.mode==PMODE_PPP_FIXED) { - for (i=0;i<3;i++) initx(rtk,rtk->opt.ru[i],1E-8,i); - return; - } + if (rtk->opt.mode == PMODE_PPP_FIXED) + { + for (i = 0;i<3;i++) initx(rtk, rtk->opt.ru[i], 1E-8, i); + return; + } /* initialize position for first epoch */ - if (norm_rtk(rtk->x,3)<=0.0) { - for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i); - } + if (norm_rtk(rtk->x, 3) <= 0.0) + { + for (i = 0;i<3;i++) initx(rtk, rtk->sol.rr[i], VAR_POS_PPP, i); + } /* static ppp mode */ - if (rtk->opt.mode==PMODE_PPP_STATIC) return; + if (rtk->opt.mode == PMODE_PPP_STATIC) return; /* kinmatic mode without 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_PPP, i); + } } + + /* temporal update of clock --------------------------------------------------*/ - void udclk_ppp(rtk_t *rtk) +void udclk_ppp(rtk_t *rtk) { double dtr; int i; - trace(3,"udclk_ppp:\n"); + trace(3, "udclk_ppp:\n"); /* initialize every epoch for clock (white noise) */ - for (i=0;iopt.sateph==EPHOPT_PREC) { - /* time of prec ephemeris is based gpst */ - /* negelect receiver inter-system bias */ - dtr=rtk->sol.dtr[0]; + for (i = 0;iopt.sateph == EPHOPT_PREC) + { + /* time of prec ephemeris is based gpst */ + /* negelect receiver inter-system bias */ + dtr = rtk->sol.dtr[0]; + } + else + { + dtr = i == 0?rtk->sol.dtr[0]:rtk->sol.dtr[0]+rtk->sol.dtr[i]; + } + initx(rtk, SPEED_OF_LIGHT*dtr, VAR_CLK, IC_PPP(i, &rtk->opt)); } - else { - dtr=i==0?rtk->sol.dtr[0]:rtk->sol.dtr[0]+rtk->sol.dtr[i]; - } - initx(rtk,SPEED_OF_LIGHT*dtr,VAR_CLK,IC(i,&rtk->opt)); - } } + + /* temporal update of tropospheric parameters --------------------------------*/ - void udtrop_ppp(rtk_t *rtk) +void udtrop_ppp(rtk_t *rtk) { - double pos[3],azel[]={0.0,PI/2.0},ztd,var; - int i=IT_PPP(&rtk->opt),j; + double pos[3], azel[] = {0.0, PI/2.0}, ztd, var; + int i = IT_PPP(&rtk->opt), j; - trace(3,"udtrop_ppp:\n"); + trace(3, "udtrop_ppp:\n"); - if (rtk->x[i]==0.0) { - ecef2pos(rtk->sol.rr,pos); - ztd=sbstropcorr(rtk->sol.time,pos,azel,&var); - initx(rtk,ztd,var,i); + if (rtk->x[i] == 0.0) + { + ecef2pos(rtk->sol.rr, pos); + ztd = sbstropcorr(rtk->sol.time, pos, azel, &var); + initx(rtk, ztd, var, i); - if (rtk->opt.tropopt>=TROPOPT_ESTG) { - for (j=0;j<2;j++) initx(rtk,1E-6,VAR_GRA,++i); + if (rtk->opt.tropopt >= TROPOPT_ESTG) + { + for (j = 0;j<2;j++) initx(rtk, 1E-6, VAR_GRA_PPP, ++i); + } } - } - else { - rtk->P[i*(1+rtk->nx)]+=SQR(rtk->opt.prn[2])*fabs(rtk->tt); + else + { + rtk->P[i*(1+rtk->nx)] += std::pow(rtk->opt.prn[2], 2.0)*fabs(rtk->tt); - if (rtk->opt.tropopt>=TROPOPT_ESTG) { - for (j=0;j<2;j++) { - rtk->P[++i*(1+rtk->nx)]+=SQR(rtk->opt.prn[2]*0.1)*fabs(rtk->tt); - } + if (rtk->opt.tropopt >= TROPOPT_ESTG) + { + for (j = 0;j<2;j++) + { + rtk->P[++i*(1+rtk->nx)] += std::pow(rtk->opt.prn[2]*0.1, 2.0)*fabs(rtk->tt); + } + } } - } } + + /* detect cycle slip by LLI --------------------------------------------------*/ - void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n) +void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n) { - int i,j; + int i, j; - trace(3,"detslp_ll: n=%d\n",n); + trace(3, "detslp_ll: n=%d\n", n); - for (i=0;iopt.nf;j++) { - if (obs[i].L[j]==0.0||!(obs[i].LLI[j]&3)) continue; + for (i = 0;iopt.nf;j++) + { + if (obs[i].L[j] == 0.0 || !(obs[i].LLI[j]&3)) continue; - trace(3,"detslp_ll: slip detected sat=%2d f=%d\n",obs[i].sat,j+1); + trace(3, "detslp_ll: slip detected sat=%2d f=%d\n", obs[i].sat, j+1); - rtk->ssat[obs[i].sat-1].slip[j]=1; - } -} -/* detect cycle slip by geometry free phase jump -----------------------------*/ - void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) -{ - double g0,g1; - int i,j; - - trace(3,"detslp_gf: n=%d\n",n); - - for (i=0;issat[obs[i].sat-1].gf; - rtk->ssat[obs[i].sat-1].gf=g1; - - trace(4,"detslip_gf: sat=%2d gf0=%8.3f gf1=%8.3f\n",obs[i].sat,g0,g1); - - if (g0!=0.0&&fabs(g1-g0)>rtk->opt.thresslip) { - trace(3,"detslip_gf: slip detected sat=%2d gf=%8.3f->%8.3f\n", - obs[i].sat,g0,g1); - - for (j=0;jopt.nf;j++) rtk->ssat[obs[i].sat-1].slip[j]|=1; + rtk->ssat[obs[i].sat-1].slip[j] = 1; } - } } -/* temporal update of phase biases -------------------------------------------*/ - void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) + + +/* detect cycle slip by geometry free phase jump -----------------------------*/ +void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) { - double meas[2],var[2],bias[MAXOBS]={0},offset=0.0,pos[3]={0}; - int i,j,k,sat,brk=0; + double g0, g1; + int i, j; - trace(3,"udbias : n=%d\n",n); + trace(3, "detslp_gf: n=%d\n", n); - for (i=0;iopt.nf;j++) { - rtk->ssat[i].slip[j]=0; - } + for (i = 0;issat[obs[i].sat-1].gf; + rtk->ssat[obs[i].sat-1].gf = g1; + + trace(4, "detslip_gf: sat=%2d gf0=%8.3f gf1=%8.3f\n", obs[i].sat, g0, g1); + + if (g0 != 0.0 && fabs(g1-g0)>rtk->opt.thresslip) + { + trace(3, "detslip_gf: slip detected sat=%2d gf=%8.3f->%8.3f\n", + obs[i].sat, g0, g1); + + for (j = 0;jopt.nf;j++) rtk->ssat[obs[i].sat-1].slip[j]|=1; + } + } +} + + +/* temporal update of phase biases -------------------------------------------*/ +void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +{ + double meas[2], var[2], bias[MAXOBS] = {0}, offset = 0.0, pos[3] = {0}; + int i, j, k, sat, brk = 0; + + trace(3, "udbias : n=%d\n", n); + + for (i = 0;iopt.nf;j++) + { + rtk->ssat[i].slip[j] = 0; + } /* detect cycle slip by LLI */ - detslp_ll(rtk,obs,n); + detslp_ll(rtk, obs, n); /* detect cycle slip by geometry-free phase jump */ - detslp_gf(rtk,obs,n,nav); + detslp_gf(rtk, obs, n, nav); /* reset phase-bias if expire obs outage counter */ - for (i=0;issat[i].outc[0]>(unsigned int)rtk->opt.maxout) { - initx(rtk,0.0,0.0,IB(i+1,&rtk->opt)); + for (i = 0;issat[i].outc[0]>(unsigned int)rtk->opt.maxout) + { + initx(rtk, 0.0, 0.0, IB_PPP(i+1, &rtk->opt)); + } } - } - ecef2pos(rtk->sol.rr,pos); + ecef2pos(rtk->sol.rr, pos); - for (i=k=0;iopt); - if (!corrmeas(obs+i,nav,pos,rtk->ssat[sat-1].azel,&rtk->opt,NULL,NULL, - 0.0,meas,var,&brk)) continue; + for (i = k = 0;iopt); + if (!corrmeas(obs+i, nav, pos, rtk->ssat[sat-1].azel, &rtk->opt, NULL, NULL, + 0.0, meas, var, &brk)) continue; - if (brk) { - rtk->ssat[sat-1].slip[0]=1; - trace(2,"%s: sat=%2d correction break\n",time_str(obs[i].time,0),sat); + if (brk) + { + rtk->ssat[sat-1].slip[0] = 1; + trace(2, "%s: sat=%2d correction break\n", time_str(obs[i].time, 0), sat); + } + bias[i] = meas[0]-meas[1]; + if (rtk->x[j] == 0.0 || + rtk->ssat[sat-1].slip[0] || rtk->ssat[sat-1].slip[1]) continue; + offset += bias[i]-rtk->x[j]; + k++; } - bias[i]=meas[0]-meas[1]; - if (rtk->x[j]==0.0|| - rtk->ssat[sat-1].slip[0]||rtk->ssat[sat-1].slip[1]) continue; - offset+=bias[i]-rtk->x[j]; - k++; - } /* correct phase-code jump to enssure phase-code coherency */ - if (k>=2&&fabs(offset/k)>0.0005*SPEED_OF_LIGHT) { - for (i=0;iopt); - if (rtk->x[j]!=0.0) rtk->x[j]+=offset/k; + if (k >= 2 && fabs(offset/k)>0.0005*SPEED_OF_LIGHT) + { + for (i = 0;iopt); + if (rtk->x[j] != 0.0) rtk->x[j] += offset/k; + } + trace(2, "phase-code jump corrected: %s n=%2d dt=%12.9fs\n", + time_str(rtk->sol.time, 0), k, offset/k/SPEED_OF_LIGHT); } - trace(2,"phase-code jump corrected: %s n=%2d dt=%12.9fs\n", - time_str(rtk->sol.time,0),k,offset/k/SPEED_OF_LIGHT); - } - for (i=0;iopt); + for (i = 0;iopt); - rtk->P[j+j*rtk->nx]+=SQR(rtk->opt.prn[0])*fabs(rtk->tt); + rtk->P[j+j*rtk->nx] += std::pow(rtk->opt.prn[0], 2.0)*fabs(rtk->tt); - if (rtk->x[j]!=0.0&& - !rtk->ssat[sat-1].slip[0]&&!rtk->ssat[sat-1].slip[1]) continue; + if (rtk->x[j] != 0.0 && + !rtk->ssat[sat-1].slip[0] && !rtk->ssat[sat-1].slip[1]) continue; - if (bias[i]==0.0) continue; + if (bias[i] == 0.0) continue; - /* reinitialize phase-bias if detecting cycle slip */ - initx(rtk,bias[i],VAR_BIAS,IB(sat,&rtk->opt)); + /* reinitialize phase-bias if detecting cycle slip */ + initx(rtk, bias[i], VAR_BIAS, IB_PPP(sat, &rtk->opt)); - trace(5,"udbias_ppp: sat=%2d bias=%.3f\n",sat,meas[0]-meas[1]); - } + trace(5, "udbias_ppp: sat=%2d bias=%.3f\n", sat, meas[0]-meas[1]); + } } + + /* temporal update of states --------------------------------------------------*/ - void udstate_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +void udstate_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) { - trace(3,"udstate_ppp: n=%d\n",n); + trace(3, "udstate_ppp: n=%d\n", n); /* temporal update of position */ udpos_ppp(rtk); @@ -1233,280 +1384,317 @@ void pppoutsolstat(rtk_t *rtk, int level, FILE *fp) udclk_ppp(rtk); /* temporal update of tropospheric parameters */ - if (rtk->opt.tropopt>=TROPOPT_EST) { - udtrop_ppp(rtk); - } + if (rtk->opt.tropopt >= TROPOPT_EST) + { + udtrop_ppp(rtk); + } /* temporal update of phase-bias */ - udbias_ppp(rtk,obs,n,nav); + udbias_ppp(rtk, obs, n, nav); } + + /* satellite antenna phase center variation ----------------------------------*/ - void satantpcv(const double *rs, const double *rr, const pcv_t *pcv, - double *dant) +void satantpcv(const double *rs, const double *rr, const pcv_t *pcv, + double *dant) { - double ru[3],rz[3],eu[3],ez[3],nadir,cosa; + double ru[3], rz[3], eu[3], ez[3], nadir, cosa; int i; - for (i=0;i<3;i++) { - ru[i]=rr[i]-rs[i]; - rz[i]=-rs[i]; - } - if (!normv3(ru,eu)||!normv3(rz,ez)) return; + for (i = 0;i<3;i++) + { + ru[i] = rr[i]-rs[i]; + rz[i] = -rs[i]; + } + if (!normv3(ru, eu) || !normv3(rz, ez)) return; - cosa=dot(eu,ez,3); - cosa=cosa<-1.0?-1.0:(cosa>1.0?1.0:cosa); - nadir=acos(cosa); + cosa = dot(eu, ez, 3); + cosa = cosa<-1.0?-1.0:(cosa>1.0?1.0:cosa); + nadir = acos(cosa); - antmodel_s(pcv,nadir,dant); + antmodel_s(pcv, nadir, dant); } + + /* precise tropospheric model ------------------------------------------------*/ - double prectrop(gtime_t time, const double *pos, const double *azel, - const prcopt_t *opt, const double *x, double *dtdx, - double *var) +double prectrop(gtime_t time, const double *pos, const double *azel, + const prcopt_t *opt, const double *x, double *dtdx, + double *var) { - const double zazel[]={0.0,PI/2.0}; - double zhd,m_h,m_w,cotz,grad_n,grad_e; + const double zazel[] = {0.0, PI/2.0}; + double zhd, m_h, m_w, cotz, grad_n, grad_e; /* zenith hydrostatic delay */ - zhd=tropmodel(time,pos,zazel,0.0); + zhd = tropmodel(time, pos, zazel, 0.0); /* mapping function */ - m_h=tropmapf(time,pos,azel,&m_w); + m_h = tropmapf(time, pos, azel, &m_w); - if ((opt->tropopt==TROPOPT_ESTG||opt->tropopt==TROPOPT_CORG)&&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[1]+grad_e*x[2]; - dtdx[1]=grad_n*(x[0]-zhd); - dtdx[2]=grad_e*(x[0]-zhd); - } - dtdx[0]=m_w; - *var=SQR(0.01); + if ((opt->tropopt == TROPOPT_ESTG || opt->tropopt == TROPOPT_CORG) && 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[1]+grad_e*x[2]; + dtdx[1] = grad_n*(x[0]-zhd); + dtdx[2] = grad_e*(x[0]-zhd); + } + dtdx[0] = m_w; + *var = std::pow(0.01, 2.0); return m_h*zhd+m_w*(x[0]-zhd); } + + /* phase and code residuals --------------------------------------------------*/ - int res_ppp(int iter, const obsd_t *obs, int n, const double *rs, - const double *dts, const double *vare, const int *svh, - const nav_t *nav, const double *x, rtk_t *rtk, double *v, - double *H, double *R, double *azel) +int res_ppp(int iter, const obsd_t *obs, int n, const double *rs, + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const double *x, rtk_t *rtk, double *v, + double *H, double *R, double *azel) { - prcopt_t *opt=&rtk->opt; - double r,rr[3],disp[3],pos[3],e[3],meas[2],dtdx[3],dantr[NFREQ]={0}; - double dants[NFREQ]={0},var[MAXOBS*2],dtrp=0.0,vart=0.0,varm[2]={0}; - int i,j,k,sat,sys,nv=0,nx=rtk->nx,brk,tideopt; + prcopt_t *opt = &rtk->opt; + double r, rr[3], disp[3], pos[3], e[3], meas[2], dtdx[3], dantr[NFREQ] = {0}; + double dants[NFREQ] = {0}, var[MAXOBS*2], dtrp = 0.0, vart = 0.0, varm[2] = {0}; + int i, j, k, sat, sys, nv = 0, nx = rtk->nx, brk, tideopt; - trace(3,"res_ppp : n=%d nx=%d\n",n,nx); + trace(3, "res_ppp : n=%d nx=%d\n", n, nx); - for (i=0;issat[i].vsat[0]=0; + for (i = 0;issat[i].vsat[0] = 0; - for (i=0;i<3;i++) rr[i]=x[i]; + for (i = 0;i<3;i++) rr[i] = x[i]; /* earth tides correction */ - if (opt->tidecorr) { - tideopt=opt->tidecorr==1?1:7; /* 1:solid, 2:solid+otl+pole */ + if (opt->tidecorr) + { + tideopt = opt->tidecorr == 1?1:7; /* 1:solid, 2:solid+otl+pole */ - tidedisp(gpst2utc(obs[0].time),rr,tideopt,&nav->erp,opt->odisp[0], - disp); - for (i=0;i<3;i++) rr[i]+=disp[i]; - } - ecef2pos(rr,pos); - - for (i=0;issat[sat-1].vs) continue; - - /* geometric distance/azimuth/elevation angle */ - if ((r=geodist(rs+i*6,rr,e))<=0.0|| - satazel(pos,e,azel+i*2)elmin) continue; - - /* excluded satellite? */ - if (satexclude(obs[i].sat,svh[i],opt)) continue; - - /* tropospheric delay correction */ - if (opt->tropopt==TROPOPT_SAAS) { - dtrp=tropmodel(obs[i].time,pos,azel+i*2,REL_HUMI); - vart=SQR(ERR_SAAS); + tidedisp(gpst2utc(obs[0].time), rr, tideopt, &nav->erp, opt->odisp[0], + disp); + for (i = 0;i<3;i++) rr[i] += disp[i]; } - else if (opt->tropopt==TROPOPT_SBAS) { - dtrp=sbstropcorr(obs[i].time,pos,azel+i*2,&vart); - } - else if (opt->tropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) { - dtrp=prectrop(obs[i].time,pos,azel+i*2,opt,x+IT_PPP(opt),dtdx,&vart); - } - else if (opt->tropopt==TROPOPT_COR||opt->tropopt==TROPOPT_CORG) { - dtrp=prectrop(obs[i].time,pos,azel+i*2,opt,x,dtdx,&vart); - } - /* satellite antenna model */ - if (opt->posopt[0]) { - satantpcv(rs+i*6,rr,nav->pcvs+sat-1,dants); - } - /* receiver antenna model */ - antmodel(opt->pcvr,opt->antdel[0],azel+i*2,opt->posopt[1],dantr); + ecef2pos(rr, pos); - /* phase windup correction */ - if (opt->posopt[2]) { - windupcorr(rtk->sol.time,rs+i*6,rr,&rtk->ssat[sat-1].phw); - } - /* ionosphere and antenna phase corrected measurements */ - if (!corrmeas(obs+i,nav,pos,azel+i*2,&rtk->opt,dantr,dants, - rtk->ssat[sat-1].phw,meas,varm,&brk)) { - continue; - } - /* satellite clock and tropospheric delay */ - r+=-SPEED_OF_LIGHT*dts[i*2]+dtrp; + for (i = 0;issat[sat-1].vs) continue; - trace(5,"sat=%2d azel=%6.1f %5.1f dtrp=%.3f dantr=%6.3f %6.3f dants=%6.3f %6.3f phw=%6.3f\n", - sat,azel[i*2]*R2D,azel[1+i*2]*R2D,dtrp,dantr[0],dantr[1],dants[0], - dants[1],rtk->ssat[sat-1].phw); + /* geometric distance/azimuth/elevation angle */ + if ((r = geodist(rs+i*6, rr, e)) <= 0.0 || + satazel(pos, e, azel+i*2)elmin) continue; - for (j=0;j<2;j++) { /* for phase and code */ + /* excluded satellite? */ + if (satexclude(obs[i].sat, svh[i], opt)) continue; - if (meas[j]==0.0) continue; - - for (k=0;ktropopt>=TROPOPT_EST) { - for (k=0;k<(opt->tropopt>=TROPOPT_ESTG?3:1);k++) { - H[IT_PPP(opt)+k+nx*nv]=dtdx[k]; + /* tropospheric delay correction */ + if (opt->tropopt == TROPOPT_SAAS) + { + dtrp = tropmodel(obs[i].time, pos, azel+i*2, REL_HUMI); + vart = std::pow(ERR_SAAS, 2.0); } - } - if (j==0) { - v[nv]-=x[IB(obs[i].sat,opt)]; - H[IB(obs[i].sat,opt)+nx*nv]=1.0; - } - var[nv]=varerr(obs[i].sat,sys,azel[1+i*2],j,opt)+varm[j]+vare[i]+vart; + else if (opt->tropopt == TROPOPT_SBAS) + { + dtrp = sbstropcorr(obs[i].time, pos, azel+i*2, &vart); + } + else if (opt->tropopt == TROPOPT_EST || opt->tropopt == TROPOPT_ESTG) + { + dtrp = prectrop(obs[i].time, pos, azel+i*2, opt, x+IT_PPP(opt), dtdx, &vart); + } + else if (opt->tropopt == TROPOPT_COR || opt->tropopt == TROPOPT_CORG) + { + dtrp = prectrop(obs[i].time, pos, azel+i*2, opt, x, dtdx, &vart); + } + /* satellite antenna model */ + if (opt->posopt[0]) + { + satantpcv(rs+i*6, rr, nav->pcvs+sat-1, dants); + } + /* receiver antenna model */ + antmodel(opt->pcvr, opt->antdel[0], azel+i*2, opt->posopt[1], dantr); - if (j==0) rtk->ssat[sat-1].resc[0]=v[nv]; - else rtk->ssat[sat-1].resp[0]=v[nv]; + /* phase windup correction */ + if (opt->posopt[2]) + { + windupcorr(rtk->sol.time, rs+i*6, rr, &rtk->ssat[sat-1].phw); + } + /* ionosphere and antenna phase corrected measurements */ + if (!corrmeas(obs+i, nav, pos, azel+i*2, &rtk->opt, dantr, dants, + rtk->ssat[sat-1].phw, meas, varm, &brk)) + { + continue; + } + /* satellite clock and tropospheric delay */ + r += -SPEED_OF_LIGHT*dts[i*2]+dtrp; - /* test innovation */ + trace(5, "sat=%2d azel=%6.1f %5.1f dtrp=%.3f dantr=%6.3f %6.3f dants=%6.3f %6.3f phw=%6.3f\n", + sat, azel[i*2]*R2D, azel[1+i*2]*R2D, dtrp, dantr[0], dantr[1], dants[0], + dants[1], rtk->ssat[sat-1].phw); + + for (j = 0;j<2;j++) + { /* for phase and code */ + + if (meas[j] == 0.0) continue; + + for (k = 0;ktropopt >= TROPOPT_EST) + { + for (k = 0;k<(opt->tropopt >= TROPOPT_ESTG?3:1);k++) + { + H[IT_PPP(opt)+k+nx*nv] = dtdx[k]; + } + } + if (j == 0) + { + v[nv] -= x[IB_PPP(obs[i].sat, opt)]; + H[IB_PPP(obs[i].sat, opt)+nx*nv] = 1.0; + } + var[nv] = varerr(obs[i].sat, sys, azel[1+i*2], j, opt)+varm[j]+vare[i]+vart; + + if (j == 0) rtk->ssat[sat-1].resc[0] = v[nv]; + else rtk->ssat[sat-1].resp[0] = v[nv]; + + /* test innovation */ #if 0 - if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) { + if (opt->maxinno>0.0 && fabs(v[nv])>opt->maxinno) + { #else - if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno&&sys!=SYS_GLO) { + if (opt->maxinno>0.0 && fabs(v[nv])>opt->maxinno && sys != SYS_GLO) + { #endif - trace(2,"ppp outlier rejected %s sat=%2d type=%d v=%.3f\n", - time_str(obs[i].time,0),sat,j,v[nv]); - rtk->ssat[sat-1].rejc[0]++; - continue; - } - if (j==0) rtk->ssat[sat-1].vsat[0]=1; - nv++; + trace(2, "ppp outlier rejected %s sat=%2d type=%d v=%.3f\n", + time_str(obs[i].time, 0), sat, j, v[nv]); + rtk->ssat[sat-1].rejc[0]++; + continue; + } + if (j == 0) rtk->ssat[sat-1].vsat[0] = 1; + nv++; + } } - } - for (i=0;iopt; - double *rs,*dts,*var,*v,*H,*R,*azel,*xp,*Pp; - int i,nv,info,svh[MAXOBS],stat=SOLQ_SINGLE; + const prcopt_t *opt = &rtk->opt; + double *rs, *dts, *var, *v, *H, *R, *azel, *xp, *Pp; + int i, nv, info, svh[MAXOBS], stat = SOLQ_SINGLE; - trace(3,"pppos : nx=%d n=%d\n",rtk->nx,n); + trace(3, "pppos : nx=%d n=%d\n", rtk->nx, n); - rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel=zeros(2,n); + rs = mat(6, n); dts = mat(2, n); var = mat(1, n); azel = zeros(2, n); - for (i=0;issat[i].fix[0]=0; + for (i = 0; i < MAXSAT; i++) rtk->ssat[i].fix[0] = 0; /* temporal update of states */ - udstate_ppp(rtk,obs,n,nav); + udstate_ppp(rtk, obs, n, nav); - trace(4,"x(0)="); tracemat(4,rtk->x,1,NR(opt),13,4); + trace(4, "x(0)="); tracemat(4, rtk->x, 1, NR_PPP(opt), 13, 4); /* satellite positions and clocks */ - satposs(obs[0].time,obs,n,nav,rtk->opt.sateph,rs,dts,var,svh); + satposs(obs[0].time, obs, n, nav, rtk->opt.sateph, rs, dts, var, svh); /* exclude measurements of eclipsing satellite */ - if (rtk->opt.posopt[3]) { - testeclipse(obs,n,nav,rs); - } - xp=mat(rtk->nx,1); Pp=zeros(rtk->nx,rtk->nx); - matcpy(xp,rtk->x,rtk->nx,1); - nv=n*rtk->opt.nf*2; v=mat(nv,1); H=mat(rtk->nx,nv); R=mat(nv,nv); - - for (i=0;iopt.niter;i++) { - - /* phase and code residuals */ - if ((nv=res_ppp(i,obs,n,rs,dts,var,svh,nav,xp,rtk,v,H,R,azel))<=0) break; - - /* measurement update */ - matcpy(Pp,rtk->P,rtk->nx,rtk->nx); - - if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) { - trace(2,"ppp filter error %s info=%d\n",time_str(rtk->sol.time,0), - info); - break; + if (rtk->opt.posopt[3]) + { + testeclipse(obs, n, nav, rs); } - trace(4,"x(%d)=",i+1); tracemat(4,xp,1,NR(opt),13,4); + xp = mat(rtk->nx, 1); Pp = zeros(rtk->nx, rtk->nx); + matcpy(xp, rtk->x, rtk->nx, 1); + nv = n*rtk->opt.nf*2; v = mat(nv, 1); H = mat(rtk->nx, nv); R = mat(nv, nv); - stat=SOLQ_PPP; - } - if (stat==SOLQ_PPP) { - /* postfit residuals */ - res_ppp(1,obs,n,rs,dts,var,svh,nav,xp,rtk,v,H,R,azel); + for (i = 0; i < rtk->opt.niter; i++) + { + /* phase and code residuals */ + if ((nv = res_ppp(i, obs, n, rs, dts, var, svh, nav, xp, rtk, v, H, R, azel)) <= 0) break; - /* update state and covariance matrix */ - matcpy(rtk->x,xp,rtk->nx,1); - matcpy(rtk->P,Pp,rtk->nx,rtk->nx); + /* measurement update */ + matcpy(Pp, rtk->P, rtk->nx, rtk->nx); - /* ambiguity resolution in ppp */ - if (opt->modear==ARMODE_PPPAR||opt->modear==ARMODE_PPPAR_ILS) { - if (pppamb(rtk,obs,n,nav,azel)) stat=SOLQ_FIX; - } - /* update solution status */ - rtk->sol.ns=0; - for (i=0;issat[obs[i].sat-1].vsat[0]) continue; - rtk->ssat[obs[i].sat-1].lock[0]++; - rtk->ssat[obs[i].sat-1].outc[0]=0; - rtk->ssat[obs[i].sat-1].fix [0]=4; - rtk->sol.ns++; - } - rtk->sol.stat=stat; + if ((info = filter(xp, Pp, H, v, R, rtk->nx, nv))) + { + trace(2, "ppp filter error %s info=%d\n", time_str(rtk->sol.time, 0), info); + break; + } + trace(4, "x(%d)=", i+1); tracemat(4, xp, 1, NR_PPP(opt), 13, 4); - for (i=0;i<3;i++) { - rtk->sol.rr[i]=rtk->x[i]; - rtk->sol.qr[i]=(float)rtk->P[i+i*rtk->nx]; + stat = SOLQ_PPP; } - rtk->sol.qr[3]=(float)rtk->P[1]; - rtk->sol.qr[4]=(float)rtk->P[2+rtk->nx]; - rtk->sol.qr[5]=(float)rtk->P[2]; - rtk->sol.dtr[0]=rtk->x[IC(0,opt)]; - rtk->sol.dtr[1]=rtk->x[IC(1,opt)]-rtk->x[IC(0,opt)]; - for (i=0;issat[obs[i].sat-1].snr[0]=MIN(obs[i].SNR[0],obs[i].SNR[1]); + if (stat == SOLQ_PPP) + { + /* postfit residuals */ + res_ppp(1, obs, n, rs, dts, var, svh, nav, xp, rtk, v, H, R, azel); + + /* update state and covariance matrix */ + matcpy(rtk->x, xp, rtk->nx, 1); + matcpy(rtk->P, Pp, rtk->nx, rtk->nx); + + /* ambiguity resolution in ppp */ + if (opt->modear == ARMODE_PPPAR || opt->modear == ARMODE_PPPAR_ILS) + { + if (pppamb(rtk, obs, n, nav, azel)) stat = SOLQ_FIX; + } + /* update solution status */ + rtk->sol.ns = 0; + for (i = 0; i < n && i < MAXOBS; i++) + { + if (!rtk->ssat[obs[i].sat-1].vsat[0]) continue; + rtk->ssat[obs[i].sat-1].lock[0]++; + rtk->ssat[obs[i].sat-1].outc[0] = 0; + rtk->ssat[obs[i].sat-1].fix [0] = 4; + rtk->sol.ns++; + } + rtk->sol.stat = stat; + + 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[2+rtk->nx]; + rtk->sol.qr[5] = (float)rtk->P[2]; + rtk->sol.dtr[0] = rtk->x[IC_PPP(0, opt)]; + rtk->sol.dtr[1] = rtk->x[IC_PPP(1, opt)]-rtk->x[IC_PPP(0, opt)]; + for (i = 0; i < n && i < MAXOBS; i++) + { + rtk->ssat[obs[i].sat-1].snr[0] = MIN_PPP(obs[i].SNR[0], obs[i].SNR[1]); + } + for (i = 0; i < MAXSAT; i++) + { + if (rtk->ssat[i].slip[0]&3) rtk->ssat[i].slipc[0]++; + } } - for (i=0;issat[i].slip[0]&3) rtk->ssat[i].slipc[0]++; - } - } free(rs); free(dts); free(var); free(azel); free(xp); free(Pp); free(v); free(H); free(R); } diff --git a/src/algorithms/libs/rtklib/rtklib_ppp.h b/src/algorithms/libs/rtklib/rtklib_ppp.h index 83866c97e..314e0d164 100644 --- a/src/algorithms/libs/rtklib/rtklib_ppp.h +++ b/src/algorithms/libs/rtklib/rtklib_ppp.h @@ -57,48 +57,28 @@ #include "rtklib.h" -#define SQR(x) ((x)*(x)) -#define MIN(x,y) ((x)<=(y)?(x):(y)) -#define ROUND(x) (int)floor((x)+0.5) +#define MIN_PPP(x,y) ((x)<=(y)?(x):(y)) +#define ROUND_PPP(x) (int)floor((x)+0.5) #define SWAP_I(x,y) do {int _z=x; x=y; y=_z;} while (0) #define SWAP_D(x,y) do {double _z=x; x=y; y=_z;} while (0) -#define MIN_ARC_GAP 300.0 /* min arc gap (s) */ -#define CONST_AMB 0.001 /* constraint to fixed ambiguity */ -#define THRES_RES 0.3 /* threashold of residuals test (m) */ -#define LOG_PI 1.14472988584940017 /* log(pi) */ -#define SQRT2 1.41421356237309510 /* sqrt(2) */ +const double MIN_ARC_GAP = 300.0; /* min arc gap (s) */ +const double CONST_AMB = 0.001; /* constraint to fixed ambiguity */ +const double THRES_RES = 0.3; /* threashold of residuals test (m) */ +const double LOG_PI = 1.14472988584940017; /* log(pi) */ +const double SQRT2 = 1.41421356237309510; /* sqrt(2) */ -#define AS2R (D2R/3600.0) /* arc sec to radian */ -//#define GME 3.986004415E+14 /* earth gravitational constant */ -//const double GMS = 1.327124E+20; /* sun gravitational constant */ -//#define GMM 4.902801E+12 /* moon gravitational constant */ - - /* initial variances */ -#define VAR_POS SQR(100.0) /* receiver position (m^2) */ -#define VAR_CLK SQR(100.0) /* receiver clock (m^2) */ -#define VAR_ZTD SQR( 0.3) /* ztd (m^2) */ -#define VAR_GRA SQR(0.001) /* gradient (m^2) */ -#define VAR_BIAS SQR(100.0) /* phase-bias (m^2) */ - -#define VAR_IONO_OFF SQR(10.0) /* variance of iono-model-off */ - -#define ERR_SAAS 0.3 /* saastamoinen model error std (m) */ -#define ERR_BRDCI 0.5 /* broadcast iono model error factor */ -#define ERR_CBIAS 0.3 /* code bias error std (m) */ -#define REL_HUMI 0.7 /* relative humidity for saastamoinen model */ - -#define NP(opt) ((opt)->dynamics?9:3) /* number of pos solution */ -#define IC(s,opt) (NP(opt)+(s)) /* state index of clocks (s=0:gps,1:glo) */ -#define IT(opt) (IC(0,opt)+NSYS) /* state index of tropos */ -#define NR(opt) (IT(opt)+((opt)->tropopttropopt==TROPOPT_EST?1:3))) - /* number of solutions */ -#define IB(s,opt) (NR(opt)+(s)-1) /* state index of phase bias */ -#define NX(opt) (IB(MAXSAT,opt)+1) /* number of estimated states */ +const double VAR_POS_PPP = std::pow(100.0, 2.0); /* init variance receiver position (m^2) */ +const double VAR_CLK = std::pow(100.0, 2.0); /* init variance receiver clock (m^2) */ +const double VAR_ZTD = std::pow( 0.3, 2.0); /* init variance ztd (m^2) */ +const double VAR_GRA_PPP = std::pow(0.001, 2.0); /* init variance gradient (m^2) */ +const double VAR_BIAS = std::pow(100.0, 2.0); /* init variance phase-bias (m^2) */ +const double VAR_IONO_OFF = std::pow(10.0, 2.0); /* variance of iono-model-off */ +/* functions originally included in RTKLIB/src/ppp_ar.c v2.4.2*/ double lam_LC(int i, int j, int k); double L_LC(int i, int j, int k, const double *L); @@ -115,8 +95,7 @@ double f_erfc(double x); double conffunc(int N, double B, double sig); -void average_LC(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, - const double *azel); +void average_LC(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, const double *azel); int fix_amb_WL(rtk_t *rtk, const nav_t *nav, int sat1, int sat2, int *NW); @@ -124,39 +103,33 @@ int is_depend(int sat1, int sat2, int *flgs, int *max_flg); int sel_amb(int *sat1, int *sat2, double *N, double *var, int n); -int fix_sol(rtk_t *rtk, const int *sat1, const int *sat2, - const double *NC, int n); +int fix_sol(rtk_t *rtk, const int *sat1, const int *sat2, const double *NC, int n); int fix_amb_ROUND(rtk_t *rtk, int *sat1, int *sat2, const int *NW, int n); int fix_amb_ILS(rtk_t *rtk, int *sat1, int *sat2, int *NW, int n); - -int pppamb(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, - const double *azel); - - +int pppamb(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, const double *azel); +/* functions originally included in RTKLIB/src/ppp.c v2.4.2 */ void pppoutsolstat(rtk_t *rtk, int level, FILE *fp); -void tide_pl(const double *eu, const double *rp, double GMp, - const double *pos, double *dr); +void tide_pl(const double *eu, const double *rp, double GMp, const double *pos, double *dr); void tide_solid(const double *rsun, const double *rmoon, - const double *pos, const double *E, double gmst, int opt, - double *dr); + const double *pos, const double *E, double gmst, int opt, + double *dr); void tide_oload(gtime_t tut, const double *odisp, double *denu); void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar); -void tide_pole(gtime_t tut, const double *pos, const double *erpv, - double *denu); +void tide_pole(gtime_t tut, const double *pos, const double *erpv, double *denu); void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp, - const double *odisp, double *dr); + const double *odisp, double *dr); void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs); @@ -165,19 +138,19 @@ double varerr(int sat, int sys, double el, int type, const prcopt_t *opt); void initx(rtk_t *rtk, double xi, double var, int i); int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel, - const prcopt_t *opt, const double *dantr, const double *dants, - double phw, double *meas, double *var); + const prcopt_t *opt, const double *dantr, const double *dants, + double phw, double *meas, double *var); double gettgd_ppp(int sat, const nav_t *nav); int corr_ion(gtime_t time, const nav_t *nav, int sat, const double *pos, - const double *azel, int ionoopt, double *ion, double *var, - int *brk); + const double *azel, int ionoopt, double *ion, double *var, + int *brk); int corrmeas(const obsd_t *obs, const nav_t *nav, const double *pos, - const double *azel, const prcopt_t *opt, - const double *dantr, const double *dants, double phw, - double *meas, double *var, int *brk); + const double *azel, const prcopt_t *opt, + const double *dantr, const double *dants, double phw, + double *meas, double *var, int *brk); double gfmeas(const obsd_t *obs, const nav_t *nav); @@ -195,17 +168,16 @@ void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); void udstate_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); -void satantpcv(const double *rs, const double *rr, const pcv_t *pcv, - double *dant); +void satantpcv(const double *rs, const double *rr, const pcv_t *pcv, double *dant); double prectrop(gtime_t time, const double *pos, const double *azel, - const prcopt_t *opt, const double *x, double *dtdx, - double *var); + const prcopt_t *opt, const double *x, double *dtdx, + double *var); int res_ppp(int iter, const obsd_t *obs, int n, const double *rs, - const double *dts, const double *vare, const int *svh, - const nav_t *nav, const double *x, rtk_t *rtk, double *v, - double *H, double *R, double *azel); + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const double *x, rtk_t *rtk, double *v, + double *H, double *R, double *azel); int pppnx(const prcopt_t *opt);