static int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x,
const double *P, const int *sat, double *y, double *e,
double *azel, const int *iu, const int *ir, int ns, double *v,
double *H, double *R, int *vflg,char **msg)
{
prcopt_t *opt=&rtk->opt;
double bl,dr[3],posu[3],posr[3],*im;
double *tropr,*tropu,*dtdxr,*dtdxu,*Ri,*Rj,s,lami,lamj,*Hi=NULL;
int i,j,k,f,nv=0,nb[6]= {0},b=0,sysi,sysj,nf=1;
bl=baseline(x,rtk->rb,dr);
ecef2pos(x,posu);
ecef2pos(rtk->rb,posr);
sysi=sysj=SYS_GPS;
Ri=mat(ns*2+2,1);
Rj=mat (ns*2+2,1);
im=mat(ns,1);
tropu=mat(ns,1);
tropr=mat(ns,1);
dtdxu=mat(ns,3);
dtdxr=mat(ns,3);
for (i=0; i<MAX_SAT; i++) {
rtk->ssat[i].resp=rtk->ssat[i].resc=0.0;
}
for (f=0; f<2; f++)
{
/* search reference satellite with highest elevation */
for (i=-1,j=0; j<ns; j++) {
if (validobs(iu[j],ir[j],f,1,y)) continue;
if (i<0||azel[1+iu[j]*2]>=azel[1+iu[i]*2]) i=j;
}
if (i<0) continue;
/* make double difference */
for (j=0; j<ns; j++) {
if (i==j) continue;
if (validobs(iu[j],ir[j],f,1,y)) continue;
lami=lamj=LAM1;
if (H) Hi=H+nv*rtk->nx;
/* double-differenced residual */
v[nv]=(y[f+iu[i]*2]-y[f+ir[i]*2])-
(y[f+iu[j]*2]-y[f+ir[j]*2]);
/* partial derivatives by rover position */
if (H) {
for (k=0; k<3; k++) {
Hi[k]=-e[k+iu[i]*3]+e[k+iu[j]*3];
}
}
/* double-differenced phase-bias term */
if (f<1) {
v[nv]-=lami*x[IB(sat[i],f,opt)]-lamj*x[IB(sat[j],f,opt)];
if (H)
{
Hi[IB(sat[i],f,opt)]= lami;
Hi[IB(sat[j],f,opt)]=-lamj;
}
}
if (f<1) rtk->ssat[sat[j]-1].resc=v[nv];
else rtk->ssat[sat[j]-1].resp=v[nv];
/* test innovation */
if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) {
if (f<1) {
rtk->ssat[sat[i]-1].rejc++;
rtk->ssat[sat[j]-1].rejc++;
}
#ifdef _DEBUG_MSG
(*msg)+=sprintf(*msg,"outlier rejected (sat=%3d-%3d %s%d v=%.3f)\n",
sat[i],sat[j],f<1?"L":"P",f+1,v[nv]);
#endif
continue;
}
/* single-differenced measurement error variances */
Ri[nv]=varerr(sat[i],sysi,azel[1+iu[i]*2],bl,dt,f,opt);
Rj[nv]=varerr(sat[j],sysj,azel[1+iu[j]*2],bl,dt,f,opt);
/* set valid data flags */
if (f<1) rtk->ssat[sat[i]-1].vsat=rtk->ssat[sat[j]-1].vsat=1;
//trace(4,"sat=%3d-%3d %s%d v=%13.3f R=%8.6f %8.6f\n",sat[i],
// sat[j],f<nf?"L":"P",f%nf+1,v[nv],Ri[nv],Rj[nv]);
vflg[nv++]=(sat[i]<<16)|(sat[j]<<8)|((f<nf?0:1)<<4);
nb[b]++;
}
/* restore single-differenced residuals assuming sum equal zero */
if (f<nf) {
for (j=0,s=0.0; j<MAX_SAT; j++) s+=rtk->ssat[j].resc;
s/=nb[b]+1;
for (j=0; j<MAX_SAT; j++) {
if (j==sat[i]-1||rtk->ssat[j].resc!=0.0) rtk->ssat[j].resc-=s;
}
}
else {
for (j=0,s=0.0; j<MAX_SAT; j++) s+=rtk->ssat[j].resp;
s/=nb[b]+1;
for (j=0; j<MAX_SAT; j++) {
if (j==sat[i]-1||rtk->ssat[j].resp!=0.0)
rtk->ssat[j].resp-=s;
}
}
b++;
}
/* end of system loop */
/* baseline length constraint for moving baseline */
/* double-differenced measurement error covariance */
ddcov(nb,b,Ri,Rj,nv,R);
free(Ri);
free(Rj);
free(im);
free(tropu);
free(tropr);
free(dtdxu);
free(dtdxr);
return nv;
}
/* 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);
}
/* 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;
}
