Skip to content
Snippets Groups Projects
Commit edbded11 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

debugging prints

parent afa37436
No related branches found
No related tags found
No related merge requests found
...@@ -275,24 +275,45 @@ static int rescode(int iter, const obsd_t *obs, int n, const double *rs, ...@@ -275,24 +275,45 @@ static int rescode(int iter, const obsd_t *obs, int n, const double *rs,
/* pseudorange residual */ /* pseudorange residual */
v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp); 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 */ /* design matrix */
for (j=0;j<NX;j++) H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0); 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 */ /* time system and receiver bias offset correction */
if (sys==SYS_GLO) {v[nv]-=x[4]; H[4+nv*NX]=1.0; mask[1]=1;} 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_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 if (sys==SYS_CMP) {v[nv]-=x[6]; H[6+nv*NX]=1.0; mask[3]=1;}
else mask[0]=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)++; vsat[i]=1; resp[i]=v[nv]; (*ns)++;
/* error variance */ /* error variance */
...@@ -618,6 +639,7 @@ extern int pntpos(const obsd_t *obs, int n, const nav_t *nav, ...@@ -618,6 +639,7 @@ extern int pntpos(const obsd_t *obs, int n, const nav_t *nav,
if (!stat&&n>=6&&opt->posopt[4]) { if (!stat&&n>=6&&opt->posopt[4]) {
stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,ssat,sol,azel_,vsat,resp,msg); stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,ssat,sol,azel_,vsat,resp,msg);
} }
/* estimate receiver velocity with doppler */ /* estimate receiver velocity with doppler */
if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat); 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, ...@@ -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]; 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); free(rs); free(dts); free(var); free(azel_); free(resp);
return stat; return stat;
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment