Exemplo n.º 1
0
/* precise tropospheric model ------------------------------------------------*/
static 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;
    
    /* zenith hydrostatic delay */
    zhd=tropmodel(time,pos,zazel,0.0);
    
    /* mapping function */
    m_h=tropmapf(time,pos,azel,&m_w);
    
    if ((opt->tropopt==TROPOPT_ESTG||opt->tropopt==TROPOPT_CORG)&&azel[1]>0.0) {
        
        /* m_w=m_0+m_0*cot(el)*(Gn*cos(az)+Ge*sin(az)): ref [6] */
        cotz=1.0/tan(azel[1]);
        grad_n=m_w*cotz*cos(azel[0]);
        grad_e=m_w*cotz*sin(azel[0]);
        m_w+=grad_n*x[1]+grad_e*x[2];
        dtdx[1]=grad_n*(x[0]-zhd);
        dtdx[2]=grad_e*(x[0]-zhd);
    }
    dtdx[0]=m_w;
    *var=SQR(0.01);
    return m_h*zhd+m_w*(x[0]-zhd);
}
Exemplo n.º 2
0
/*
static int zdres(int base, const obsd_t *obs, int n, const double *rs,
                 const double *dts, const int *svh, const nav_t *nav,
                 const double *rr, const prcopt_t *opt, double *y,
                 double *e, double *azel)
Description: zero differenced residual
Params: 	base		I			1:base, 0: rover
					obs			I
					n				I			number of obs data
					rs			I			sat positions/velocities
					dts			I			sat clock bias
					svh			I
					nav			I
					rr			I			receiver position
					y				O			zero diff residuals
					e				O
					azel		O			{az/el}
Return:	0 ok, !0 error
*/
static int zdres(int base, const obsd_t *obs, int n, const double *rs,
                 const double *dts, const int *svh, const nav_t *nav,
                 const double *rr, const prcopt_t *opt, int index, double *y,
                 double *e, double *azel)
{
    double r,rr_[3],pos[3];
    double zhd, zazel[]= {0.0,90.0*D2R}; //tai sao init zazel = {0,90}
    int i;

    for (i=0; i<2*n; i++) y[i]=0.0;
    if (sos3(rr)<=0.0) return -1;//no receiver pos
    for (i=0; i<3; i++) rr_[i]=rr[i];
    ecef2pos(rr_,pos);


    for (i=0; i<n; i++)
    {
        if ((r=geodist(rs+i*6,rr_,e+i*3))<=0.0) continue;
        if (satazel(pos,e+i*3,azel+i*2)<opt->elmin) continue;
        //satellite clock-bias
        r -= CLIGHT*dts[i*2];
        /* troposphere delay model (hydrostatic) */
        zhd=tropmodel(obs[0].time,pos,zazel,0.0);//tai sao chon rel_humi = 0???????????
        r+=tropmapf(obs[i].time,pos,azel+i*2,NULL)*zhd;
        /* undifferenced phase/code residual for satellite */
        zdres_sat(base,r,obs+i,nav,azel+i*2,opt,y+i*2);
    }
    return 0;
}
Exemplo n.º 3
0
/* tropmodel */
void utest3(void)
{
    gtime_t time={0};
    double pos1 []={ 35*D2R, 140*D2R, 100.0};
    double pos2 []={-80*D2R,-170*D2R,1000.0};
    double pos3 []={-80*D2R,-170*D2R,100000.0};
    double pos4 []={-80*D2R,-170*D2R,-200.0};
    double azel1[]={ 60*D2R, 75*D2R};
    double azel2[]={190*D2R,  3*D2R};
    double azel3[]={190*D2R,-10*D2R};
    double dtrp;
    
    dtrp=tropmodel(time,pos1,azel1,0.5);
        assert(fabs(dtrp-2.44799870144088)<1e-8);
    
    dtrp=tropmodel(time,pos1,azel2,0.5);
        assert(fabs(dtrp-45.1808916506163)<1e-8);
    
    dtrp=tropmodel(time,pos2,azel1,0.5);
        assert(fabs(dtrp-2.17295817298152)<1e-8);
    
    dtrp=tropmodel(time,pos1,azel3,0.0);
        assert(fabs(dtrp-0.00000000000000)<1e-8);
    
    dtrp=tropmodel(time,pos3,azel1,0.0);
        assert(fabs(dtrp-0.00000000000000)<1e-8);
    
    dtrp=tropmodel(time,pos4,azel1,0.0);
        assert(fabs(dtrp-0.00000000000000)<1e-8);
    
    printf("%s utest3 : OK\n",__FILE__);
}
Exemplo n.º 4
0
/* 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;
}
Exemplo n.º 5
0
/* 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;
}
Exemplo n.º 6
0
/* 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);
}
Exemplo n.º 7
0
/* generate simulated observation data ---------------------------------------*/
static int simobs(gtime_t ts, gtime_t te, double tint, const double *rr,
                  nav_t *nav, obs_t *obs, int opt)
{
    gtime_t time;
    obsd_t data[MAXSAT]={{{0}}};
    double pos[3],rs[3*MAXSAT],dts[MAXSAT],r,e[3],azel[2];
    double ecp[MAXSAT][NFREQ]={{0}},epr[MAXSAT][NFREQ]={{0}};
    double snr[MAXSAT][NFREQ]={{0}},ers[MAXSAT][3]={{0}};
    double iono,trop,fact,cp,pr,dtr=0.0,rref[3],bl;
    int i,j,k,n,ns,amb[MAXSAT][NFREQ]={{0}},sys,prn;
    char s[64];
    
    double pref[]={36.106114294,140.087190410,70.3010}; /* ref station */
    
    trace(3,"simobs:nnav=%d ngnav=%d\n",nav->n,nav->ng);
    
    for (i=0;i<2;i++) pref[i]*=D2R;
    pos2ecef(pref,rref);
    for (i=0;i<3;i++) rref[i]-=rr[i];
    bl=norm(rref,3)/1E4; /* baseline (10km) */
    srand(0);
    
    /* ephemeris error */
    for (i=0;i<MAXSAT;i++) {
        data[i].sat=i+1;
        data[i].P[0]=2E7;
        for (j=0;j<3;j++) ers[i][j]=randn(0.0,erreph);
    }
    srand(tickget());
    
    ecef2pos(rr,pos);
    n=(int)(timediff(te,ts)/tint+1.0);
    
    for (i=0;i<n;i++) {
        time=timeadd(ts,tint*i);
        time2str(time,s,0);
        
        for (j=0;j<MAXSAT;j++) data[j].time=time;
        
        for (j=0;j<3;j++) { /* iteration for pseudorange */
            satpos(time,data,MAXSAT,nav,rs,dts);
            for (k=0;k<MAXSAT;k++) {
                if ((r=geodist(rs+k*3,rr,e))<=0.0) continue;
                data[k].P[0]=r+CLIGHT*(dtr-dts[k]);
            }
        }
        satpos(time,data,MAXSAT,nav,rs,dts);
        for (j=ns=0;j<MAXSAT;j++) {
            
            /* add ephemeris error */
            for (k=0;k<3;k++) rs[k+j*3]+=ers[j][k];
            
            if ((r=geodist(rs+j*3,rr,e))<=0.0) continue;
            satazel(pos,e,azel);
            if (azel[1]<minel*D2R) continue;
            
            iono=ionmodel(time,nav->ion,pos,azel);
            trop=tropmodel(pos,azel,0.3);
            
            /* add ionospheric error */
            iono+=errion*bl*ionmapf(pos,azel);
            
            snrmodel(azel,snr[j]);
            errmodel(azel,snr[j],ecp[j],epr[j]);
            sys=satsys(data[j].sat,&prn);
            
            for (k=0;k<NFREQ;k++) {
                data[j].L[k]=data[j].P[k]=0.0;
                data[j].SNR[k]=0;
                data[j].LLI[k]=0;
                
                if (sys==SYS_GPS) {
                    if (k>=3) continue; /* no L5a/L5b in gps */
                    if (k>=2&&!gpsblock[prn-1]) continue; /* no L5 in block II */
                }
                else if (sys==SYS_GLO) {
                    if (k>=3) continue;
                }
                else if (sys==SYS_GAL) {
                    if (k==1) continue; /* no L2 in galileo */
                }
                else continue;
                
                /* generate observation data */
                fact=lam[k]*lam[k]/lam[0]/lam[0];
                cp=r+CLIGHT*(dtr-dts[j])-fact*iono+trop+ecp[j][k];
                pr=r+CLIGHT*(dtr-dts[j])+fact*iono+trop+epr[j][k];
                
                if (amb[j][k]==0) amb[j][k]=(int)(-cp/lam[k]);
                data[j].L[k]=cp/lam[k]+amb[j][k];
                data[j].P[k]=pr;
                data[j].SNR[k]=(unsigned char)snr[j][k];
                data[j].LLI[k]=data[j].SNR[k]<slipthres?1:0;
            }
            if (obs->nmax<=obs->n) {
                if (obs->nmax==0) obs->nmax=65532; else obs->nmax+=65532;
                if (!(obs->data=(obsd_t *)realloc(obs->data,sizeof(obsd_t)*obs->nmax))) {
                    fprintf(stderr,"malloc error\n");
                    return 0;
                }
            }
            obs->data[obs->n++]=data[j];
            ns++;
        }
        fprintf(stderr,"time=%s nsat=%2d\r",s,ns);
    }
    fprintf(stderr,"\n");
    return 1;
}