示例#1
0
/* tle_pos() accuracy --------------------------------------------------------*/
static void utest3(void)
{
    const char *file1="../data/tle/brdc3050.12*";
    const char *file2="../data/tle/TLE_GNSS_20121101.txt";
    const char *file3="../data/tle/igs17127.erp";
    const double ep[6]={2012,10,31,0,0,0};
    nav_t nav={0};
    erp_t erp={0};
    tle_t tle={0};
    gtime_t time;
    char sat[32];
    double rs1[6],rs2[6],ds[6],dts[2],var;
    int i,j,k,stat,svh;
    
    readrnx(file1,0,"",NULL,&nav,NULL);
        assert(nav.n>0);
    
    stat=readerp(file3,&erp);
        assert(stat);
    
    stat=tle_read(file2,&tle);
        assert(stat);
    
    for (i=0;i<MAXSAT;i++) {
        satno2id(i+1,sat);
        
        fprintf(OUT,"SAT=%s\n",sat);
        
        for (j=0;j<96;j++) {
            time=timeadd(epoch2time(ep),900.0*j);
            
            if (!satpos(time,time,i+1,EPHOPT_BRDC,&nav,rs1,dts,&var,&svh)) continue;
            
            if (satsys(i+1,NULL)==SYS_QZS) svh&=0xFE;
            
            if (svh) continue;
            
            stat=tle_pos(time,sat,"","",&tle,&erp,rs2);
                assert(stat);
            
            for (k=0;k<3;k++) ds[k]=rs2[k]-rs1[k];
            
            fprintf(OUT,"%6.0f %11.3f %11.3f %11.3f %11.3f\n",900.0*j,
                    ds[0]/1e3,ds[1]/1e3,ds[2]/1e3,norm(ds,3)/1e3);
                
                assert(norm(ds,3)/1e3<300.0);
        }
        fprintf(OUT,"\n");
    }
    fprintf(OUT,"%s utest3 : OK\n",__FILE__);
}
示例#2
0
/* satellite positions and clocks ----------------------------------------------
* compute satellite positions, velocities and clocks
* args   : gtime_t teph     I   time to select ephemeris (gpst)
*          obsd_t *obs      I   observation data
*          int    n         I   number of observation data
*          nav_t  *nav      I   navigation data
*          int    ephopt    I   ephemeris option (EPHOPT_???)
*          double *rs       O   satellite positions and velocities (ecef)
*          double *dts      O   satellite clocks
*          double *var      O   sat position and clock error variances (m^2)
*          int    *svh      O   sat health flag (-1:correction not available)
* return : none
* notes  : rs [(0:2)+i*6]= obs[i] sat position {x,y,z} (m)
*          rs [(3:5)+i*6]= obs[i] sat velocity {vx,vy,vz} (m/s)
*          dts[(0:1)+i*2]= obs[i] sat clock {bias,drift} (s|s/s)
*          var[i]        = obs[i] sat position and clock error variance (m^2)
*          svh[i]        = obs[i] sat health flag
*          if no navigation data, set 0 to rs[], dts[], var[] and svh[]
*          satellite position and clock are values at signal transmission time
*          satellite position is referenced to antenna phase center
*          satellite clock does not include code bias correction (tgd or bgd)
*          any pseudorange and broadcast ephemeris are always needed to get
*          signal transmission time
*-----------------------------------------------------------------------------*/
extern void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
                    int ephopt, double *rs, double *dts, double *var, int *svh)
{
    gtime_t time[MAXOBS]={{0}};
    double dt,pr;
    int i,j;
    
    trace(3,"satposs : teph=%s n=%d ephopt=%d\n",time_str(teph,3),n,ephopt);
    
    for (i=0;i<n&&i<MAXOBS;i++) {
        for (j=0;j<6;j++) rs [j+i*6]=0.0;
        for (j=0;j<2;j++) dts[j+i*2]=0.0;
        var[i]=0.0; svh[i]=0;
        
        /* search any psuedorange */
        for (j=0,pr=0.0;j<NFREQ;j++) if ((pr=obs[i].P[j])!=0.0) break;
        
        if (j>=NFREQ) {
            trace(2,"no pseudorange %s sat=%2d\n",time_str(obs[i].time,3),obs[i].sat);
            continue;
        }
        /* transmission time by satellite clock */
        time[i]=timeadd(obs[i].time,-pr/CLIGHT);
        
        /* satellite clock bias by broadcast ephemeris */
        if (!ephclk(time[i],teph,obs[i].sat,nav,&dt)) {
            trace(2,"no broadcast clock %s sat=%2d\n",time_str(time[i],3),obs[i].sat);
            continue;
        }
        time[i]=timeadd(time[i],-dt);
        
        /* satellite position and clock at transmission time */
        if (!satpos(time[i],teph,obs[i].sat,ephopt,nav,rs+i*6,dts+i*2,var+i,
                    svh+i)) {
            trace(2,"no ephemeris %s sat=%2d\n",time_str(time[i],3),obs[i].sat);
            continue;
        }
        /* if no precise clock available, use broadcast clock instead */
        if (dts[i*2]==0.0) {
            if (!ephclk(time[i],teph,obs[i].sat,nav,dts+i*2)) continue;
            dts[1+i*2]=0.0;
            *var=SQR(STD_BRDCCLK);
        }
    }
    for (i=0;i<n&&i<MAXOBS;i++) {
        trace(4,"%s sat=%2d rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n",
              time_str(time[i],6),obs[i].sat,rs[i*6],rs[1+i*6],rs[2+i*6],
              dts[i*2]*1E9,var[i],svh[i]);
    }
}
示例#3
0
文件: ephemeris.c 项目: net21/GPSRTK
/* -- void satposs(gtime_t teph,const obsd_t *obs,int n,const nav_t *nav,
		double *rs, double *dts, double *var, int *svh) --------------------------------------
 * 
 * Description	: 
 * Parameters	: *rs		O	satellite positions and velocities (ecef), size n
 *							*dts	O	sat clocks
 *							*var	O	sat position and clock error variances (m^2)
 *							*svh	O sat health flag
 * Return		: 
 */
void satposs(gtime_t teph,const obsd_t *obs,int n,const nav_t *nav,
		double *rs, double *dts, double *var, int *svh,char **msg)
{
	gtime_t time[MAX_OBS]={{0}};
	double dt;
	int i,j;
	
	for (i=0;(i<n)&&(i<MAX_OBS);i++)
	{
		for (j=0;j<6;j++) rs[j+i*6]=0.0;
		for (j=0;j<2;j++) dts[j+i*2]=0.0;
		var[i]=0.0;svh[i]=0.0;
		if (obs[i].P==0.0)
		{			
			continue;
		}
		
		/* transmission time by satellite clock */
		time[i]=timeadd(obs[i].time,-obs[i].P/CLIGHT);
		/* satellite clock bias by broadcast ephemeris */
    if (!ephclk(time[i],teph,obs[i].sat,nav,&dt,msg)) {
      continue;
    }
		//*msg += sprintf(*msg,"clk");
		time[i]=timeadd(time[i],-dt);
		//start = TIM2->CNT;
		/* satellite position and clock at transmission time */
		if (!satpos(time[i],teph,obs[i].sat,nav,rs+i*6,dts+i*2,var+i,
                    svh+i,msg)) {

				continue;
    }
		//if (i==0)
		//		SendIntStr(TIM2->CNT-start);
		//*msg += sprintf(*msg,"satpos");								
		//if no precise clock available, use broadcast clock instead
		if (dts[i*2]==0.0)
		{
      if (!ephclk(time[i],teph,obs[i].sat,nav,dts+i*2,msg)) 
				continue;
      dts[1+i*2]=0.0;
			*var=SQR(STD_BRDCCLK);
		}
	}
}			
示例#4
0
// update observation data index, azimuth/elevation, satellite list ---------
void __fastcall TPlot::UpdateObs(int nobs)
{
    AnsiString s;
    prcopt_t opt=prcopt_default;
    gtime_t time;
    sol_t sol={0};
    double pos[3],rr[3],e[3],azel[MAXOBS*2]={0},rs[6],dts[2],var;
    int i,j,k,svh,per,per_=-1;
    char msg[128];
    
    trace(3,"UpdateObs\n");
    
    delete [] IndexObs; IndexObs=NULL;
    delete [] Az; Az=NULL;
    delete [] El; El=NULL;
    NObs=0;
    if (nobs<=0) return;
    
    IndexObs=new int[nobs];
    Az=new double[Obs.n];
    El=new double[Obs.n];
    
    opt.err[0]=900.0;
    
    ReadWaitStart();
    ShowLegend(NULL);
    
    for (i=0;i<Obs.n;i=j) {
        time=Obs.data[i].time;
        for (j=i;j<Obs.n;j++) {
            if (timediff(Obs.data[j].time,time)>TTOL) break;
        }
        IndexObs[NObs++]=i;
        
        if (Nav.n<=0&&Nav.ng<=0&&Nav.ns<=0) {
            for (k=0;k<j-i;k++) Az[i+k]=El[i+k]=0.0;
            continue;
        }
        if (RcvPos==0) {
            pntpos(Obs.data+i,j-i,&Nav,&opt,&sol,azel,NULL,msg);
        }
        else {
            if (RcvPos==1) { // lat/lon/height
                for (k=0;k<3;k++) pos[k]=OOPos[k];
                pos2ecef(pos,rr);
            }
            else { // rinex header position
                for (k=0;k<3;k++) rr[k]=Sta.pos[k];
                ecef2pos(rr,pos);
            }
            for (k=0;k<j-i;k++) {
                azel[k*2]=azel[1+k*2]=0.0;
                if (!satpos(time,time,Obs.data[i+k].sat,EPHOPT_BRDC,&Nav,rs,dts,
                            &var,&svh)) continue;
                if (geodist(rs,rr,e)>0.0) satazel(pos,e,azel+k*2);
            }
        }
        for (k=0;k<j-i;k++) {
            Az[i+k]=azel[  k*2];
            El[i+k]=azel[1+k*2];
            if (Az[i+k]<0.0) Az[i+k]+=2.0*PI;
        }
        per=(i+1)*100/Obs.n;
        if (per!=per_) {
            ShowMsg(s.sprintf("updating azimuth/elevation... (%d%%)",(per_=per)));
            Application->ProcessMessages();
        }
    }
    IndexObs[NObs]=Obs.n;
    
    UpdateSatList();
    
    ReadWaitEnd();
}
示例#5
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;
}