debugging prints

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;
 }