/* single-point positioning ---------------------------------------------------- * compute receiver position, velocity, clock bias by single-point positioning * with pseudorange and doppler observables * args : obsd_t *obs I observation data * int n I number of observation data * nav_t *nav I navigation data * prcopt_t *opt I processing options * sol_t *sol IO solution * double *azel IO azimuth/elevation angle (rad) (NULL: no output) * ssat_t *ssat IO satellite status (NULL: no output) * char *msg O error message for error exit * return : status(1:ok,0:error) * notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and * receiver bias are negligible (only involving glonass-gps time offset * and receiver bias) *-----------------------------------------------------------------------------*/ extern int pntpos(const obsd_t *obs, int n, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, char *msg) { prcopt_t opt_=*opt; double *rs,*dts,*var,*azel_,*resp; int i,stat,vsat[MAXOBS]={0},svh[MAXOBS]; sol->stat=SOLQ_NONE; if (n<=0) {strcpy(msg,"no observation data"); return 0;} trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n); sol->time=obs[0].time; msg[0]='\0'; rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n); if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */ #if 0 opt_.sateph =EPHOPT_BRDC; #endif opt_.ionoopt=IONOOPT_BRDC; opt_.tropopt=TROPOPT_SAAS; } /* satellite positons, velocities and clocks */ satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh); /* estimate receiver position with pseudorange */ stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); /* raim fde */ if (!stat&&n>=6&&opt->posopt[4]) { stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); } /* estimate receiver velocity with doppler */ if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat); if (azel) { for (i=0;i<n*2;i++) azel[i]=azel_[i]; } if (ssat) { for (i=0;i<MAXSAT;i++) { ssat[i].vs=0; ssat[i].azel[0]=ssat[i].azel[1]=0.0; ssat[i].resp[0]=ssat[i].resc[0]=0.0; ssat[i].snr[0]=0; } for (i=0;i<n;i++) { ssat[obs[i].sat-1].azel[0]=azel_[ i*2]; ssat[obs[i].sat-1].azel[1]=azel_[1+i*2]; ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0]; if (!vsat[i]) continue; ssat[obs[i].sat-1].vs=1; ssat[obs[i].sat-1].resp[0]=resp[i]; } } free(rs); free(dts); free(var); free(azel_); free(resp); return stat; }
/* single-point positioning ---------------------------------------------------- * compute receiver position, velocity, clock bias by single-point positioning * with pseudorange and doppler observables * args : obsd_t *obs I observation data * int n I number of observation data * nav_t *nav I navigation data * prcopt_t *opt I processing options * sol_t *sol IO solution * double *azel IO azimuth/elevation angle (rad) (NULL: no output) * ssat_t *ssat IO satellite status (NULL: no output) * char *msg O error message for error exit * return : status(1:ok,0:error) * notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and * receiver bias are negligible (only involving glonass-gps time offset * and receiver bias) *-----------------------------------------------------------------------------*/ extern int pntpos(const obsd_t *obs, int n, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, char *msg) { prcopt_t opt_=*opt; double *rs,*dts,*var,*azel_,*resp; int i,stat,vsat[MAXOBS]={0},svh[MAXOBS]; double ep[6], ep2[6]; output = fopen("solution_log.txt", "a"); fprintf(output, "\nBegin solution in single point positioning mode\n"); fprintf(output, " Input observaton data:\n"); for(i = 0; i < n; i++) { time2epoch(obs[i].time, ep); fprintf(output, " %i) time: %4.0f/%2.0f/%2.0f %2.0f:%2.0f:%6.4f, sat: %i, code: %i, P1: %f, L1: %f, D1: %f, S1: %f, P2: %f, L2: %f, D2: %f, S2: %f\n", i+1, ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], obs[i].sat, obs[i].code, obs[i].P[0], obs[i].L[0], obs[i].D[0], obs[i].SNR[0], obs[i].P[1], obs[i].L[1], obs[i].D[1], obs[i].SNR[1]); } fprintf(output, " Input GPS ephemeris:\n"); for(i = 0; i < nav->n; i++) { time2epoch(nav->eph[i].toe, ep); time2epoch(nav->eph[i].toc, ep2); fprintf(output, " %i) sat: %i, IODE: %i, sva: %i, svh: %i, week: %i, toe: %4.0f/%2.0f/%2.0f %2.0f:%2.0f:%6.4f, toc: %4.0f/%2.0f/%2.0f %2.0f:%2.0f:%6.4f, sqrt(A): %f, E: %f, i0: %f, OMG0: %f, omega: %f, M0: %f, delta_n: %f, OMGdot: %f, Idot: %f, Crc: %f, Crs: %f, Cuc: %f, Cus: %f, Cic: %f, Cis: %f, af0: %f, af1: %f, af2: %f, Tgd: %f\n", i + 1, nav->eph[i].sat, nav->eph[i].iode, nav->eph[i].sva, nav->eph[i].svh, nav->eph[i].week, ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], ep2[0], ep2[1], ep2[2], ep2[3], ep2[4], ep2[5], sqrt(nav->eph[i].A), nav->eph[i].e, nav->eph[i].i0, nav->eph[i].OMG0, nav->eph[i].omg, nav->eph[i].M0, nav->eph[i].deln, nav->eph[i].OMGd, nav->eph[i].idot, nav->eph[i].crc, nav->eph[i].crs, nav->eph[i].cuc, nav->eph[i].cus, nav->eph[i].cic, nav->eph[i].cis, nav->eph[i].f0, nav->eph[i].f0, nav->eph[i].f1, nav->eph[i].f2, nav->eph[i].tgd[0]); } fprintf(output, " Input GLONASS ephemeris:\n"); for(i = 0; i < nav->ng; i++) { time2epoch(nav->geph[i].toe, ep); fprintf(output, " %i) sat: %i, sva: %i, svh: %i, toe: %4.0f/%2.0f/%2.0f %2.0f:%2.0f:%6.4f, X: %f, Y: %f, Z: %f, VX: %f, VY: %f, VZ: %f, tau_n: %f, gamma_n: %f\n", i + 1, nav->geph[i].sat, nav->geph[i].sva, nav->geph[i].svh, ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], nav->geph[i].pos[0], nav->geph[i].pos[1], nav->geph[i].pos[2], nav->geph[i].vel[0], nav->geph[i].vel[1], nav->geph[i].vel[2], nav->geph[i].taun, nav->geph[i].gamn); } sol->stat=SOLQ_NONE; if (n<=0) {strcpy(msg,"no observation data"); return 0;} trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n); sol->time=obs[0].time; msg[0]='\0'; rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n); if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */ #if 0 opt_.sateph =EPHOPT_BRDC; #endif opt_.ionoopt=IONOOPT_BRDC; opt_.tropopt=TROPOPT_SAAS; } /* satellite positons, velocities and clocks */ satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh); fprintf(output, " Satellite positions, velocities and clocks:\n"); for(i = 0; i < n; i++) { fprintf(output, " sat %i: X = %f, Y = %f, Z = %f, VX = %f, VY = %f, VZ = %f, dt = %f, dt_dot = %f\n", obs[i].sat, rs[i*6], rs[i*6 + 1], rs[i*6 + 2], rs[i*6 + 3], rs[i*6 + 4], rs[i*6 + 5], dts[i*2], dts[i*2 + 1]); } /* estimate receiver position with pseudorange */ stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); /* raim fde */ if (!stat&&n>=6&&opt->posopt[4]) { stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); } /* estimate receiver velocity with doppler */ if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat); if (azel) { for (i=0;i<n*2;i++) azel[i]=azel_[i]; } if (ssat) { for (i=0;i<MAXSAT;i++) { ssat[i].vs=0; ssat[i].azel[0]=ssat[i].azel[1]=0.0; ssat[i].resp[0]=ssat[i].resc[0]=0.0; ssat[i].snr[0]=0; } for (i=0;i<n;i++) { ssat[obs[i].sat-1].azel[0]=azel_[ i*2]; ssat[obs[i].sat-1].azel[1]=azel_[1+i*2]; ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0]; if (!vsat[i]) continue; ssat[obs[i].sat-1].vs=1; ssat[obs[i].sat-1].resp[0]=resp[i]; } } free(rs); free(dts); free(var); free(azel_); free(resp); fprintf(output, "End calculations in single point positioning mode\n"); fclose(output); output = fopen("cov_matr_single.txt", "a"); time2epoch(sol->time, ep); fprintf(output, "%4.0f/%2.0f/%2.0f %2.0f:%2.0f:%2.4f coords: %f %f %f %f %f %f velocities: %f %f %f %f %f %f\n", ep[0], ep[1], ep[2], ep[3], ep[4], ep[5], sol->qr[0], sol->qr[3], sol->qr[5], sol->qr[1], sol->qr[4], sol->qr[2], sol->qvr[0], sol->qvr[3], sol->qvr[5], sol->qvr[1], sol->qvr[4], sol->qvr[2]); fclose(output); return stat; }