diff --git a/src/pntpos.c b/src/pntpos.c index 2b006c91b14c16be2960fc0f61ebe12eb58371cd..d6dc54ae0a72040c73afa8530cf37d472b975ddf 100644 --- a/src/pntpos.c +++ b/src/pntpos.c @@ -275,24 +275,45 @@ static int rescode(int iter, const obsd_t *obs, int n, const double *rs, /* pseudorange residual */ v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp); - //printf("sat=%2d:\n",obs[i].sat); - //printf("\tx=%5.1f %5.1f %5.1f\n",rr[0],rr[1],rr[2]); - //printf("\tnorm=%5.1f\n",r); - //printf("\tdtr=%5.8f\n",dtr); - //printf("\tP-(dtr-CLIGHT*dts[i*2]+dion+dtrp)=%5.1f\n",P-(dtr-CLIGHT*dts[i*2]+dion+dtrp)); - //printf("\tP-(-CLIGHT*dts[i*2]+dion+dtrp)=%5.1f\n",P-(-CLIGHT*dts[i*2]+dion+dtrp)); - //printf("\tsat=%5.1f %5.1f %5.1f\n",*(rs+1*6),*(rs+1*6+1),*(rs+1*6+2)); - //printf("\terror=%5.1f\n",v[nv]); - /* 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; - + + // DEBUGGING ================ + /*{ + printf("sat=%2d:\n",obs[i].sat); + printf("\tsys=%2d:\n",sys); + + double prange_corrected = P-(-CLIGHT*dts[i*2]+dion+dtrp); + double clock_bias_constellation = 0; + if (sys==SYS_GLO) + clock_bias_constellation = x[4]; + else if (sys==SYS_GAL) + clock_bias_constellation = x[5]; + else if (sys==SYS_CMP) + clock_bias_constellation = x[6]; + + printf("\tsat pos=%5.4f %5.4f %5.4f\n",*(rs+i*6),*(rs+i*6+1),*(rs+i*6+2)); + printf("\tx=%5.4f %5.4f %5.4f\n",x[0],x[1],x[2]); + printf("\tprange=%5.4f\n",P); + printf("\tsat_clock_corr=%5.4f\n",-CLIGHT*dts[i*2]); + printf("\tiono_corr=%5.4f\n",dion); + printf("\ttropo_corr=%5.4f\n",dtrp); + printf("\tprange_corrected=%5.4f\n",prange_corrected); + printf("\tclock_bias=%5.4f\n",dtr); + printf("\tclock_bias_constellation=%5.4f\n",clock_bias_constellation); + printf("\tnorm=%5.4f\n",r); + printf("\texp_wo=%5.4f\n",r+dtr); + printf("\texp=%5.4f\n",r+dtr+clock_bias_constellation); + printf("\t(exp-prange_corrected)=%5.4f\n",r+dtr+clock_bias_constellation-prange_corrected); + printf("\terror=%5.4f\n\n",v[nv]); + }//*/ + vsat[i]=1; resp[i]=v[nv]; (*ns)++; /* error variance */ @@ -618,6 +639,7 @@ extern int pntpos(const obsd_t *obs, int n, const nav_t *nav, if (!stat&&n>=6&&opt->posopt[4]) { stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,ssat,sol,azel_,vsat,resp,msg); } + /* estimate receiver velocity with doppler */ if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat); @@ -638,6 +660,26 @@ extern int pntpos(const obsd_t *obs, int n, const nav_t *nav, ssat[obs[i].sat-1].resp[0]=resp[i]; } } + + // DEBUG: compute and print final residuals + /*{ + printf("//////////////////// FINAL RESIDUALS:\n"); + double x[NX]={0},*v,*H,*var_aux; + int nv,ns; + + x[0]=sol->rr[0]; + x[1]=sol->rr[1]; + x[2]=sol->rr[2]; + x[3] = sol->dtr[0] * CLIGHT; + x[4] = sol->dtr[1] * CLIGHT; + x[5] = sol->dtr[2] * CLIGHT; + x[6] = sol->dtr[3] * CLIGHT; + printf("x=%5.3f %5.3f %5.3f %5.3f %5.3f %5.3f %5.3f\n",x[0],x[1],x[2],x[3],x[4],x[5],x[6]); + v=mat(n+4,1); H=mat(NX,n+4); var_aux=mat(n+4,1); + + nv=rescode(2,obs,n,rs,dts,var,svh,nav,x,opt,ssat,v,H,var_aux,azel,vsat,resp, &ns); + }//*/ + free(rs); free(dts); free(var); free(azel_); free(resp); return stat; }