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