/* 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);
}
/*
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;
}
void utest4(void)
{
double mapfd,mapfw;
double e1[]={2007,1,16,6,0,0},e2[]={2030,12,31,23,59,59};
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);
mapfd=tropmapf(t1,pos1,azel1,&mapfw);
assert(fabs(mapfd-1.035184526466435)<1e-8);
assert(fabs(mapfw-1.035233787448654)<1e-8);
mapfd=tropmapf(t1,pos1,azel2,&mapfw);
assert(fabs(mapfd-14.643271711748200)<1e-8);
assert(fabs(mapfw-16.455045694559484)<1e-8);
mapfd=tropmapf(t1,pos1,azel3,&mapfw);
assert(fabs(mapfd-1.154226397147367)<1e-8);
assert(fabs(mapfw-1.154481126139610)<1e-8);
mapfd=tropmapf(t1,pos1,azel4,&mapfw);
assert(fabs(mapfd-1.000000000000000)<1e-8);
assert(fabs(mapfw-1.000000000000000)<1e-8);
mapfd=tropmapf(t2,pos1,azel1,&mapfw);
assert(fabs(mapfd-1.035184415128022)<1e-8);
assert(fabs(mapfw-1.035233787448654)<1e-8);
mapfd=tropmapf(t1,pos2,azel1,&mapfw);
assert(fabs(mapfd-1.035186155749051)<1e-8);
assert(fabs(mapfw-1.035230548304367)<1e-8);
mapfd=tropmapf(t1,pos3,azel1,&mapfw);
assert(fabs(mapfd-1.035181919429758)<1e-8);
assert(fabs(mapfw-1.035233200318210)<1e-8);
mapfd=tropmapf(t1,pos1,azel1,NULL);
assert(fabs(mapfd-1.035184526466435)<1e-8);
printf("%s utest4 : OK\n",__FILE__);
}
