/* temporal update of tropospheric parameters --------------------------------*/ static void udtrop_ppp(rtk_t *rtk) { double pos[3],azel[]={0.0,PI/2.0},ztd,var; int i=IT(&rtk->opt),j; 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->opt.tropopt>=TROPOPT_ESTG) { for (j=0;j<2;j++) initx(rtk,1E-6,VAR_GRA,++i); } } else { rtk->P[i*(1+rtk->nx)]+=SQR(rtk->opt.prn[2])*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); } } } }
/* tropospheric correction ----------------------------------------------------- * compute tropospheric correction * args : gtime_t time I time * nav_t *nav I navigation data * double *pos I receiver position {lat,lon,h} (rad|m) * double *azel I azimuth/elevation angle {az,el} (rad) * int tropopt I tropospheric correction option (TROPOPT_???) * double *trp O tropospheric delay (m) * double *var O tropospheric delay variance (m^2) * return : status(1:ok,0:error) *-----------------------------------------------------------------------------*/ extern int tropcorr(gtime_t time, const nav_t *nav, const double *pos, const double *azel, int tropopt, double *trp, double *var) { /* saastamoinen model */ if (tropopt == TROPOPT_SAAS || tropopt == TROPOPT_EST || tropopt == TROPOPT_ESTG) { *trp = tropmodel(time, pos, azel, REL_HUMI); *var = SQR(ERR_SAAS / (sin(azel[1]) + 0.1)); return 1; } /* sbas troposphere model */ if (tropopt == TROPOPT_SBAS) { *trp = sbstropcorr(time, pos, azel, var); return 1; } /* no correction */ *trp = 0.0; *var = tropopt == TROPOPT_OFF ? SQR(ERR_TROP) : 0.0; return 1; }
/* tropospheric correction ----------------------------------------------------- * compute tropospheric correction * args : gtime_t time I time * nav_t *nav I navigation data * double *pos I receiver position {lat,lon,h} (rad|m) * double *azel I azimuth/elevation angle {az,el} (rad) * int tropopt I tropospheric correction option (TROPOPT_???) * double *trp O tropospheric delay (m) * double *var O tropospheric delay variance (m^2) * return : status(1:ok,0:error) *-----------------------------------------------------------------------------*/ extern int tropcorr(gtime_t time, const nav_t *nav, const double *pos, const double *azel, int tropopt, double *trp, double *var) { trace(4,"tropcorr: time=%s opt=%d pos=%.3f %.3f azel=%.3f %.3f\n", time_str(time,3),tropopt,pos[0]*R2D,pos[1]*R2D,azel[0]*R2D, azel[1]*R2D); /* saastamoinen model */ if (tropopt==TROPOPT_SAAS||tropopt==TROPOPT_EST||tropopt==TROPOPT_ESTG) { *trp=tropmodel(time,pos,azel,REL_HUMI); *var=SQR(ERR_SAAS/(sin(azel[1])+0.1)); return 1; } /* sbas troposphere model */ if (tropopt==TROPOPT_SBAS) { *trp=sbstropcorr(time,pos,azel,var); return 1; } /* no correction */ *trp=0.0; *var=tropopt==TROPOPT_OFF?SQR(ERR_TROP):0.0; return 1; }
/* phase and code residuals --------------------------------------------------*/ static int res_ppp(int iter, const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const double *x, rtk_t *rtk, double *v, double *H, double *R, double *azel) { prcopt_t *opt=&rtk->opt; double r,rr[3],disp[3],pos[3],e[3],meas[2],dtdx[3],dantr[NFREQ]={0}; double dants[NFREQ]={0},var[MAXOBS*2],dtrp=0.0,vart=0.0,varm[2]={0}; int i,j,k,sat,sys,nv=0,nx=rtk->nx,brk,tideopt; trace(3,"res_ppp : n=%d nx=%d\n",n,nx); for (i=0;i<MAXSAT;i++) rtk->ssat[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;i<n&&i<MAXOBS;i++) { sat=obs[i].sat; if (!(sys=satsys(sat,NULL))||!rtk->ssat[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)<opt->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); } else if (opt->tropopt==TROPOPT_SBAS) { dtrp=sbstropcorr(obs[i].time,pos,azel+i*2,&vart); } else if (opt->tropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) { dtrp=prectrop(obs[i].time,pos,azel+i*2,opt,x+IT(opt),dtdx,&vart); } else if (opt->tropopt==TROPOPT_COR||opt->tropopt==TROPOPT_CORG) { dtrp=prectrop(obs[i].time,pos,azel+i*2,opt,x,dtdx,&vart); } /* satellite antenna model */ if (opt->posopt[0]) { satantpcv(rs+i*6,rr,nav->pcvs+sat-1,dants); } /* receiver antenna model */ antmodel(opt->pcvr,opt->antdel[0],azel+i*2,opt->posopt[1],dantr); /* 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+=-CLIGHT*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;k<nx;k++) H[k+nx*nv]=0.0; v[nv]=meas[j]-r; for (k=0;k<3;k++) H[k+nx*nv]=-e[k]; if (sys!=SYS_GLO) { v[nv]-=x[IC(0,opt)]; H[IC(0,opt)+nx*nv]=1.0; } else { v[nv]-=x[IC(1,opt)]; H[IC(1,opt)+nx*nv]=1.0; } if (opt->tropopt>=TROPOPT_EST) { for (k=0;k<(opt->tropopt>=TROPOPT_ESTG?3:1);k++) { H[IT(opt)+k+nx*nv]=dtdx[k]; } } 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]; /* 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;i<nv;i++) for (j=0;j<nv;j++) { R[i+j*nv]=i==j?var[i]:0.0; } trace(5,"x=\n"); tracemat(5,x, 1,nx,8,3); trace(5,"v=\n"); tracemat(5,v, 1,nv,8,3); trace(5,"H=\n"); tracemat(5,H,nx,nv,8,3); trace(5,"R=\n"); tracemat(5,R,nv,nv,8,5); return nv; } /* number of estimated states ------------------------------------------------*/ extern int pppnx(const prcopt_t *opt) { return NX(opt); } /* precise point positioning -------------------------------------------------*/ extern 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; int i,nv,info,svh[MAXOBS],stat=SOLQ_SINGLE; 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); for (i=0;i<MAXSAT;i++) rtk->ssat[i].fix[0]=0; /* 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); /* 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;i<rtk->opt.niter;i++) { /* phase and code residuals */ if ((nv=res_ppp(i,obs,n,rs,dts,var,svh,nav,xp,rtk,v,H,R,azel))<=0) break; /* 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; } 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; } /* update solution status */ rtk->sol.ns=0; for (i=0;i<n&&i<MAXOBS;i++) { if (!rtk->ssat[obs[i].sat-1].vsat[0]) continue; rtk->ssat[obs[i].sat-1].lock[0]++; rtk->ssat[obs[i].sat-1].outc[0]=0; rtk->ssat[obs[i].sat-1].fix [0]=4; rtk->sol.ns++; } rtk->sol.stat=stat; for (i=0;i<3;i++) { rtk->sol.rr[i]=rtk->x[i]; rtk->sol.qr[i]=(float)rtk->P[i+i*rtk->nx]; } rtk->sol.qr[3]=(float)rtk->P[1]; rtk->sol.qr[4]=(float)rtk->P[2+rtk->nx]; rtk->sol.qr[5]=(float)rtk->P[2]; rtk->sol.dtr[0]=rtk->x[IC(0,opt)]; rtk->sol.dtr[1]=rtk->x[IC(1,opt)]-rtk->x[IC(0,opt)]; for (i=0;i<n&&i<MAXOBS;i++) { rtk->ssat[obs[i].sat-1].snr[0]=MIN(obs[i].SNR[0],obs[i].SNR[1]); } for (i=0;i<MAXSAT;i++) { if (rtk->ssat[i].slip[0]&3) rtk->ssat[i].slipc[0]++; } } free(rs); free(dts); free(var); free(azel); free(xp); free(Pp); free(v); free(H); free(R); }