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