From 4f6ba053ef7ae775e4ac3d02212f815d4070f2f2 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 6 May 2017 10:01:54 +0200 Subject: [PATCH] Get PPP working --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 4 +- src/algorithms/libs/rtklib/rtklib.h | 55 +- src/algorithms/libs/rtklib/rtklib_ppp.cc | 2426 ++++++++++--------- src/algorithms/libs/rtklib/rtklib_ppp.h | 207 +- src/algorithms/libs/rtklib/rtklib_rtkcmn.cc | 30 +- src/algorithms/libs/rtklib/rtklib_rtkcmn.h | 2 + src/algorithms/libs/rtklib/rtklib_rtkpos.cc | 305 ++- src/algorithms/libs/rtklib/rtklib_rtkpos.h | 2 +- src/tests/system-tests/position_test.cc | 5 +- 9 files changed, 1639 insertions(+), 1397 deletions(-) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 0dcbf5d18..0a63e7649 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -394,7 +394,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, int nx = 0; /* Number of estimated states */ if(positioning_mode <= PMODE_FIXED) nx = 4 + 3; if(positioning_mode >= PMODE_PPP_KINEMA) nx = NX_PPP(&rtklib_configuration_options); - int na = NR_PPP(&rtklib_configuration_options); + int na = NP_PPP(&rtklib_configuration_options);std::cout << "NP_PPP: " << na << std::endl; double x[nx]; double Px[nx*nx]; double xa[na]; @@ -411,7 +411,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, 3, /* number of continuous fixes of ambiguity */ {ambc_}, /* ambiguity control */ {ssat_}, /* satellite status */ - 128, /* bytes in error message buffer */ + 256, /* bytes in error message buffer */ {'0'}, /* error message buffer */ rtklib_configuration_options /* processing options */ }; diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index 840ff135a..32658807b 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -156,10 +156,17 @@ const int NSATGLO = 0; const int NSYSGLO = 0; #endif +#ifdef ENAGAL const int MINPRNGAL = 1; //!< min satellite PRN number of Galileo const int MAXPRNGAL = 30; //!< max satellite PRN number of Galileo const int NSATGAL = (MAXPRNGAL - MINPRNGAL + 1); //!< number of Galileo satellites const int NSYSGAL = 1; +#else +const int MINPRNGAL = 0; +const int MAXPRNGAL = 0; +const int NSATGAL = 0; +const int NSYSGAL = 0; +#endif #ifdef ENAQZS const int MINPRNQZS = 193; //!< min satellite PRN number of QZSS @@ -261,7 +268,9 @@ const int TROPOPT_SAAS = 1; //!< troposphere option: Saastamoinen model const int TROPOPT_SBAS = 2; //!< troposphere option: SBAS model const int TROPOPT_EST = 3; //!< troposphere option: ZTD estimation const int TROPOPT_ESTG = 4; //!< troposphere option: ZTD+grad estimation -const int TROPOPT_ZTD = 5; //!< troposphere option: ZTD correction +const int TROPOPT_COR = 5; //!< troposphere option: ZTD correction +const int TROPOPT_CORG = 6; //!< troposphere option: ZTD+grad correction + const int EPHOPT_BRDC = 0; //!< ephemeris option: broadcast ephemeris const int EPHOPT_PREC = 1; //!< ephemeris option: precise ephemeris @@ -286,34 +295,26 @@ const int ARMODE_OFF = 0; //!< AR mode: off const int ARMODE_CONT = 1; //!< AR mode: continuous const int ARMODE_INST = 2; //!< AR mode: instantaneous const int ARMODE_FIXHOLD = 3; //!< AR mode: fix and hold -const int ARMODE_WLNL = 4; //!< AR mode: wide lane/narrow lane -const int ARMODE_TCAR = 5; //!< AR mode: triple carrier ar +const int ARMODE_PPPAR = 4; //!< AR mode: PPP-AR +const int ARMODE_PPPAR_ILS = 5; //!< AR mode: AR mode: PPP-AR ILS +const int ARMODE_WLNL = 6; +const int ARMODE_TCAR = 7; + const int POSOPT_RINEX = 3; //!< pos option: rinex header pos */ - -/* number and index of states for PPP */ -#define NF_PPP(opt) ((opt)->ionoopt==IONOOPT_IFLC?1:(opt)->nf) -#define NP_PPP(opt) ((opt)->dynamics?9:3) -#define NC_PPP(opt) (NSYS) -#define NT_PPP(opt) ((opt)->tropopttropopt==TROPOPT_EST?1:3)) -#define NI_PPP(opt) ((opt)->ionoopt==IONOOPT_EST?MAXSAT:0) -#define ND_PPP(opt) ((opt)->nf>=3?1:0) -#define NR_PPP(opt) (NP_PPP(opt)+NC_PPP(opt)+NT_PPP(opt)+NI_PPP(opt)+ND_PPP(opt)) -#define NB_PPP(opt) (NF_PPP(opt)*MAXSAT) -#define NX_PPP(opt) (NR_PPP(opt)+NB_PPP(opt)) -#define IC_PPP(s,opt) (NP_PPP(opt)+(s)) -#define IT_PPP(opt) (NP_PPP(opt)+NC_PPP(opt)) -#define II_PPP(s,opt) (NP_PPP(opt)+NC_PPP(opt)+NT_PPP(opt)+(s)-1) -#define ID_PPP(opt) (NP_PPP(opt)+NC_PPP(opt)+NT_PPP(opt)+NI_PPP(opt)) -#define IB_PPP(s,f,opt) (NR_PPP(opt)+MAXSAT*(f)+(s)-1) - - - - typedef void fatalfunc_t(const char *); //!< fatal callback function type +#define NP_PPP(opt) ((opt)->dynamics?9:3) /* number of pos solution */ +#define IC_PPP(s,opt) (NP_PPP(opt)+(s)) /* state index of clocks (s=0:gps,1:glo) */ +#define IT_PPP(opt) (IC_PPP(0,opt)+NSYS) /* state index of tropos */ +#define NR_PPP(opt) (IT_PPP(opt)+((opt)->tropopttropopt==TROPOPT_EST?1:3))) + /* number of solutions */ +#define IB_PPP(s,opt) (NR_PPP(opt)+(s)-1) /* state index of phase bias */ +#define NX_PPP(opt) (IB_PPP(MAXSAT,opt)+1) /* number of estimated states */ + + typedef struct { /* time struct */ time_t time; /* time (s) expressed by standard time_t */ double sec; /* fraction of second under 1 s */ @@ -457,7 +458,7 @@ typedef struct { /* SBAS ephemeris type */ typedef struct { /* norad two line element data type */ char name [32]; /* common name */ char alias[32]; /* alias name */ - char satno[16]; /* satellite catalog number */ + char satno[16]; /* satellilte catalog number */ char satclass; /* classification */ char desig[16]; /* international designator */ gtime_t epoch; /* element set epoch (UTC) */ @@ -747,8 +748,8 @@ typedef struct { /* solution type */ unsigned char stat; /* solution status (SOLQ_???) */ unsigned char ns; /* number of valid satellites */ float age; /* age of differential (s) */ - float ratio; /* AR ratio factor for validation */ - float thres; /* AR ratio threshold for validation */ + float ratio; /* AR ratio factor for valiation */ + float thres; /* AR ratio threshold for valiation */ } sol_t; @@ -976,7 +977,7 @@ typedef struct { /* RTK control/result type */ double *x, *P; /* float states and their covariance */ double *xa,*Pa; /* fixed states and their covariance */ int nfix; /* number of continuous fixes of ambiguity */ - ambc_t ambc[MAXSAT]; /* ambibuity control */ + ambc_t ambc[MAXSAT]; /* ambiguity control */ ssat_t ssat[MAXSAT]; /* satellite status */ int neb; /* bytes in error message buffer */ char errbuf[MAXERRMSG]; /* error message buffer */ diff --git a/src/algorithms/libs/rtklib/rtklib_ppp.cc b/src/algorithms/libs/rtklib/rtklib_ppp.cc index f7f4cfed1..91b1aa626 100644 --- a/src/algorithms/libs/rtklib/rtklib_ppp.cc +++ b/src/algorithms/libs/rtklib/rtklib_ppp.cc @@ -56,737 +56,1175 @@ #include "rtklib_ephemeris.h" #include "rtklib_ionex.h" #include "rtklib_tides.h" +#include "rtklib_lambda.h" -/* read ppp corrections -------------------------------------------------------- - * read ppp correction data from external file - * args : pppcorr_t *corr IO ppp correction data - * char *file I file - * return : status (1:ok,0:error) - * notes : file types are recognized by file extenstions as follows. - * .stat,.STAT : solution status file by rtklib - * .stec,.STEC : stec parameters file by mgest - * others : sinex troposphere file - * read data are added to ppp correction data. - * To clear data, call pppcorr_free() - *-----------------------------------------------------------------------------*/ -int pppcorr_read(pppcorr_t *corr __attribute__((unused)), const char *file __attribute__((unused))) +/* wave length of LC (m) -----------------------------------------------------*/ + double lam_LC(int i, int j, int k) { - return 0; + 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]; + 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]; + 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) +{ + 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); +} +/* complementaty error function (ref [1] p.227-229) --------------------------*/ + + double p_gamma(double a, double x, double log_gamma_a) +{ + 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); + + 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; + } + return y; +} + double q_gamma(double a, double x, double log_gamma_a) +{ + 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); +} +/* confidence function of integer ambiguity ----------------------------------*/ + double conffunc(int N, double B, double sig) +{ + 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)); + } + return p; +} +/* average LC ----------------------------------------------------------------*/ + 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; + + for (i=0;iopt.elmin) 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); + + sig=sqrt(SQR(rtk->opt.err[1])+SQR(rtk->opt.err[2]/sin(azel[1+2*i]))); + + /* 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; + + 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; + } + /* 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; + + /* wide-lane ambiguity */ +#ifndef REV_WL_FCB + 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]; +#endif + *NW=ROUND(BW); + + /* variance of wide-lane ambiguity */ + vW=(amb1->LCv[0]/amb1->n[0]+amb2->LCv[0]/amb2->n[0])/SQR(lam_WL); + + /* validation of integer wide-lane ambigyity */ + 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 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;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); + + /* 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]; + } + } + /* set flags */ + 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) +{ + 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]; + + C1= SQR(lam2)/(SQR(lam2)-SQR(lam1)); + C2=-SQR(lam1)/(SQR(lam2)-SQR(lam1)); + + NC=zeros(n,1); var=zeros(n,1); + + for (i=0;iopt); + k=IB(sat2[i],&rtk->opt); + + /* narrow-lane ambiguity */ + B1=(rtk->x[j]-rtk->x[k]+C2*lam2*NW[i])/lam_NL; + N1=ROUND(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); + + /* 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; + } + /* select fixed ambiguities by dependancy check */ + m=sel_amb(sat1,sat2,NC,var,m); + + /* fixed solution */ + stat=fix_sol(rtk,sat1,sat2,NC,m); + + free(NC); free(var); + + 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) +{ + 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]; + + C1= SQR(lam2)/(SQR(lam2)-SQR(lam1)); + C2=-SQR(lam1)/(SQR(lam2)-SQR(lam1)); + + 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(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(B1[m]); + + /* 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; + + 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); + + /* 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; + + rtk->sol.ratio=(float)(MIN(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); + + /* narrow-lane to iono-free ambiguity */ + for (i=0;iopt.ionoopt!=IONOOPT_IFLC||rtk->opt.nf<2) return 0; + + 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; + + sat1=imat(n*n,1); sat2=imat(n*n,1); NW=imat(n*n,1); + + /* average LC */ + 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]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++; + } + /* 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); + } + free(sat1); free(sat2); free(NW); + + return stat; } -/* free ppp corrections -------------------------------------------------------- - * free and clear ppp correction data - * args : pppcorr_t *corr IO ppp correction data - * return : none - *-----------------------------------------------------------------------------*/ -void pppcorr_free(pppcorr_t *corr __attribute__((unused))) -{ -} - - -/* get tropospheric correction ------------------------------------------------- - * get tropospheric correction from ppp correcion data - * args : pppcorr_t *corr I ppp correction data - * gtime_t time I time (GPST) - * double *pos I receiver position {lat,lon,heght} (rad,m) - * double *trp O tropos parameters {ztd,grade,gradn} (m) - * double *std O standard deviation (m) - * return : status (1:ok,0:error) - *-----------------------------------------------------------------------------*/ -int pppcorr_trop(const pppcorr_t *corr __attribute__((unused)), gtime_t time __attribute__((unused)), const double *pos __attribute__((unused)), - double *trp __attribute__((unused)), double *std __attribute__((unused))) -{ - return 0; -} - - -int pppcorr_stec(const pppcorr_t *corr __attribute__((unused)), gtime_t time __attribute__((unused)), const double *pos __attribute__((unused)), - double *ion __attribute__((unused)), double *std __attribute__((unused))) -{ - return 0; -} - - -/* ambiguity resolution in ppp -----------------------------------------------*/ -int ppp_ar(rtk_t *rtk __attribute__((unused)), const obsd_t *obs __attribute__((unused)), int n __attribute__((unused)), int *exc __attribute__((unused)), - const nav_t *nav __attribute__((unused)), const double *azel __attribute__((unused)), double *x __attribute__((unused)), double *P __attribute__((unused))) -{ - return 0; -} - - -/* standard deviation of state -----------------------------------------------*/ -double STD(rtk_t *rtk, int i) -{ - if (rtk->sol.stat == SOLQ_FIX) return std::sqrt(rtk->Pa[i+i*rtk->nx]); - return std::sqrt(rtk->P[i+i*rtk->nx]); -} - - -/* write solution status for PPP ---------------------------------------------*/ -int pppoutstat(rtk_t *rtk, char *buff) +void pppoutsolstat(rtk_t *rtk, int level, FILE *fp) { ssat_t *ssat; - double tow, pos[3], vel[3], acc[3], *x; - int i, j, week; - char id[32], *p = buff; + double tow,pos[3],vel[3],acc[3]; + int i,j,week,nfreq=1; + char id[32]; - if (!rtk->sol.stat) return 0; + if (level<=0||!fp) return; - trace(3, "pppoutstat:\n"); + trace(3,"pppoutsolstat:\n"); - tow = time2gpst(rtk->sol.time, &week); - - x = rtk->sol.stat == SOLQ_FIX ? rtk->xa : rtk->x; + tow=time2gpst(rtk->sol.time,&week); /* receiver position */ - p += sprintf(p, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow, - rtk->sol.stat, x[0], x[1], x[2], STD(rtk, 0), STD(rtk, 1), STD(rtk, 2)); + 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); - 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], 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_PPP(0, &rtk->opt); - p += sprintf(p, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", - week, tow, rtk->sol.stat, 1, x[i]*1E9/SPEED_OF_LIGHT, x[i+1]*1E9/SPEED_OF_LIGHT, - STD(rtk, i)*1E9/SPEED_OF_LIGHT, STD(rtk, i+1)*1E9/SPEED_OF_LIGHT); + 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); /* tropospheric parameters */ - if (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG) - { - i = IT_PPP(&rtk->opt); - p += sprintf(p, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, - 1, x[i], STD(rtk, i)); + 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]); } - if (rtk->opt.tropopt == TROPOPT_ESTG) - { - i = IT_PPP(&rtk->opt); - p += sprintf(p, "$TRPG,%d,%.3f,%d,%d,%.5f,%.5f,%.5f,%.5f\n", week, tow, - rtk->sol.stat, 1, x[i+1], x[i+2], STD(rtk, i+1), STD(rtk, i+2)); - } - /* ionosphere parameters */ - if (rtk->opt.ionoopt == IONOOPT_EST) - { - for (i = 0;i < MAXSAT;i++) - { - ssat = rtk->ssat+i; - if (!ssat->vs) continue; - j = II_PPP(i+1, &rtk->opt); - if (rtk->x[j] == 0.0) continue; - satno2id(i+1, id); - p += sprintf(p, "$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n", week, tow, - rtk->sol.stat, id, rtk->ssat[i].azel[0]*R2D, - rtk->ssat[i].azel[1]*R2D, x[j], STD(rtk, j)); - } - } -#ifdef OUTSTAT_AMB - /* ambiguity parameters */ - for (i = 0;i < MAXSAT;i++) for (j = 0;j < NF_PPP(&rtk->opt);j++) - { - k = IB_PPP(i+1, j, &rtk->opt); - if (rtk->x[k] == 0.0) continue; - satno2id(i+1, id); - p += sprintf(p, "$AMB,%d,%.3f,%d,%s,%d,%.4f,%.4f\n", week, tow, - rtk->sol.stat, id, j+1, x[k], STD(rtk, k)); - } -#endif - return (int)(p-buff); + } } - - -/* exclude meas of eclipsing satellite (block IIA) ---------------------------*/ -void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs) +/* solar/lunar tides (ref [2] 7) ---------------------------------------------*/ + void tide_pl(const double *eu, const double *rp, double GMp, + const double *pos, double *dr) { - double rsun[3], esun[3], r, ang, erpv[5] = {0}, cosa; - int i, j; + 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); + + if ((r=norm_rtk(rp,3))<=0.0) return; + + 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]); + + /* 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); + + /* 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); + + /* 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)); + + 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]); +} +/* 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); + + /* step2: frequency domain, only K1 radial */ + 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]; + + /* 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]); +} +/* 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; + + 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]; + + 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) +{ + const double ep2000[]={2000,1,1,0,0,0}; + double y,y2,y3; + + 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; + } +} +/* 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); + + /* ref [7] eq.7.24 */ + 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) */ + + 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) +{ + gtime_t tut; + 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; +#endif + + trace(3,"tidedisp: tutc=%s\n",time_str(tutc,0)); + + if (erp) geterp(erp,tutc,erpv); + + tut=timeadd(tutc,erpv[2]); + + dr[0]=dr[1]=dr[2]=0.0; + + 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); + + if (opt&1) { /* solid earth tides */ + + /* 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; + + /* call DEHANTTIDEINEL */ + dehanttideinel_((double *)rr,&year,&mon,&day,&fhr,rs,rm,drt); +#else + 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]); +} +/* exclude meas of eclipsing satellite (block IIA) ---------------------------*/ + 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; 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;i < n;i++) - { - type = nav->pcvs[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; +#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); - /* only block IIA */ - if (*type && !strstr(type, "BLOCK IIA")) continue; + /* test eclipse */ + if (angRE_WGS84) continue; - /* 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); + trace(2,"eclipsing sat excluded %s sat=%2d\n",time_str(obs[0].time,0), + obs[i].sat); - /* test eclipse */ - if (ang < PI/2.0 || r*sin(ang)>RE_WGS84) continue; - - trace(3, "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; + } } - - -/* nominal yaw-angle ---------------------------------------------------------*/ -double yaw_nominal(double beta, double mu) -{ - if (fabs(beta) < 1E-12 && fabs(mu) < 1E-12) return PI; - return atan2(-tan(beta), sin(mu))+PI; -} - - -/* yaw-angle of satellite ----------------------------------------------------*/ -double yaw_angle(int sat __attribute__((unused)), const char *type __attribute__((unused)), int opt __attribute__((unused)), double beta, double mu, - double *yaw) -{ - *yaw = yaw_nominal(beta,mu); - return 1; -} - - -/* satellite attitude model --------------------------------------------------*/ -int sat_yaw(gtime_t time, int sat, const char *type, int opt, - const double *rs, double *exs, double *eys) -{ - double rsun[3], ri[6], es[3], esun[3], n[3], p[3], en[3], ep[3], ex[3], E, beta, mu; - double yaw, cosy, siny, erpv[5] = {0}; - int i; - - sunmoonpos(gpst2utc(time), erpv, rsun, NULL, NULL); - - /* beta and orbit angle */ - matcpy(ri, rs, 6, 1); - ri[3] -= DEFAULT_OMEGA_EARTH_DOT*ri[1]; - ri[4] += DEFAULT_OMEGA_EARTH_DOT*ri[0]; - cross3(ri, ri+3, n); - cross3(rsun, n, p); - if (!normv3(rs, es) || !normv3(rsun, esun) || !normv3(n, en) || - !normv3(p, ep)) return 0; - beta = PI/2.0-acos(dot(esun, en, 3)); - E = acos(dot(es, ep, 3)); - mu = PI/2.0+(dot(es, esun, 3) <= 0 ? -E : E); - if (mu < -PI/2.0) mu += 2.0*PI; - else if (mu>=PI/2.0) mu -= 2.0*PI; - - /* yaw-angle of satellite */ - if (!yaw_angle(sat, type, opt, beta, mu, &yaw)) return 0; - - /* satellite fixed x, y-vector */ - cross3(en, es, ex); - cosy = cos(yaw); - siny = sin(yaw); - for (i = 0;i < 3;i++) - { - exs[i] = -siny*en[i]+cosy*ex[i]; - eys[i] = -cosy*en[i]-siny*ex[i]; - } - return 1; -} - - -/* phase windup model --------------------------------------------------------*/ -int model_phw(gtime_t time, int sat, const char *type, int opt, - const double *rs, const double *rr, double *phw) -{ - double exs[3], eys[3], ek[3], exr[3], eyr[3], eks[3], ekr[3], E[9]; - double dr[3], ds[3], drs[3], r[3], pos[3], cosp, ph; - int i; - - if (opt <= 0) return 1; /* no phase windup */ - - /* satellite yaw attitude model */ - if (!sat_yaw(time, sat, type, opt, rs, exs, eys)) return 0; - - /* unit vector satellite to receiver */ - for (i = 0;i < 3;i++) r[i] = rr[i]-rs[i]; - if (!normv3(r, ek)) return 0; - - /* unit vectors of receiver antenna */ - ecef2pos(rr, pos); - xyz2enu(pos, E); - exr[0] = E[1]; exr[1] = E[4]; exr[2] = E[7]; /* x = north */ - eyr[0] = -E[0]; eyr[1] = -E[3]; eyr[2] = -E[6]; /* y = west */ - - /* phase windup effect */ - cross3(ek, eys, eks); - cross3(ek, eyr, ekr); - for (i = 0;i < 3;i++) - { - ds[i] = exs[i]-ek[i]*dot(ek, exs, 3)-eks[i]; - dr[i] = exr[i]-ek[i]*dot(ek, exr, 3)+ekr[i]; - } - cosp = dot(ds, dr, 3)/norm_rtk(ds, 3)/norm_rtk(dr, 3); - if (cosp < -1.0) cosp = -1.0; - else if (cosp> 1.0) cosp = 1.0; - ph = acos(cosp)/2.0/PI; - cross3(ds, dr, drs); - if (dot(ek, drs, 3) < 0.0) ph = -ph; - - *phw = ph+floor(*phw-ph+0.5); /* in cycle */ - return 1; -} - - /* measurement error variance ------------------------------------------------*/ -double varerr(int sat __attribute__((unused)), int sys, double el, int freq, int type, - const prcopt_t *opt) + double varerr(int sat, int sys, double el, int type, const prcopt_t *opt) { - double fact = 1.0, sinel = sin(el); + double a,b,a2,b2,fact=1.0; + double sinel=sin(el); + int i=sys==SYS_GLO?1:(sys==SYS_GAL?2:0); - if (type == 1) fact *= opt->eratio[freq == 0 ? 0 : 1]; - fact *= sys == SYS_GLO ? EFACT_GLO : (sys == SYS_SBS ? EFACT_SBS : EFACT_GPS); - - if (sys == SYS_GPS || sys == SYS_QZS) - { - if (freq == 2) fact *= EFACT_GPS_L5; /* GPS/QZS L5 error factor */ + /* 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 (opt->ionoopt == IONOOPT_IFLC) fact *= 3.0; - return std::pow(fact*opt->err[1], 2.0)+std::pow(fact*opt->err[2]/sinel, 2.0); + } + 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 { /* 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;j < rtk->nx;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; + } } - - -/* geometry-free phase measurement -------------------------------------------*/ -double gfmeas(const obsd_t *obs, const nav_t *nav) +/* 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]; - int i = (satsys(obs->sat, NULL)&(SYS_GAL|SYS_SBS)) ? 2 : 1; + 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; - if (lam[0] == 0.0 || lam[i] == 0.0 || obs->L[0] == 0.0 || obs->L[i] == 0.0) return 0.0; - return lam[0]*obs->L[0]-lam[i]*obs->L[i]; + 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<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) */ + + 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; + + /* 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 (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]; + } + /* 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]; + } + return 1; } - - -/* Melbourne-Wubbena linear combination --------------------------------------*/ -double mwmeas(const obsd_t *obs, const nav_t *nav) +/* get tgd parameter (m) -----------------------------------------------------*/ + double gettgd_ppp(int sat, const nav_t *nav) { - const double *lam = nav->lam[obs->sat-1]; - int i = (satsys(obs->sat, NULL)&(SYS_GAL|SYS_SBS)) ? 2 : 1; - - if (lam[0] == 0.0 || lam[i] == 0.0 || obs->L[0] == 0.0 || obs->L[i] == 0.0 || - obs->P[0] == 0.0 || obs->P[i] == 0.0) return 0.0; - return lam[0]*lam[i]*(obs->L[0]-obs->L[i])/(lam[i]-lam[0])- - (lam[i]*obs->P[0]+lam[0]*obs->P[i])/(lam[i]+lam[0]); + int i; + for (i=0;in;i++) { + if (nav->eph[i].sat!=sat) continue; + return SPEED_OF_LIGHT*nav->eph[i].tgd[0]; + } + return 0.0; } - - -/* antenna corrected measurements --------------------------------------------*/ -void corr_meas(const obsd_t *obs, const nav_t *nav, const double *azel, - const prcopt_t *opt, const double *dantr, - const double *dants, double phw, double *L, double *P, - double *Lc, double *Pc) +/* 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) { - const double *lam = nav->lam[obs->sat-1]; - double C1, C2; - int i, sys; - - for (i = 0;i < NFREQ;i++) - { - L[i] = P[i] = 0.0; - if (lam[i] == 0.0 || obs->L[i] == 0.0 || obs->P[i] == 0.0) continue; - if (testsnr(0, 0, azel[1], obs->SNR[i]*0.25, &opt->snrmask)) continue; - - /* antenna phase center and phase windup correction */ - L[i] = obs->L[i]*lam[i]-dants[i]-dantr[i]-phw*lam[i]; - P[i] = obs->P[i] -dants[i]-dantr[i]; - - /* P1-C1, P2-C2 dcb correction (C1->P1, C2->P2) */ - if (obs->code[i] == CODE_L1C) - { - P[i] += nav->cbias[obs->sat-1][1]; - } - else if (obs->code[i] == CODE_L2C || obs->code[i] == CODE_L2X || - obs->code[i] == CODE_L2L || obs->code[i] == CODE_L2S) - { - P[i] += nav->cbias[obs->sat-1][2]; -#if 0 - L[i] -= 0.25*lam[i]; /* 1/4 cycle-shift */ +#ifdef EXTSTEC + double rate; #endif - } - } + /* sbas ionosphere model */ + 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); + } +#ifdef EXTSTEC + /* slant tec model */ + 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; + } + /* ionosphere model 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) +{ + 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"); + + meas[0]=meas[1]=var[0]=var[1]=0.0; + /* iono-free LC */ - *Lc = *Pc = 0.0; - sys = satsys(obs->sat, NULL); - i = (sys&(SYS_GAL|SYS_SBS)) ? 2 : 1; /* L1/L2 or L1/L5 */ - if (lam[0] == 0.0 || lam[i] == 0.0) return; + 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; - C1 = std::pow(lam[i], 2.0)/(std::pow(lam[i], 2.0)-std::pow(lam[0], 2.0)); - C2 = -std::pow(lam[0], 2.0)/(std::pow(lam[i], 2.0)-std::pow(lam[0], 2.0)); + if (testsnr(0,0,azel[1],obs->SNR[0]*0.25,&opt->snrmask)) return 0; -#if 0 - /* P1-P2 dcb correction (P1->Pc,P2->Pc) */ - if (sys&(SYS_GPS|SYS_GLO|SYS_QZS)) - { - if (P[0]!=0.0) P[0] -= C2*nav->cbias[obs->sat-1][0]; - if (P[1]!=0.0) P[1] += C1*nav->cbias[obs->sat-1][0]; - } -#endif - if (L[0] != 0.0 && L[i] != 0.0) *Lc = C1*L[0]+C2*L[i]; - if (P[0] != 0.0 && P[i] != 0.0) *Pc = C1*P[0]+C2*P[i]; + 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 */ + + /* 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; + } + /* ionosphere and windup corrected phase and code */ + meas[0]=L1+ion-lam[0]*phw; + meas[1]=PC-ion; + + var[0]+=vari; + var[1]+=vari+SQR(ERR_CBIAS); + + /* antenna phase center variation correction */ + for (i=0;i<2;i++) { + if (dants) meas[i]-=dants[0]; + if (dantr) meas[i]-=dantr[0]; + } + return 1; } - - -/* detect cycle slip by LLI --------------------------------------------------*/ -void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n) +/* L1/L2 geometry-free phase measurement -------------------------------------*/ + double gfmeas(const obsd_t *obs, const nav_t *nav) { - int i, j; + const double *lam=nav->lam[obs->sat-1]; - trace(3, "detslp_ll: n=%d\n", n); + if (lam[0]==0.0||lam[1]==0.0||obs->L[0]==0.0||obs->L[1]==0.0) return 0.0; - for (i = 0;i < n && i < MAXOBS;i++) for (j = 0;j < rtk->opt.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); - - rtk->ssat[obs[i].sat-1].slip[j] = 1; - } + return lam[0]*obs->L[0]-lam[1]*obs->L[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;i < n && i < MAXOBS;i++) - { - if ((g1 = gfmeas(obs+i, nav)) == 0.0) continue; - - g0 = rtk->ssat[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;j < rtk->opt.nf;j++) rtk->ssat[obs[i].sat-1].slip[j]|=1; - } - } -} - - -/* detect slip by Melbourne-Wubbena linear combination jump ------------------*/ -void detslp_mw(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) -{ - double w0, w1; - int i, j; - - trace(3, "detslp_mw: n=%d\n", n); - - for (i = 0;i < n && i < MAXOBS;i++) - { - if ((w1 = mwmeas(obs+i, nav)) == 0.0) continue; - - w0 = rtk->ssat[obs[i].sat-1].mw; - rtk->ssat[obs[i].sat-1].mw = w1; - - trace(4, "detslip_mw: sat=%2d mw0=%8.3f mw1=%8.3f\n", obs[i].sat, w0, w1); - - if (w0 != 0.0 && fabs(w1-w0)>THRES_MW_JUMP) - { - trace(3, "detslip_mw: slip detected sat=%2d mw=%8.3f->%8.3f\n", - obs[i].sat, w0, w1); - - for (j = 0;j < rtk->opt.nf;j++) rtk->ssat[obs[i].sat-1].slip[j]|=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_PPP, i); - } + if (norm_rtk(rtk->x,3)<=0.0) { + for (i=0;i<3;i++) initx(rtk,rtk->sol.rr[i],VAR_POS,i); + } /* static ppp mode */ - if (rtk->opt.mode == PMODE_PPP_STATIC) - { - for (i = 0;i < 3;i++) - { - rtk->P[i*(1+rtk->nx)] += std::pow(rtk->opt.prn[5], 2.0)*fabs(rtk->tt); - } - 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_PPP, i); - } + for (i=0;i<3;i++) { + initx(rtk,rtk->sol.rr[i],VAR_POS,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;i < NSYS;i++) { - if (rtk->opt.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)); + 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(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 = i+1;j < i+3;j++) initx(rtk, 1E-6, VAR_GRA_PPP, j); - } + if (rtk->opt.tropopt>=TROPOPT_ESTG) { + for (j=0;j<2;j++) initx(rtk,1E-6,VAR_GRA,++i); } - else - { - rtk->P[i+i*rtk->nx] += std::pow(rtk->opt.prn[2], 2.0)*fabs(rtk->tt); + } + else { + rtk->P[i*(1+rtk->nx)]+=SQR(rtk->opt.prn[2])*fabs(rtk->tt); - if (rtk->opt.tropopt>=TROPOPT_ESTG) - { - for (j = i+1;j < i+3;j++) - { - rtk->P[j+j*rtk->nx] += std::pow(rtk->opt.prn[2]*0.1, 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); + } } + } } - - -/* temporal update of ionospheric parameters ---------------------------------*/ -void udiono_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +/* detect cycle slip by LLI --------------------------------------------------*/ + void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n) { - const double *lam; - double ion, sinel, pos[3], *azel; - char *p; - int i, j, k, gap_resion = GAP_RESION; + int i,j; - trace(3, "udiono_ppp:\n"); + trace(3,"detslp_ll: n=%d\n",n); - if ((p = strstr(rtk->opt.pppopt, "-GAP_RESION="))) - { - sscanf(p, "-GAP_RESION=%d", &gap_resion); - } - for (i = 0;i < MAXSAT;i++) - { - j = II_PPP(i+1, &rtk->opt); - if (rtk->x[j] != 0.0 && (int)rtk->ssat[i].outc[0]>gap_resion) - { - rtk->x[j] = 0.0; - } - } - for (i = 0;i < n;i++) - { - j = II_PPP(obs[i].sat, &rtk->opt); - if (rtk->x[j] == 0.0) - { - k = satsys(obs[i].sat, NULL) == SYS_GAL ? 2 : 1; - lam = nav->lam[obs[i].sat-1]; - if (obs[i].P[0] == 0.0 || obs[i].P[k] == 0.0 || lam[0] == 0.0 || lam[k] == 0.0) - { - continue; - } - ion = (obs[i].P[0]-obs[i].P[k])/(1.0-std::pow(lam[k]/lam[0], 2.0)); - ecef2pos(rtk->sol.rr, pos); - azel = rtk->ssat[obs[i].sat-1].azel; - ion/=ionmapf(pos, azel); - initx(rtk, ion, VAR_IONO, j); - } - else - { - sinel = sin(MAX_PPP(rtk->ssat[obs[i].sat-1].azel[1], 5.0*D2R)); - rtk->P[j+j*rtk->nx] += std::pow(rtk->opt.prn[1]/sinel, 2.0)*fabs(rtk->tt); - } - } + 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); + + rtk->ssat[obs[i].sat-1].slip[j]=1; + } } - - -/* temporal update of L5-receiver-dcb parameters -----------------------------*/ -void uddcb_ppp(rtk_t *rtk) +/* detect cycle slip by geometry free phase jump -----------------------------*/ + void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) { - int i = ID_PPP(&rtk->opt); + double g0,g1; + int i,j; - trace(3, "uddcb_ppp:\n"); + trace(3,"detslp_gf: n=%d\n",n); - if (rtk->x[i] == 0.0) - { - initx(rtk, 1E-6, VAR_DCB, i); + 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) + void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) { - const double *lam; - double L[NFREQ], P[NFREQ], Lc, Pc, bias[MAXOBS], offset = 0.0, pos[3] = {0}; - double ion, dantr[NFREQ] = {0}, dants[NFREQ] = {0}; - int i, j, k, l, f, sat, slip[MAXOBS] = {0}, clk_jump = 0; + 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); + trace(3,"udbias : n=%d\n",n); - /* handle day-boundary clock jump */ - if (rtk->opt.posopt[5]) - { - clk_jump = ROUND_PPP(time2gpst(obs[0].time, NULL)*10) % 864000 == 0; - } - for (i = 0;i < MAXSAT;i++) for (j = 0;j < rtk->opt.nf;j++) - { - rtk->ssat[i].slip[j] = 0; - } + 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); - /* detect slip by Melbourne-Wubbena linear combination jump */ - detslp_mw(rtk, obs, n, nav); - - ecef2pos(rtk->sol.rr, pos); - - for (f = 0;f < NF_PPP(&rtk->opt);f++) - { - /* reset phase-bias if expire obs outage counter */ - for (i = 0;i < MAXSAT;i++) - { - if (++rtk->ssat[i].outc[f] > (unsigned int)rtk->opt.maxout || - rtk->opt.modear == ARMODE_INST || clk_jump) - { - initx(rtk, 0.0, 0.0, IB_PPP(i+1, f, &rtk->opt)); - } - } - for (i = k = 0;i < n && i < MAXOBS;i++) - { - sat = obs[i].sat; - j = IB_PPP(sat, f, &rtk->opt); - corr_meas(obs+i, nav, rtk->ssat[sat-1].azel, &rtk->opt, dantr, dants, - 0.0, L, P, &Lc, &Pc); - - bias[i] = 0.0; - - if (rtk->opt.ionoopt == IONOOPT_IFLC) - { - bias[i] = Lc-Pc; - slip[i] = rtk->ssat[sat-1].slip[0] || rtk->ssat[sat-1].slip[1]; - } - else if (L[f] != 0.0 && P[f] != 0.0) - { - slip[i] = rtk->ssat[sat-1].slip[f]; - l = satsys(sat, NULL) == SYS_GAL ? 2 : 1; - lam = nav->lam[sat-1]; - if (obs[i].P[0] == 0.0 || obs[i].P[l] == 0.0 || - lam[0] == 0.0 || lam[l] == 0.0 || lam[f] == 0.0) continue; - ion = (obs[i].P[0]-obs[i].P[l])/(1.0-std::pow(lam[l]/lam[0], 2.0)); - bias[i] = L[f]-P[f]+2.0*ion*std::pow(lam[f]/lam[0], 2.0); - } - if (rtk->x[j] == 0.0 || slip[i] || bias[i] == 0.0) continue; - - offset += bias[i]-rtk->x[j]; - k++; - } - /* correct phase-code jump to ensure phase-code coherency */ - if (k>=2 && fabs(offset/k)>0.0005*SPEED_OF_LIGHT) - { - for (i = 0;i < MAXSAT;i++) - { - j = IB_PPP(i+1, f, &rtk->opt); - 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); - } - for (i = 0;i < n && i < MAXOBS;i++) - { - sat = obs[i].sat; - j = IB_PPP(sat, f, &rtk->opt); - - rtk->P[j+j*rtk->nx] += std::pow(rtk->opt.prn[0], 2.0)*fabs(rtk->tt); - - if (bias[i] == 0.0 || (rtk->x[j] != 0.0 && !slip[i])) continue; - - /* reinitialize phase-bias if detecting cycle slip */ - initx(rtk, bias[i], VAR_BIAS, IB_PPP(sat, f, &rtk->opt)); - - /* reset fix flags */ - for (k = 0;k < MAXSAT;k++) rtk->ambc[sat-1].flags[k] = 0; - - trace(5, "udbias_ppp: sat=%2d bias=%.3f\n", sat, bias[i]); - } + /* 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)); } + } + 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; + + 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++; + } + /* 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; + } + 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); + + rtk->P[j+j*rtk->nx]+=SQR(rtk->opt.prn[0])*fabs(rtk->tt); + + 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; + + /* reinitialize phase-bias if detecting cycle slip */ + initx(rtk,bias[i],VAR_BIAS,IB(sat,&rtk->opt)); + + 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); @@ -795,558 +1233,280 @@ void udstate_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) udclk_ppp(rtk); /* temporal update of tropospheric parameters */ - if (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG) - { - udtrop_ppp(rtk); - } - /* temporal update of ionospheric parameters */ - if (rtk->opt.ionoopt == IONOOPT_EST) - { - udiono_ppp(rtk, obs, n, nav); - } - /* temporal update of L5-receiver-dcb parameters */ - if (rtk->opt.nf>=3) - { - uddcb_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 trop_model_prec(gtime_t time, const double *pos, - const double *azel, 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 (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); + 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); return m_h*zhd+m_w*(x[0]-zhd); } - - -/* tropospheric model ---------------------------------------------------------*/ -int model_trop(gtime_t time, const double *pos, const double *azel, - const prcopt_t *opt, const double *x, double *dtdx, - const nav_t *nav, double *dtrp, double *var) +/* 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) { - double trp[3] = {0}, std[3]; + 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; - if (opt->tropopt == TROPOPT_SAAS) - { - *dtrp = tropmodel(time, pos, azel, REL_HUMI); - *var = std::pow(ERR_SAAS, 2.0); - return 1; + trace(3,"res_ppp : n=%d nx=%d\n",n,nx); + + for (i=0;issat[i].vsat[0]=0; + + 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 */ + + 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); } - if (opt->tropopt == TROPOPT_SBAS) - { - *dtrp = sbstropcorr(time, pos, azel, var); - return 1; + else if (opt->tropopt==TROPOPT_SBAS) { + dtrp=sbstropcorr(obs[i].time,pos,azel+i*2,&vart); } - if (opt->tropopt == TROPOPT_EST || opt->tropopt == TROPOPT_ESTG) - { - matcpy(trp, x+IT_PPP(opt), opt->tropopt == TROPOPT_EST ? 1 : 3, 1); - *dtrp = trop_model_prec(time, pos, azel, trp, dtdx, var); - return 1; + 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); } - if (opt->tropopt == TROPOPT_ZTD) - { - if (pppcorr_trop(&nav->pppcorr, time, pos, trp, std)) - { - *dtrp = trop_model_prec(time, pos, azel, trp, dtdx, var); - *var = std::pow(dtdx[0]*std[0], 2.0); - return 1; + 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); + + /* 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; + + 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]; } - } - return 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; + if (j==0) rtk->ssat[sat-1].resc[0]=v[nv]; + else rtk->ssat[sat-1].resp[0]=v[nv]; -/* ionospheric model ---------------------------------------------------------*/ -int model_iono(gtime_t time, const double *pos, const double *azel, - const prcopt_t *opt, int sat, const double *x, - const nav_t *nav, double *dion, double *var) -{ - static double iono_p[MAXSAT] = {0}, std_p[MAXSAT] = {0}; - static gtime_t time_p; - - if (opt->ionoopt == IONOOPT_SBAS) - { - return sbsioncorr(time, nav, pos, azel, dion, var); - } - if (opt->ionoopt == IONOOPT_TEC) - { - return iontec(time, nav, pos, azel, 1, dion, var); - } - if (opt->ionoopt == IONOOPT_BRDC) - { - *dion = ionmodel(time, nav->ion_gps, pos, azel); - *var = std::pow(*dion*ERR_BRDCI, 2.0); - return 1; - } - if (opt->ionoopt == IONOOPT_EST) - { - *dion = x[II_PPP(sat, opt)]; - *var = 0.0; - return 1; - } - if (opt->ionoopt == IONOOPT_IFLC) - { - *dion = *var = 0.0; - return 1; - } - if (opt->ionoopt == IONOOPT_STEC) - { - if (timediff(time, time_p) != 0.0 && - !pppcorr_stec(&nav->pppcorr, time, pos, iono_p, std_p)) return 0; - if (iono_p[sat-1] == 0.0 || std_p[sat-1]>0.1) return 0; - time_p = time; - *dion = iono_p[sat-1]; - *var = std::pow(std_p[sat-1], 2.0); - return 1; - } - return 0; -} - - -/* constraint to local correction --------------------------------------------*/ -int const_corr(const obsd_t *obs, int n, const int *exc, - const nav_t *nav, const double *x, const double *pos, - const double *azel __attribute__((unused)), rtk_t *rtk, double *v, double *H, - double *var) -{ - gtime_t time = obs[0].time; - double trop[3], std_trop[3], iono[MAXSAT], std_iono[MAXSAT]; - int i, j, k, sat, nv = 0; - - /* constraint to external troposphere correction */ - if ((rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG) && - pppcorr_trop(&nav->pppcorr, time, pos, trop, std_trop)) - { - - for (i = 0;i < (rtk->opt.tropopt == TROPOPT_EST ? 1 : 3);i++) - { - if (std_trop[i] == 0.0) continue; - j = IT_PPP(&rtk->opt)+i; - v[nv] = trop[i]-x[j]; - for (k = 0;k < rtk->nx;k++) H[k+nv*rtk->nx] = k == j ? 1.0 : 0.0; - var[nv++] = std::pow(std_trop[i], 2.0); - } - } - /* constraint to external ionosphere correction */ - if (rtk->opt.ionoopt == IONOOPT_EST && - pppcorr_stec(&nav->pppcorr, time, pos, iono, std_iono)) - { - - for (i = 0;i < n;i++) - { - sat = obs[i].sat; - if (exc[i] || iono[sat-1] == 0.0 || std_iono[sat-1]>0.5) continue; - j = II_PPP(sat, &rtk->opt); - v[nv] = iono[sat-1]-x[j]; - for (k = 0;k < rtk->nx;k++) H[k+nv*rtk->nx] = k == j ? 1.0 : 0.0; - var[nv++] = std::pow(std_iono[sat-1], 2.0); - } + /* test innovation */ +#if 0 + if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) { +#else + 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++; } + } + for (i=0;iopt; - double y, r, cdtr, bias, C, rr[3], pos[3], e[3], dtdx[3], L[NFREQ], P[NFREQ], Lc, Pc; - double var[MAXOBS*2], dtrp = 0.0, dion = 0.0, vart = 0.0, vari = 0.0, dcb; - double dantr[NFREQ] = {0}, dants[NFREQ] = {0}; - double ve[MAXOBS*2*NFREQ] = {0}, vmax = 0; - char str[32]; - int ne = 0, obsi[MAXOBS*2*NFREQ] = {0}, frqi[MAXOBS*2*NFREQ], maxobs, maxfrq, rej; - int i, j, k, sat, sys, nv = 0, nx = rtk->nx, stat = 1; - - time2str(obs[0].time, str, 2); - - for (i = 0;i < MAXSAT;i++) for (j = 0;j < opt->nf;j++) rtk->ssat[i].vsat[j] = 0; - - for (i = 0;i < 3;i++) rr[i] = x[i]+dr[i]; - ecef2pos(rr, pos); - - for (i = 0;i < n && i < MAXOBS;i++) - { - sat = obs[i].sat; - lam = nav->lam[sat-1]; - if (lam[j/2] == 0.0 || lam[0] == 0.0) continue; - - if ((r = geodist(rs+i*6, rr, e)) <= 0.0 || - satazel(pos, e, azel+i*2) < opt->elmin) - { - exc[i] = 1; - continue; - } - if (!(sys = satsys(sat, NULL)) || !rtk->ssat[sat-1].vs || - satexclude(obs[i].sat, svh[i], opt) || exc[i]) - { - exc[i] = 1; - continue; - } - /* tropospheric and ionospheric model */ - if (!model_trop(obs[i].time, pos, azel+i*2, opt, x, dtdx, nav, &dtrp, &vart) || - !model_iono(obs[i].time, pos, azel+i*2, opt, sat, x, nav, &dion, &vari)) - { - continue; - } - /* satellite and receiver antenna model */ - if (opt->posopt[0]) satantpcv(rs+i*6, rr, nav->pcvs+sat-1, dants); - antmodel(opt->pcvr, opt->antdel[0], azel+i*2, opt->posopt[1], dantr); - - /* phase windup model */ - if (!model_phw(rtk->sol.time, sat, nav->pcvs[sat-1].type, - opt->posopt[2] ? 2 : 0, rs+i*6, rr, &rtk->ssat[sat-1].phw)) - { - continue; - } - /* corrected phase and code measurements */ - corr_meas(obs+i, nav, azel+i*2, &rtk->opt, dantr, dants, - rtk->ssat[sat-1].phw, L, P, &Lc, &Pc); - - /* stack phase and code residuals {L1, P1, L2, P2, ...} */ - for (j = 0;j < 2*NF_PPP(opt);j++) - { - dcb = bias = 0.0; - - if (opt->ionoopt == IONOOPT_IFLC) - { - if ((y = j % 2 == 0 ? Lc : Pc) == 0.0) continue; - } - else - { - if ((y = j % 2 == 0 ? L[j/2] : P[j/2]) == 0.0) continue; - - /* receiver DCB correction for P2 */ - if (j/2 == 1) dcb = -nav->rbias[0][sys == SYS_GLO ? 1 : 0][0]; - } - C = std::pow(lam[j/2]/lam[0], 2.0)*ionmapf(pos, azel+i*2)*(j % 2 == 0 ? -1.0 : 1.0); - - for (k = 0;k < nx;k++) H[k+nx*nv] = k < 3 ? -e[k] : 0.0; - - /* receiver clock */ - k = sys == SYS_GLO ? 1 : (sys == SYS_GAL ? 2 : (sys == SYS_BDS ? 3 : 0)); - cdtr = x[IC_PPP(k, opt)]; - H[IC_PPP(k, opt)+nx*nv] = 1.0; - - if (opt->tropopt == TROPOPT_EST || opt->tropopt == TROPOPT_ESTG) - { - for (k = 0;k < (opt->tropopt>=TROPOPT_ESTG ? 3 : 1);k++) - { - H[IT_PPP(opt)+k+nx*nv] = dtdx[k]; - } - } - if (opt->ionoopt == IONOOPT_EST) - { - if (rtk->x[II_PPP(sat, opt)] == 0.0) continue; - H[II_PPP(sat, opt)+nx*nv] = C; - } - if (j/2 == 2 && j % 2 == 1) - { /* L5-receiver-dcb */ - dcb += rtk->x[ID_PPP(opt)]; - H[ID_PPP(opt)+nx*nv] = 1.0; - } - if (j % 2 == 0) - { /* phase bias */ - if ((bias = x[IB_PPP(sat, j/2, opt)]) == 0.0) continue; - H[IB_PPP(sat, j/2, opt)+nx*nv] = 1.0; - } - /* residual */ - v[nv] = y-(r+cdtr-SPEED_OF_LIGHT*dts[i*2]+dtrp+C*dion+dcb+bias); - - if (j % 2 == 0) rtk->ssat[sat-1].resc[j/2] = v[nv]; - else rtk->ssat[sat-1].resp[j/2] = v[nv]; - - /* variance */ - var[nv] = varerr(obs[i].sat, sys, azel[1+i*2], j/2, j % 2, opt)+ - vart+std::pow(C, 2.0)*vari+var_rs[i]; - if (sys == SYS_GLO && j % 2 == 1) var[nv] += VAR_GLO_IFB; - - trace(4,"%s sat=%2d %s%d res=%9.4f sig=%9.4f el=%4.1f\n", str, sat, - j % 2 ? "P" : "L", j/2+1, v[nv], sqrt(var[nv]), azel[1+i*2]*R2D); - - /* reject satellite by pre-fit residuals */ - if (!post && opt->maxinno>0.0 && fabs(v[nv])>opt->maxinno) - { - trace(2, "outlier (%d) rejected %s sat=%2d %s%d res=%9.4f el=%4.1f\n", - post, str, sat, j % 2 ? "P" : "L", j/2+1, v[nv], azel[1+i*2]*R2D); - exc[i] = 1; rtk->ssat[sat-1].rejc[j % 2]++; - continue; - } - /* record large post-fit residuals */ - if (post && fabs(v[nv])>sqrt(var[nv])*THRES_REJECT) - { - obsi[ne] = i; frqi[ne] = j; ve[ne] = v[nv]; ne++; - } - if (j % 2 == 0) rtk->ssat[sat-1].vsat[j/2] = 1; - nv++; - } - } - /* reject satellite with large and max post-fit residual */ - if (post && ne>0) - { - vmax = ve[0]; maxobs = obsi[0]; maxfrq = frqi[0]; rej = 0; - for (j = 1;j < ne;j++) - { - if (fabs(vmax)>=fabs(ve[j])) continue; - vmax = ve[j]; maxobs = obsi[j]; maxfrq = frqi[j]; rej = j; - } - sat = obs[maxobs].sat; - trace(2, "outlier (%d) rejected %s sat=%2d %s%d res=%9.4f el=%4.1f\n", - post, str, sat, maxfrq % 2 ? "P" : "L", maxfrq/2+1, vmax, azel[1+maxobs*2]*R2D); - exc[maxobs] = 1; rtk->ssat[sat-1].rejc[maxfrq % 2]++; stat = 0; - ve[rej] = 0; - } - /* constraint to local correction */ - nv += const_corr(obs, n, exc, nav, x, pos, azel, rtk, v+nv, H+nv*rtk->nx, var+nv); - - for (i = 0;i < nv;i++) for (j = 0;j < nv;j++) - { - R[i+j*nv] = i == j ? var[i] : 0.0; - } - return post ? stat : nv; -} - - /* number of estimated states ------------------------------------------------*/ int pppnx(const prcopt_t *opt) { - return NX_PPP(opt); + return NX_PPP(opt); } - - -/* update solution status ----------------------------------------------------*/ -void update_stat(rtk_t *rtk, const obsd_t *obs, int n, int stat) -{ - const prcopt_t *opt = &rtk->opt; - int i, j; - - /* test # of valid satellites */ - rtk->sol.ns = 0; - for (i = 0;i < n && i < MAXOBS;i++) - { - for (j = 0;j < opt->nf;j++) - { - if (!rtk->ssat[obs[i].sat-1].vsat[j]) continue; - rtk->ssat[obs[i].sat-1].lock[j]++; - rtk->ssat[obs[i].sat-1].outc[j] = 0; - if (j == 0) rtk->sol.ns++; - } - } - rtk->sol.stat = rtk->sol.ns < MIN_NSAT_SOL ? SOLQ_NONE : stat; - - if (rtk->sol.stat == SOLQ_FIX) - { - for (i = 0;i < 3;i++) - { - rtk->sol.rr[i] = rtk->xa[i]; - rtk->sol.qr[i] = (float)rtk->Pa[i+i*rtk->na]; - } - rtk->sol.qr[3] = (float)rtk->Pa[1]; - rtk->sol.qr[4] = (float)rtk->Pa[1+2*rtk->na]; - rtk->sol.qr[5] = (float)rtk->Pa[2]; - } - else - { - 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++) for (j = 0;j < opt->nf;j++) - { - rtk->ssat[obs[i].sat-1].snr[j] = obs[i].SNR[j]; - } - for (i = 0;i < MAXSAT;i++) for (j = 0;j < opt->nf;j++) - { - if (rtk->ssat[i].slip[j]&3) rtk->ssat[i].slipc[j]++; - if (rtk->ssat[i].fix[j] == 2 && stat != SOLQ_FIX) rtk->ssat[i].fix[j] = 1; - } -} - - -/* test hold ambiguity -------------------------------------------------------*/ -int test_hold_amb(rtk_t *rtk) -{ - int i, j, stat = 0; - - /* no fix-and-hold mode */ - if (rtk->opt.modear != ARMODE_FIXHOLD) return 0; - - /* reset # of continuous fixed if new ambiguity introduced */ - for (i = 0;i < MAXSAT;i++) - { - if (rtk->ssat[i].fix[0] != 2 && rtk->ssat[i].fix[1] != 2) continue; - for (j = 0;j < MAXSAT;j++) - { - if (rtk->ssat[j].fix[0] != 2 && rtk->ssat[j].fix[1] != 2) continue; - if (!rtk->ambc[j].flags[i] || !rtk->ambc[i].flags[j]) stat = 1; - rtk->ambc[j].flags[i] = rtk->ambc[i].flags[j] = 1; - } - } - if (stat) - { - rtk->nfix = 0; - return 0; - } - /* test # of continuous fixed */ - return ++rtk->nfix >= rtk->opt.minfix; -} - - /* precise point positioning -------------------------------------------------*/ -void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) + void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) { - const prcopt_t *opt = &rtk->opt; - double *rs, *dts, *var, *v, *H, *R, *azel, *xp, *Pp, dr[3] = {0}, std[3]; - char str[32]; - int i, j, nv, info, svh[MAXOBS], exc[MAXOBS] = {0}, 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; - time2str(obs[0].time, str, 2); - trace(3, "pppos : time=%s nx=%d n=%d\n", str, 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;i < MAXSAT;i++) for (j = 0;j < opt->nf;j++) rtk->ssat[i].fix[j] = 0; + for (i=0;issat[i].fix[0]=0; - /* temporal update of ekf states */ - udstate_ppp(rtk, obs, n, nav); + /* temporal update of states */ + udstate_ppp(rtk,obs,n,nav); + + trace(4,"x(0)="); tracemat(4,rtk->x,1,NR(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 (block IIA) */ - if (rtk->opt.posopt[3]) - { - testeclipse(obs, n, nav, rs); + /* 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; } - /* earth tides correction */ - if (opt->tidecorr) - { - tidedisp(gpst2utc(obs[0].time), rtk->x, opt->tidecorr == 1 ? 1 : 7, &nav->erp, - opt->odisp[0], dr); + trace(4,"x(%d)=",i+1); tracemat(4,xp,1,NR(opt),13,4); + + 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); + + /* 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; } - nv = n*rtk->opt.nf*2+MAXSAT+3; - xp = mat(rtk->nx, 1); Pp = zeros(rtk->nx, rtk->nx); - v = mat(nv, 1); H = mat(rtk->nx, nv); R = mat(nv, nv); - - for (i = 0;i < MAX_ITER;i++) - { - matcpy(xp, rtk->x, rtk->nx, 1); - matcpy(Pp, rtk->P, rtk->nx, rtk->nx); - - /* prefit residuals */ - if (!(nv = ppp_res(0, obs, n, rs, dts, var, svh, dr, exc, nav, xp, rtk, v, H, R, azel))) - { - trace(2, "%s ppp (%d) no valid obs data\n", str, i+1); - break; - } - /* measurement update of ekf states */ - if ((info = filter(xp, Pp, H, v, R, rtk->nx, nv))) - { - trace(2, "%s ppp (%d) filter error info=%d\n", str, i+1, info); - break; - } - /* postfit residuals */ - if (ppp_res(i+1, obs, n, rs, dts, var, svh, dr, exc, nav, xp, rtk, v, H, R, azel)) - { - matcpy(rtk->x, xp, rtk->nx, 1); - matcpy(rtk->P, Pp, rtk->nx, rtk->nx); - stat = SOLQ_PPP; - break; - } + /* 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++; } - if (i>=MAX_ITER) - { - trace(2, "%s ppp (%d) iteration overflows\n", str, i); + 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]; } - if (stat == SOLQ_PPP) - { - - /* ambiguity resolution in ppp */ - if (ppp_ar(rtk, obs, n, exc, nav, azel, xp, Pp) && - ppp_res(9, obs, n, rs, dts, var, svh, dr, exc, nav, xp, rtk, v, H, R, azel)) - { - matcpy(rtk->xa, xp, rtk->nx, 1); - matcpy(rtk->Pa, Pp, rtk->nx, rtk->nx); - - for (i = 0;i < 3;i++) std[i] = sqrt(Pp[i+i*rtk->nx]); - if (norm_rtk(std, 3) < MAX_STD_FIX) stat = SOLQ_FIX; - } - else - { - rtk->nfix = 0; - } - /* update solution status */ - update_stat(rtk, obs, n, stat); - - /* hold fixed ambiguities */ - if (stat == SOLQ_FIX && test_hold_amb(rtk)) - { - matcpy(rtk->x, xp, rtk->nx, 1); - matcpy(rtk->P, Pp, rtk->nx, rtk->nx); - trace(2, "%s hold ambiguity\n", str); - rtk->nfix = 0; - } + 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]); } + 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 d63de4f13..83866c97e 100644 --- a/src/algorithms/libs/rtklib/rtklib_ppp.h +++ b/src/algorithms/libs/rtklib/rtklib_ppp.h @@ -56,130 +56,159 @@ #include "rtklib.h" -#define MAX_PPP(x,y) ((x)>(y)?(x):(y)) -#define ROUND_PPP(x) (int)floor((x)+0.5) -const int MAX_ITER = 8; /* max number of iterations */ -const double MAX_STD_FIX = 0.15; /* max std-dev (3d) to fix solution */ -const int MIN_NSAT_SOL = 4; /* min satellite number for solution */ -const double THRES_REJECT = 4.0; /* reject threshold of posfit-res (sigma) */ +#define SQR(x) ((x)*(x)) +#define MIN(x,y) ((x)<=(y)?(x):(y)) +#define ROUND(x) (int)floor((x)+0.5) -const double THRES_MW_JUMP = 10.0; +#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) -const double VAR_POS_PPP = std::pow(60.0, 2.0); /* init variance receiver position (m^2) */ -const double VAR_CLK = std::pow(60.0, 2.0); /* init variance receiver clock (m^2) */ -const double VAR_ZTD = std::pow( 0.6, 2.0); /* init variance ztd (m^2) */ -const double VAR_GRA_PPP = std::pow(0.01, 2.0); /* init variance gradient (m^2) */ -const double VAR_DCB = std::pow(30.0, 2.0); /* init variance dcb (m^2) */ -const double VAR_BIAS = std::pow(60.0, 2.0); /* init variance phase-bias (m^2) */ -const double VAR_IONO = std::pow(60.0, 2.0); /* init variance iono-delay */ -const double VAR_GLO_IFB = std::pow( 0.6, 2.0); /* variance of glonass ifb */ +#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 EFACT_GPS_L5 = 10.0; /* error factor of GPS/QZS L5 */ +#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 */ -const double MUDOT_GPS = (0.00836*D2R); /* average angular velocity GPS (rad/s) */ -const double MUDOT_GLO = (0.00888*D2R); /* average angular velocity GLO (rad/s) */ -const double EPS0_GPS = (13.5*D2R); /* max shadow crossing angle GPS (rad) */ -const double EPS0_GLO = (14.2*D2R); /* max shadow crossing angle GLO (rad) */ -const double T_POSTSHADOW = 1800.0; /* post-shadow recovery time (s) */ -const double QZS_EC_BETA = 20.0; /* max beta angle for qzss Ec (deg) */ + /* 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 */ + + + +double lam_LC(int i, int j, int k); + +double L_LC(int i, int j, int k, const double *L); + +double P_LC(int i, int j, int k, const double *P); + +double var_LC(int i, int j, int k, double sig); + +double q_gamma(double a, double x, double log_gamma_a); + +double p_gamma(double a, double x, double log_gamma_a); + +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); + +int fix_amb_WL(rtk_t *rtk, const nav_t *nav, int sat1, int sat2, int *NW); + +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_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 pppcorr_read(pppcorr_t *corr, const char *file); -void pppcorr_free(pppcorr_t *corr); -int pppcorr_trop(const pppcorr_t *corr, gtime_t time, const double *pos, - double *trp, double *std); -int pppcorr_stec(const pppcorr_t *corr, gtime_t time, const double *pos, - double *ion, double *std); -int ppp_ar(rtk_t *rtk, const obsd_t *obs, int n, int *exc, - const nav_t *nav, const double *azel, double *x, double *P); -double STD(rtk_t *rtk, int i); -int pppoutstat(rtk_t *rtk, char *buff); +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_solid(const double *rsun, const double *rmoon, + 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 tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp, + const double *odisp, double *dr); + void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs); -double yaw_nominal(double beta, double mu); -double yaw_angle(int sat, const char *type, int opt, double beta, double mu, - double *yaw); -int sat_yaw(gtime_t time, int sat, const char *type, int opt, - const double *rs, double *exs, double *eys); +double varerr(int sat, int sys, double el, int type, const prcopt_t *opt); -int model_phw(gtime_t time, int sat, const char *type, int opt, - const double *rs, const double *rr, double *phw); - -double varerr(int sat, int sys, double el, int freq, 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); + +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); + +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); + double gfmeas(const obsd_t *obs, const nav_t *nav); -double mwmeas(const obsd_t *obs, const nav_t *nav); - -void corr_meas(const obsd_t *obs, const nav_t *nav, const double *azel, - const prcopt_t *opt, const double *dantr, - const double *dants, double phw, double *L, double *P, - double *Lc, double *Pc); - - -void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n); - -void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); - -void detslp_mw(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); - void udpos_ppp(rtk_t *rtk); void udclk_ppp(rtk_t *rtk); void udtrop_ppp(rtk_t *rtk); -void udiono_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); +void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n); -void uddcb_ppp(rtk_t *rtk); +void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); 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); + double *dant); -double trop_model_prec(gtime_t time, const double *pos, - const double *azel, 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); -int model_trop(gtime_t time, const double *pos, const double *azel, - const prcopt_t *opt, const double *x, double *dtdx, - const nav_t *nav, double *dtrp, double *var); - -int model_iono(gtime_t time, const double *pos, const double *azel, - const prcopt_t *opt, int sat, const double *x, - const nav_t *nav, double *dion, double *var); - - -int const_corr(const obsd_t *obs, int n, const int *exc, - const nav_t *nav, const double *x, const double *pos, - const double *azel, rtk_t *rtk, double *v, double *H, - double *var); - -int ppp_res(int post, const obsd_t *obs, int n, const double *rs, - const double *dts, const double *var_rs, const int *svh, - const double *dr, int *exc, 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); int pppnx(const prcopt_t *opt); -void update_stat(rtk_t *rtk, const obsd_t *obs, int n, int stat); - -int test_hold_amb(rtk_t *rtk); - - void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); - - #endif diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index caeef8dec..0d6864595 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -454,25 +454,13 @@ int satexclude(int sat, int svh, const prcopt_t *opt) { int sys = satsys(sat, NULL); - if (svh<0) - { - trace(3, "ephemeris unavailable: sat=%3d svh=%02X\n", sat, svh); - return 1; /* ephemeris unavailable */ - } + if (svh<0) return 1; /* ephemeris unavailable */ if (opt) { - if (opt->exsats[sat-1] == 1) - { - trace(3, "excluded satellite: sat=%3d svh=%02X\n", sat, svh); - return 1; /* excluded satellite */ - } + if (opt->exsats[sat-1] == 1) return 1; /* excluded satellite */ if (opt->exsats[sat-1] == 2) return 0; /* included satellite */ - if (!(sys&opt->navsys)) - { - trace(3, "unselected sat sys: sat=%3d svh=%02X\n", sat, svh); - return 1; /* unselected sat sys */ - } + if (!(sys&opt->navsys)) return 1; /* unselected sat sys */ } if (sys == SYS_QZS) svh&=0xFE; /* mask QZSS LEX health */ if (svh) @@ -3144,12 +3132,12 @@ void traceobs(int level __attribute__((unused)), const obsd_t *obs __attribute__ //void tracelevel(int level) {} void trace (int level __attribute__((unused)), const char *format __attribute__((unused)), ...) { - /*va_list ap;*/ - /* print error message to stderr */ - /*printf("RTKLIB TRACE[%i]:",level); - va_start(ap,format); - vfprintf(stdout,format,ap); - va_end(ap);*/ +// va_list ap; + /* print error message to stderr */ +// printf("RTKLIB TRACE[%i]:",level); +// va_start(ap,format); +// vfprintf(stdout,format,ap); +// va_end(ap); } //void tracet (int level, const char *format, ...) {} //void tracemat(int level, const double *A, int n, int m, int p, int q) {} diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h index 08c4d41dc..a7b4c81d7 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h @@ -266,5 +266,7 @@ void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun, void csmooth(obs_t *obs, int ns); int rtk_uncompress(const char *file, char *uncfile); int expath(const char *path, char *paths[], int nmax); +void windupcorr(gtime_t time, const double *rs, const double *rr, + double *phw); #endif /* GNSS_SDR_RTKLIB_RTKCMN_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index 47944dc86..27f57ed4a 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -51,6 +51,7 @@ * *----------------------------------------------------------------------------*/ +#include #include "rtklib_rtkpos.h" #include "rtklib_pntpos.h" #include "rtklib_ephemeris.h" @@ -180,102 +181,103 @@ void rtkclosestat(void) /* write solution status to buffer -------------------------------------------*/ -int rtkoutstat(rtk_t *rtk, char *buff) +void 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; + 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]; - if (rtk->sol.stat <= SOLQ_NONE) - { - return 0; - } - /* write ppp solution status to buffer */ - if (rtk->opt.mode >= PMODE_PPP_KINEMA) - { - return pppoutstat(rtk, buff); - } - est = rtk->opt.mode >= PMODE_DGPS; - nfreq = est ? nf : 1; - tow = time2gpst(rtk->sol.time, &week); + if (statlevel<=0||!fp_stat) return; - /* 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], - xa[2]); - } - else - { - p += sprintf(p, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow, - rtk->sol.stat, rtk->sol.rr[0], rtk->sol.rr[1], rtk->sol.rr[2], - 0.0, 0.0, 0.0); - } - /* receiver velocity and acceleration */ - if (est && rtk->opt.dynamics) - { - ecef2pos(rtk->sol.rr, pos); - ecef2enu(pos, rtk->x+3, vel); - ecef2enu(pos, rtk->x+6, acc); - if (rtk->na >= 6) ecef2enu(pos, rtk->xa+3, vela); - if (rtk->na >= 9) ecef2enu(pos, rtk->xa+6, acca); - p += sprintf(p, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", - week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], acc[0], acc[1], - acc[2], vela[0], vela[1], vela[2], acca[0], acca[1], acca[2]); - } - else - { - ecef2pos(rtk->sol.rr, pos); - ecef2enu(pos, rtk->sol.rr+3, vel); - p += sprintf(p, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", - week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - } - /* receiver clocks */ - p += sprintf(p, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\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); + trace(3,"outsolstat:\n"); - /* ionospheric parameters */ - if (est && rtk->opt.ionoopt == IONOOPT_EST) - { - for (i = 0;issat+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]); - } - } - /* tropospheric parameters */ - if (est && (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG)) - { - for (i = 0;i<2;i++) { - j = IT_RTK(i, &rtk->opt); - xa[0] = 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) - { - 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]); - } - } - return (int)(p-buff); + /* swap solution status file */ + swapsolstat(); + + est=rtk->opt.mode>=PMODE_DGPS; + nfreq=est?nf:1; + tow=time2gpst(rtk->sol.time,&week); + + /* receiver position */ + if (est) { + for (i=0;i<3;i++) xa[i]=ina?rtk->xa[i]:0.0; + fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow, + rtk->sol.stat,rtk->x[0],rtk->x[1],rtk->x[2],xa[0],xa[1],xa[2]); + } + else { + fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow, + rtk->sol.stat,rtk->sol.rr[0],rtk->sol.rr[1],rtk->sol.rr[2], + 0.0,0.0,0.0); + } + /* receiver velocity and acceleration */ + if (est&&rtk->opt.dynamics) { + ecef2pos(rtk->sol.rr,pos); + ecef2enu(pos,rtk->x+3,vel); + ecef2enu(pos,rtk->x+6,acc); + if (rtk->na>=6) ecef2enu(pos,rtk->xa+3,vela); + if (rtk->na>=9) ecef2enu(pos,rtk->xa+6,acca); + fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", + week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],acc[0],acc[1],acc[2], + vela[0],vela[1],vela[2],acca[0],acca[1],acca[2]); + } + else { + ecef2pos(rtk->sol.rr,pos); + ecef2enu(pos,rtk->sol.rr+3,vel); + fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", + week,tow,rtk->sol.stat,vel[0],vel[1],vel[2], + 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0); + } + /* receiver clocks */ + fprintf(fp_stat,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", + week,tow,rtk->sol.stat,1,rtk->sol.dtr[0]*1E9,rtk->sol.dtr[1]*1E9, + rtk->sol.dtr[2]*1E9,rtk->sol.dtr[3]*1E9); + + /* ionospheric parameters */ + if (est&&rtk->opt.ionoopt==IONOOPT_EST) { + for (i=0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1,id); + j=II_RTK(i+1,&rtk->opt); + xa[0]=jna?rtk->xa[j]:0.0; + fprintf(fp_stat,"$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n",week,tow,rtk->sol.stat, + id,ssat->azel[0]*R2D,ssat->azel[1]*R2D,rtk->x[j],xa[0]); + } + } + /* tropospheric parameters */ + if (est&&(rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG)) { + for (i=0;i<2;i++) { + j=IT_RTK(i,&rtk->opt); + xa[0]=jna?rtk->xa[j]:0.0; + fprintf(fp_stat,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat, + i+1,rtk->x[j],xa[0]); + } + } + /* receiver h/w bias */ + if (est&&rtk->opt.glomodear==2) { + for (i=0;iopt); + xa[0]=jna?rtk->xa[j]:0.0; + fprintf(fp_stat,"$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat, + i+1,rtk->x[j],xa[0]); + } + } + if (rtk->sol.stat==SOLQ_NONE||statlevel<=1) return; + + /* residuals and status */ + for (i=0;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]); + } + } } @@ -310,43 +312,101 @@ void swapsolstat(void) /* output solution status ----------------------------------------------------*/ 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); + 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]; - if (statlevel <= 0 || !fp_stat || !rtk->sol.stat) return; +if (statlevel<=0||!fp_stat) return; - trace(3, "outsolstat:\n"); +trace(3,"outsolstat:\n"); - /* swap solution status file */ - swapsolstat(); +/* swap solution status file */ +swapsolstat(); - /* write solution status */ - n = rtkoutstat(rtk, buff); buff[n] = '\0'; +est=rtk->opt.mode >= PMODE_DGPS; +nfreq=est?nf:1; +tow=time2gpst(rtk->sol.time,&week); - fputs(buff, fp_stat); +/* receiver position */ +if (est) { + for (i=0;i<3;i++) xa[i]=ina?rtk->xa[i]:0.0; + fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow, + rtk->sol.stat,rtk->x[0],rtk->x[1],rtk->x[2],xa[0],xa[1],xa[2]); +} +else { + fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow, + rtk->sol.stat,rtk->sol.rr[0],rtk->sol.rr[1],rtk->sol.rr[2], + 0.0,0.0,0.0); +} +/* receiver velocity and acceleration */ +if (est&&rtk->opt.dynamics) { + ecef2pos(rtk->sol.rr,pos); + ecef2enu(pos,rtk->x+3,vel); + ecef2enu(pos,rtk->x+6,acc); + if (rtk->na>=6) ecef2enu(pos,rtk->xa+3,vela); + if (rtk->na>=9) ecef2enu(pos,rtk->xa+6,acca); + fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", + week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],acc[0],acc[1],acc[2], + vela[0],vela[1],vela[2],acca[0],acca[1],acca[2]); +} +else { + ecef2pos(rtk->sol.rr,pos); + ecef2enu(pos,rtk->sol.rr+3,vel); + fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", + week,tow,rtk->sol.stat,vel[0],vel[1],vel[2], + 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0); +} +/* receiver clocks */ +fprintf(fp_stat,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", + week,tow,rtk->sol.stat,1,rtk->sol.dtr[0]*1E9,rtk->sol.dtr[1]*1E9, + rtk->sol.dtr[2]*1E9,rtk->sol.dtr[3]*1E9); - if (rtk->sol.stat == SOLQ_NONE || statlevel <= 1) return; +/* ionospheric parameters */ +if (est&&rtk->opt.ionoopt==IONOOPT_EST) { + for (i=0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1,id); + j=II_RTK(i+1,&rtk->opt); + xa[0]=jna?rtk->xa[j]:0.0; + fprintf(fp_stat,"$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n",week,tow,rtk->sol.stat, + id,ssat->azel[0]*R2D,ssat->azel[1]*R2D,rtk->x[j],xa[0]); + } +} +/* tropospheric parameters */ +if (est&&(rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG)) { + for (i=0;i<2;i++) { + j=IT_RTK(i,&rtk->opt); + xa[0]=jna?rtk->xa[j]:0.0; + fprintf(fp_stat,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat, + i+1,rtk->x[j],xa[0]); + } +} +/* receiver h/w bias */ +if (est&&rtk->opt.glomodear==2) { + for (i=0;iopt); + xa[0]=jna?rtk->xa[j]:0.0; + fprintf(fp_stat,"$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat, + i+1,rtk->x[j],xa[0]); + } +} +if (rtk->sol.stat==SOLQ_NONE||statlevel<=1) return; - tow = time2gpst(rtk->sol.time, &week); - nfreq = rtk->opt.mode >= PMODE_DGPS ? nf : 1; - - /* write 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]); - } - } +/* 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]); + } +} } @@ -2064,7 +2124,6 @@ int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, NULL, rtk->ssat, msg)) { errmsg(rtk, "point pos error (%s)\n", msg); - if (!rtk->opt.dynamics) { outsolstat(rtk); diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.h b/src/algorithms/libs/rtklib/rtklib_rtkpos.h index 19611d14a..82640eca9 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.h +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.h @@ -93,7 +93,7 @@ int rtkopenstat(const char *file, int level); void rtkclosestat(void); -int rtkoutstat(rtk_t *rtk, char *buff); +void rtkoutstat(rtk_t *rtk); void swapsolstat(void); diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 65682f4d2..a0a1ee974 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -382,7 +382,10 @@ int Position_Gps_L1_System_Test::configure_receiver() config->set_property("PVT.rtcm_dump_devname", "/dev/pts/1"); config->set_property("PVT.dump", "false"); config->set_property("PVT.rinex_version", std::to_string(2)); - + config->set_property("PVT.positioning_mode", std::to_string(7)); + config->set_property("PVT.iono_model", std::to_string(0)); + config->set_property("PVT.trop_model", std::to_string(0)); + config->set_property("PVT.AR_GPS", std::to_string(4)); return 0; }