/* pseudorange residuals -----------------------------------------------------*/ static int rescode(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, prcopt_t *opt, double *v, double *H, double *var, double *azel, int *vsat, double *resp, int *ns, rtk_t *rtk) { double r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P; int i, j, nv = 0, sys, mask[4] = { 0 }; double azl = 0; for (i=0;i<3;i++) rr[i]=x[i]; dtr=x[3]; ecef2pos(rr,pos); for (i=*ns=0;i<n&&i<MAXOBS;i++) { vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0; if (obs[i].sat>32){ //added by yuan; continue; } //���������ز�����α��Ͳ�����������ǵķ���; if ((fabs(obs[i].L[0]) < 0.0001) || (fabs(obs[i].P[0]) < 0.0001)){ continue; } if (!(sys=satsys(obs[i].sat,NULL))) continue; /* reject duplicated observation data */ if (i<n-1&&i<MAXOBS-1&&obs[i].sat==obs[i+1].sat) { i++; 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; azl = satazel(pos, e, azel + i * 2); /* psudorange with code bias correction */ if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) continue; /* excluded satellite? */ if (satexclude(obs[i].sat,svh[i],opt)) continue; /* ionospheric corrections */ if (!ionocorr(obs[i].time,nav,obs[i].sat,pos,azel+i*2, iter>0?opt->ionoopt:IONOOPT_BRDC,&dion,&vion)) continue; /* tropospheric corrections */ if (!tropcorr(obs[i].time,nav,pos,azel+i*2, iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) { continue; } /* pseudorange residual */ v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp); /* design matrix */ for (j=0;j<NX;j++) H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0); /* time system and receiver bias offset correction */ if (sys==SYS_GLO) {v[nv]-=x[4]; H[4+nv*NX]=1.0; mask[1]=1;} else if (sys==SYS_GAL) {v[nv]-=x[5]; H[5+nv*NX]=1.0; mask[2]=1;} else if (sys==SYS_CMP) {v[nv]-=x[6]; H[6+nv*NX]=1.0; mask[3]=1;} else mask[0]=1; vsat[i]=1; resp[i]=v[nv]; (*ns)++; rtk->sat_[nv] = obs[i].sat; //added by yuan; /* error variance */ if ((rtk->counter > 5) && (obs[i].mark_ < 4)){ var[nv++] = varerr(opt, azel[1 + i * 2], sys) + (4 - obs[i].mark_)*2 + vare[i] + vmeas + vion + vtrp; //varm:α���ز�����;vare:������������;vart:����������; //var[nv - 1] = var[nv - 1] * (4-obs[i].mark_); //�������ʧ���������������Ŵ�; } else{ var[nv++] = varerr(opt, azel[1 + i * 2], sys) + vare[i] + vmeas + vion + vtrp; //varm:α���ز�����;vare:������������;vart:����������; } } /* constraint to avoid rank-deficient */ for (i=0;i<4;i++) { if (mask[i]) continue; v[nv]=0.0; for (j=0;j<NX;j++) H[j+nv*NX]=j==i+3?1.0:0.0; var[nv++]=0.01; } return nv; }
/* pseudorange residuals -----------------------------------------------------*/ static int rescode(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, const prcopt_t *opt, double *v, double *H, double *var, double *azel, int *vsat, double *resp, int *ns) { double r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P,lam_L1; int i,j,nv=0,sys,mask[4]={0}; trace(3,"resprng : n=%d\n",n); for (i=0;i<3;i++) rr[i]=x[i]; dtr=x[3]; ecef2pos(rr,pos); for (i=*ns=0;i<n&&i<MAXOBS;i++) { vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0; if (!(sys=satsys(obs[i].sat,NULL))) continue; /* reject duplicated observation data */ if (i<n-1&&i<MAXOBS-1&&obs[i].sat==obs[i+1].sat) { trace(2,"duplicated observation data %s sat=%2d\n", time_str(obs[i].time,3),obs[i].sat); i++; 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; /* psudorange with code bias correction */ if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) continue; /* excluded satellite? */ if (satexclude(obs[i].sat,svh[i],opt)) continue; /* ionospheric corrections */ if (!ionocorr(obs[i].time,nav,obs[i].sat,pos,azel+i*2, iter>0?opt->ionoopt:IONOOPT_BRDC,&dion,&vion)) continue; /* GPS-L1 -> L1/B1 */ if ((lam_L1=nav->lam[obs[i].sat-1][0])>0.0) { dion*=SQR(lam_L1/lam_carr[0]); } /* tropospheric corrections */ if (!tropcorr(obs[i].time,nav,pos,azel+i*2, iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) { continue; } /* pseudorange residual */ v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp); /* design matrix */ for (j=0;j<NX;j++) H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0); /* time system and receiver bias offset correction */ if (sys==SYS_GLO) {v[nv]-=x[4]; H[4+nv*NX]=1.0; mask[1]=1;} else if (sys==SYS_GAL) {v[nv]-=x[5]; H[5+nv*NX]=1.0; mask[2]=1;} else if (sys==SYS_CMP) {v[nv]-=x[6]; H[6+nv*NX]=1.0; mask[3]=1;} else mask[0]=1; vsat[i]=1; resp[i]=v[nv]; (*ns)++; /* error variance */ var[nv++]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp; trace(4,"sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n",obs[i].sat, azel[i*2]*R2D,azel[1+i*2]*R2D,resp[i],sqrt(var[nv-1])); } /* constraint to avoid rank-deficient */ for (i=0;i<4;i++) { if (mask[i]) continue; v[nv]=0.0; for (j=0;j<NX;j++) H[j+nv*NX]=j==i+3?1.0:0.0; var[nv++]=0.01; } return nv; }