/* ionospheric correction ------------------------------------------------------ * compute ionospheric correction * args : gtime_t time I time * nav_t *nav I navigation data * int sat I satellite number * double *pos I receiver position {lat,lon,h} (rad|m) * double *azel I azimuth/elevation angle {az,el} (rad) * int ionoopt I ionospheric correction option (IONOOPT_???) * double *ion O ionospheric delay (L1) (m) * double *var O ionospheric delay (L1) variance (m^2) * return : status(1:ok,0:error) *-----------------------------------------------------------------------------*/ extern int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos, const double *azel, int ionoopt, double *ion, double *var) { trace(4,"ionocorr: time=%s opt=%d sat=%2d pos=%.3f %.3f azel=%.3f %.3f\n", time_str(time,3),ionoopt,sat,pos[0]*R2D,pos[1]*R2D,azel[0]*R2D, azel[1]*R2D); /* broadcast model */ if (ionoopt==IONOOPT_BRDC) { *ion=ionmodel(time,nav->ion_gps,pos,azel); *var=SQR(*ion*ERR_BRDCI); return 1; } /* 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); } /* qzss broadcast model */ if (ionoopt==IONOOPT_QZS&&norm(nav->ion_qzs,8)>0.0) { *ion=ionmodel(time,nav->ion_qzs,pos,azel); *var=SQR(*ion*ERR_BRDCI); return 1; } /* lex ionosphere model */ if (ionoopt==IONOOPT_LEX) { return lexioncorr(time,nav,pos,azel,ion,var); } *ion=0.0; *var=ionoopt==IONOOPT_OFF?SQR(ERR_ION):0.0; return 1; }
/* slant ionospheric delay ---------------------------------------------------*/ static 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) { #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 */ *ion=ionmodel(time,nav->ion_gps,pos,azel); *var=SQR(*ion*ERR_BRDCI); return 1; }
/* ionmodel() */ void utest1(void) { double e1[]={2007,1,16, 1,0,0}; double e2[]={2007,1,16,13,0,0}; double e3[]={2007,1,16,22,0,0}; double ion1[]={ 0.2E-7,-0.8e-8,-0.5e-7, 0.1e-6, 0.2E+6, 0.2e+6,-0.1e+6,-0.1e+7 }; double ion2[]={ 0.2E-7,-0.8e-8,-0.5e-7, 0.1e-6, 0.2E+6, 0.2e+6,-0.1e+6,-0.1e+7 }; double pos1 []={ 35*D2R, 140*D2R, 100.0}; double pos2 []={-80*D2R,-170*D2R,1000.0}; double pos3 []={ 10*D2R, 30*D2R, 0.0}; double azel1[]={ 60*D2R, 75*D2R}; double azel2[]={190*D2R, 3*D2R}; double azel3[]={350*D2R, 60*D2R}; double azel4[]={ 0*D2R, 90*D2R}; gtime_t t1=epoch2time(e1),t2=epoch2time(e2),t3=epoch2time(e3); double dion; dion=ionmodel(t1,ion1,pos1,azel1); assert(fabs(dion-6.73590532099438)<1e-8); dion=ionmodel(t1,ion1,pos2,azel1); assert(fabs(dion-3.56895382197387)<1e-8); dion=ionmodel(t1,ion1,pos3,azel1); assert(fabs(dion-3.80716435655161)<1e-8); dion=ionmodel(t2,ion1,pos1,azel1); assert(fabs(dion-5.21796954585452)<1e-8); dion=ionmodel(t3,ion1,pos1,azel1); assert(fabs(dion-5.90190539264777)<1e-8); dion=ionmodel(t1,ion1,pos1,azel2); assert(fabs(dion-21.6345415123632)<1e-8); dion=ionmodel(t1,ion1,pos1,azel3); assert(fabs(dion-7.33844278822561)<1e-8); dion=ionmodel(t1,ion1,pos1,azel4); assert(fabs(dion-6.58339711400694)<1e-8); dion=ionmodel(t1,ion2,pos1,azel1); assert(fabs(dion-6.73590532099438)<1e-8); printf("%s utest1 : OK\n",__FILE__); }
/* temporal update of states --------------------------------------------------*/ static void ud_state(const obsd_t *obs, int n, const nav_t *nav, const double *pos, const double *azel, ekf_t *ekf, sstat_t *sstat) { double P1,P2,L1,L2,PG,LG,tt,F[4]={0},Q[2]={0}; double x[2]={0},P[2],c_iono=1.0-SQR(lam[1]/lam[0]); int i,sat,slip; for (i=0;i<n;i++) { /* raw pseudorange and phase range */ if (!raw_obs(obs+i,nav,&P1,&P2,&L1,&L2)||azel[i*2+1]<MIN_EL) continue; sat=obs[i].sat; tt=timediff(obs[i].time,sstat[sat-1].time); LG=L1-L2; PG=P1-P2; slip=(obs[i].LLI[0]&3)||(obs[i].LLI[1]&3); slip|=fabs(LG-sstat[sat-1].LG)>THRES_LG; if (fabs(tt)>MAXGAP_IONO) { #if 1 x[0]=PG/c_iono; #else x[0]=ionmodel(obs[i].time,nav->ion_gps,pos,azel+i*2); #endif x[1]=1E-6; P[0]=VAR_IONO; P[1]=VAR_IONR; ekf_init(ekf,x,P,II(sat),2); } else { F[0]=F[3]=1.0; F[2]=tt; Q[0]=PRN_IONO*fabs(tt); Q[1]=PRN_IONR*fabs(tt); ekf_pred(ekf,F,Q,II(sat),2); } if (tt>MAXGAP_BIAS||slip) { x[0]=LG+PG; P[0]=VAR_BIAS; ekf_init(ekf,x,P,IB(sat),1); } sstat[sat-1].time=obs[i].time; sstat[sat-1].azel[0]=azel[i*2]; sstat[sat-1].azel[1]=azel[i*2+1]; sstat[sat-1].slip=slip; sstat[sat-1].LG=LG; sstat[sat-1].PG=PG; } }
/* 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; }