/* estimate receiver velocity ------------------------------------------------*/ static void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, const nav_t *nav, const prcopt_t *opt, sol_t *sol, const double *azel, const int *vsat) { double x[4]={0},dx[4],Q[16],*v,*H; int i,j,nv; trace(3,"estvel : n=%d\n",n); v=mat(n,1); H=mat(4,n); for (i=0;i<MAXITR;i++) { /* doppler residuals */ if ((nv=resdop(obs,n,rs,dts,nav,sol->rr,x,azel,vsat,v,H))<4) { break; } /* least square estimation */ if (lsq(H,v,4,nv,dx,Q)) break; for (j=0;j<4;j++) x[j]+=dx[j]; if (norm(dx,4)<1E-6) { for (i=0;i<3;i++) sol->rr[i+3]=x[i]; break; } } free(v); free(H); }
/* estimate receiver velocity ------------------------------------------------*/ static void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, const nav_t *nav, const prcopt_t *opt, sol_t *sol, const double *azel, const int *vsat) { double x[4]={0},dx[4],Q[16],*v,*H; int i,j,nv; trace(3,"estvel : n=%d\n",n); v=mat(n,1); H=mat(4,n); fprintf(output, " Begin velocity calculation\n"); for (i=0;i<MAXITR;i++) { fprintf(output, " Begin iteration %i\n", i); /* doppler residuals */ if ((nv=resdop(obs,n,rs,dts,nav,sol->rr,x,azel,vsat,v,H))<4) { break; } /* least square estimation */ if (lsq(H,v,4,nv,dx,Q)) break; fprintf(output, " Results of least square estimation: dx1 = %f, dx2 = %f, dx3 = %f, dx4 = %f\n", dx[0], dx[1], dx[2], dx[3]); for (j=0;j<4;j++) x[j]+=dx[j]; fprintf(output, " Vector V after update: V1=%f, V2 = %f, V3 = %f, V4 = %f\n", x[0], x[1], x[2] ,x[3]); fprintf(output, " norm(dx) = %f\n", norm(dx, 4)); if (norm(dx,4)<1E-6) { for (i=0;i<3;i++) sol->rr[i+3]=x[i]; for (i=0;i<3;i++) sol->qvr[i] = (float)Q[i+i*4]; sol->qvr[3] = (float)Q[1]; /* cov vxvy */ sol->qvr[4] = (float)Q[2+4]; /* cov vyvz */ sol->qvr[5] = (float)Q[2]; /* cov vzvx */ break; } } free(v); free(H); fprintf(output, " End velocity calculation\n"); }