/* 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;
}
//SPP, added by yuan;
extern int pntpos(rtk_t *rtk,obsd_t *obs, int n, nav_t *nav,double *azel,char *msg)
{
prcopt_t opt_=rtk->opt;
int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
double rs[6 * MAXOBS_]; double dts[2 * MAXOBS_]; double var[1 * MAXOBS_];
double azel_[2 * MAXOBS_]; double resp[1 * MAXOBS_];
rtk->sol.stat=SOLQ_NONE;
if (n<=0) { return 0;}
rtk->sol.time = obs[0].time; msg[0] = '\0';
opt_.sateph =EPHOPT_BRDC; //���ù㲥������modified by yuan
opt_.ionoopt=IONOOPT_BRDC;
opt_.tropopt=TROPOPT_SAAS;
/* satellite positons, velocities and clocks */
satposs(rtk->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_, &rtk->sol, azel_, vsat, resp, msg, rtk);
if (azel) {
for (i=0;i<n*2;i++) azel[i]=azel_[i];
}
if (rtk->ssat) {
for (i=0;i<MAXSAT;i++) {
rtk->ssat[i].vs = 0;
rtk->ssat[i].azel[0] = rtk->ssat[i].azel[1] = 0.0;
rtk->ssat[i].resp[0] = rtk->ssat[i].resc[0] = 0.0;
rtk->ssat[i].snr[0] = 0;
}
for (i=0;i<n;i++) {
rtk->ssat[obs[i].sat - 1].azel[0] = azel_[i * 2];
rtk->ssat[obs[i].sat - 1].azel[1] = azel_[1 + i * 2];
rtk->ssat[obs[i].sat - 1].snr[0] = obs[i].SNR[0];
if (!vsat[i]) continue;
rtk->ssat[obs[i].sat - 1].vs = 1;
rtk->ssat[obs[i].sat - 1].resp[0] = resp[i];
}
}
return stat;
}
/* raim fde (failure detection and exclution) -------------------------------*/
static int raim_fde(const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const prcopt_t *opt, sol_t *sol,
double *azel, int *vsat, double *resp, char *msg)
{
obsd_t *obs_e;
sol_t sol_e={{0}};
char tstr[32],name[16],msg_e[128];
double *rs_e,*dts_e,*vare_e,*azel_e,*resp_e,rms_e,rms=100.0;
int i,j,k,nvsat,stat=0,*svh_e,*vsat_e,sat=0;
trace(3,"raim_fde: %s n=%2d\n",time_str(obs[0].time,0),n);
if (!(obs_e=(obsd_t *)malloc(sizeof(obsd_t)*n))) return 0;
rs_e = mat(6,n); dts_e = mat(2,n); vare_e=mat(1,n); azel_e=zeros(2,n);
svh_e=imat(1,n); vsat_e=imat(1,n); resp_e=mat(1,n);
for (i=0;i<n;i++) {
/* satellite exclution */
for (j=k=0;j<n;j++) {
if (j==i) continue;
obs_e[k]=obs[j];
matcpy(rs_e +6*k,rs +6*j,6,1);
matcpy(dts_e+2*k,dts+2*j,2,1);
vare_e[k]=vare[j];
svh_e[k++]=svh[j];
}
/* estimate receiver position without a satellite */
if (!estpos(obs_e,n-1,rs_e,dts_e,vare_e,svh_e,nav,opt,&sol_e,azel_e,
vsat_e,resp_e,msg_e)) {
trace(3,"raim_fde: exsat=%2d (%s)\n",obs[i].sat,msg);
continue;
}
for (j=nvsat=0,rms_e=0.0;j<n-1;j++) {
if (!vsat_e[j]) continue;
rms_e+=SQR(resp_e[j]);
nvsat++;
}
if (nvsat<5) {
trace(3,"raim_fde: exsat=%2d lack of satellites nvsat=%2d\n",
obs[i].sat,nvsat);
continue;
}
rms_e=sqrt(rms_e/nvsat);
trace(3,"raim_fde: exsat=%2d rms=%8.3f\n",obs[i].sat,rms_e);
if (rms_e>rms) continue;
/* save result */
for (j=k=0;j<n;j++) {
if (j==i) continue;
matcpy(azel+2*j,azel_e+2*k,2,1);
vsat[j]=vsat_e[k];
resp[j]=resp_e[k++];
}
stat=1;
*sol=sol_e;
sat=obs[i].sat;
rms=rms_e;
vsat[i]=0;
strcpy(msg,msg_e);
}
if (stat) {
time2str(obs[0].time,tstr,2); satno2id(sat,name);
trace(2,"%s: %s excluded by raim\n",tstr+11,name);
}
free(obs_e);
free(rs_e ); free(dts_e ); free(vare_e); free(azel_e);
free(svh_e); free(vsat_e); free(resp_e);
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;
}
