/* ionosphere residuals ------------------------------------------------------*/ static int res_iono(const obsd_t *obs, int n, const nav_t *nav, const double *rs, const double *rr, const double *pos, const double *azel, const pcv_t *pcv, const ekf_t *ekf, double *phw, double *v, double *H, double *R) { double *sig,P1,P2,L1,L2,c_iono=1.0-SQR(lam[1]/lam[0]); double LG,PG,antdel[3]={0},dant[NFREQ]={0}; int i,j,nv=0,sat; sig=mat(1,2*n); for (i=0;i<n;i++) { if (!raw_obs(obs+i,nav,&P1,&P2,&L1,&L2)||azel[i*2+1]<MIN_EL) continue; sat=obs[i].sat; /* ionosphere-LC model */ LG=-c_iono*ekf->x[II(sat)]+ekf->x[IB(sat)]; PG= c_iono*ekf->x[II(sat)]+nav->cbias[sat-1][0]; /* receiver antenna phase center offset and variation */ if (pcv) { antmodel(pcv,antdel,azel+i*2,dant); LG+=dant[0]-dant[1]; PG+=dant[0]-dant[1]; } /* phase windup correction */ windupcorr(obs[i].time,rs+i*6,rr,phw+obs[i].sat-1); LG+=(lam[0]-lam[1])*phw[obs[i].sat-1]; /* residuals of ionosphere (geometriy-free) LC */ v[nv ]=(L1-L2)-LG; #if 0 v[nv+1]=(P1-P2)-PG; #else v[nv+1]=0.0; #endif for (j=0;j<ekf->nx*2;j++) H[ekf->nx*nv+j]=0.0; H[ekf->nx*nv +II(sat)]=-c_iono; H[ekf->nx*nv +IB(sat)]=1.0; H[ekf->nx*(nv+1)+II(sat)]=c_iono; sig[nv ]=sig_err(azel+i*2); sig[nv+1]=RATIO_ERR*sig[nv]; nv+=2; } for (i=0;i<nv;i++) for (j=0;j<nv;j++) { R[i+j*nv]=i==j?SQR(sig[i]):0.0; } free(sig); return nv; }
/* 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); }