/* 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");
}
