/* 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;
}
