diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h index f2c3aa049b25b149eaf7ac825d0d859d6e704358..06a0d310ee6006809bea5a03aeed5feddd8eb85e 100644 --- a/include/gnss_utils/gnss_utils.h +++ b/include/gnss_utils/gnss_utils.h @@ -39,16 +39,21 @@ namespace GNSSUtils }; ComputePosOutput computePos(const Observations & _observations, - Navigation & _navigation, - const prcopt_t & _prcopt); + Navigation & _navigation, + const prcopt_t & _prcopt); -// ComputePosOutput computePosOwn(const Observations & _observations, -// Navigation & _navigation, -// const prcopt_t & _prcopt); -// -// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav, -// const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, -// char *msg); + ComputePosOutput computePosOwn(const Observations & _observations, + Navigation & _navigation, + const prcopt_t & _prcopt); + + int pntposOwn(const obsd_t *obs, int n, const nav_t *nav, + const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, + char *msg); + + int estposOwn(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) Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef); } diff --git a/include/gnss_utils/single_differences.h b/include/gnss_utils/single_differences.h new file mode 100644 index 0000000000000000000000000000000000000000..9c48af4a3390ad33e930b7274d384b5a7391a6af --- /dev/null +++ b/include/gnss_utils/single_differences.h @@ -0,0 +1,54 @@ +/* + * single_differences.h + * + * Created on: Dec 4, 2019 + * Author: jvallve + */ + +#ifndef INCLUDE_GNSS_UTILS_SINGLE_DIFFERENCES_H_ +#define INCLUDE_GNSS_UTILS_SINGLE_DIFFERENCES_H_ + +#include "gnss_utils.h" +extern "C" +{ + #include "rtklib.h" +} + +namespace GNSSUtils +{ + struct SingleDifferencesParams + { + int common_sat_min; + int raim_n; + double raim_min_residual; + bool use_carrier_phase; + bool correct_tropo; + bool relinearize_jacobian; + int max_iterations; + double sigma_atm; + double sigma_code; + double sigma_carrier; + }; + + bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, + const Observations& obs_k, const Navigation& nav_r, + Eigen::Vector4d& d, Eigen::Matrix4d& cov_d, + double& residual, std::list<int>& discarded_sats, + const SingleDifferencesParams& sd_opt, const prcopt_t& opt); + + bool singleDifferences(const Observations& obs_r, const Eigen::Vector3d& pos_r, + const Observations& obs_k, const Navigation& nav_r, + Eigen::Vector4d& d, Eigen::Matrix4d& cov_d, + double& residual, std::list<int>& discarded_sats, + const SingleDifferencesParams& sd_opt, const prcopt_t& opt); + + bool singleDifferences(const Observations& common_obs_r, const Eigen::Vector3d& pos_r, + const Observations& common_obs_k, const std::map<int,Eigen::Vector3d>& common_sats_pos, + Eigen::Vector4d& d, Eigen::Matrix4d& cov_d, + double& residual, std::list<int>& discarded_sats, + const SingleDifferencesParams& sd_opt, const prcopt_t& opt); + + +} + +#endif /* INCLUDE_GNSS_UTILS_SINGLE_DIFFERENCES_H_ */ diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index fc610d33faaa89b374209caea5bf7afd0cfbd27f..12d787d210f74ab3fa8ddfe041661bd77684b075 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -5,4 +5,4 @@ INCLUDE_DIRECTORIES(../../include) ADD_EXECUTABLE(gnss_utils_test gnss_utils_test.cpp) # link necessary libraries -TARGET_LINK_LIBRARIES(gnss_utils_test ${gnss_utils_LIBRARIES}) +TARGET_LINK_LIBRARIES(gnss_utils_test gnss_utils) diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp index 018da28b37ad34443c03268fca6d14639a36ce8f..1398feceb9ee850c6801c58ace41eef0830c697e 100644 --- a/src/gnss_utils.cpp +++ b/src/gnss_utils.cpp @@ -53,131 +53,199 @@ namespace GNSSUtils return output; } -// ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations, -// GNSSUtils::Navigation & _navigation, -// const prcopt_t & _prcopt) -// { -// -// // Remove duplicated satellites -// uniqnav(&(_navigation.getNavigation())); -// -// // Define error msg -// char msg[128] = ""; -// -// GNSSUtils::ComputePosOutput output; -// sol_t sol; -// sol = {{0}}; -// -// output.pos_stat = pntposOwn(_observations.getObservations().data(), _observations.getObservations().size(), -// &(_navigation.getNavigation()), -// &_prcopt, &sol, NULL, NULL, msg); -// -// if (output.pos_stat == 0) -// { -// std::cout << "Bad computing: " << msg << "\n"; -// } -// -// output.time = sol.time.time; -// output.time = sol.time.sec; -// output.pos = Eigen::Vector3d(sol.rr); -// // std::cout << "Compute pos: " << output.pos.transpose() << "\n"; -// output.vel = Eigen::Vector3d(&sol.rr[3]); -// output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5], -// sol.qr[3], sol.qr[1], sol.qr[4], -// sol.qr[5], sol.qr[3], sol.qr[2]; -// -// // XXX: segmentation fault here. -// // if (sol.dtr != NULL) -// // { -// // output.rcv_bias << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5]; -// // } -// -// output.type = sol.type; -// output.stat = sol.stat; -// output.ns = sol.ns; -// output.age = sol.age; -// output.ratio = sol.ratio; -// output.lat_lon = ecefToLatLon(output.pos); -// -// return output; -// } -// -// /* single-point positioning COPIED FROM RTKLIB ------------------------------ -// * 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) -// *-----------------------------------------------------------------------------*/ -// int pntposOwn(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; -// } + ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations, + GNSSUtils::Navigation & _navigation, + const prcopt_t & _prcopt) + { + + // Remove duplicated satellites + uniqnav(&(_navigation.getNavigation())); + + // Define error msg + char msg[128] = ""; + + GNSSUtils::ComputePosOutput output; + sol_t sol; + sol = {{0}}; + + output.pos_stat = pntposOwn(_observations.getObservations().data(), _observations.getObservations().size(), + &(_navigation.getNavigation()), + &_prcopt, &sol, NULL, NULL, msg); + + if (output.pos_stat == 0) + { + std::cout << "Bad computing: " << msg << "\n"; + } + + output.time = sol.time.time; + output.time = sol.time.sec; + output.pos = Eigen::Vector3d(sol.rr); + // std::cout << "Compute pos: " << output.pos.transpose() << "\n"; + output.vel = Eigen::Vector3d(&sol.rr[3]); + output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5], + sol.qr[3], sol.qr[1], sol.qr[4], + sol.qr[5], sol.qr[3], sol.qr[2]; + + // XXX: segmentation fault here. + // if (sol.dtr != NULL) + // { + // output.rcv_bias << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5]; + // } + + output.type = sol.type; + output.stat = sol.stat; + output.ns = sol.ns; + output.age = sol.age; + output.ratio = sol.ratio; + output.lat_lon = ecefToLatLon(output.pos); + + return output; + } + + /* single-point positioning COPIED FROM RTKLIB ------------------------------ + * 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) + *-----------------------------------------------------------------------------*/ + int pntposOwn(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=estposOwn(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; + } + + int estposOwn(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) + { + double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig; + int i,j,k,info,stat,nv,ns; + + trace(3,"estpos : n=%d\n",n); + + v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1); + + for (i=0;i<3;i++) x[i]=sol->rr[i]; + + for (i=0;i<MAXITR;i++) { + + /* pseudorange residuals */ + nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp, + &ns); + + if (nv<NX) { + sprintf(msg,"lack of valid sats ns=%d",nv); + break; + } + /* weight by variance */ + for (j=0;j<nv;j++) { + sig=sqrt(var[j]); + v[j]/=sig; + for (k=0;k<NX;k++) H[k+j*NX]/=sig; + } + /* least square estimation */ + if ((info=lsq(H,v,NX,nv,dx,Q))) { + sprintf(msg,"lsq error info=%d",info); + break; + } + for (j=0;j<NX;j++) x[j]+=dx[j]; + + if (norm(dx,NX)<1E-4) { + sol->type=0; + sol->time=timeadd(obs[0].time,-x[3]/CLIGHT); + sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */ + sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */ + sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */ + sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */ + for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0; + for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX]; + sol->qr[3]=(float)Q[1]; /* cov xy */ + sol->qr[4]=(float)Q[2+NX]; /* cov yz */ + sol->qr[5]=(float)Q[2]; /* cov zx */ + sol->ns=(unsigned char)ns; + sol->age=sol->ratio=0.0; + + /* validate solution */ + if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) { + sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE; + } + free(v); free(H); free(var); + + return stat; + } + } + if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i); + + free(v); free(H); free(var); + + return 0; + } Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef) { diff --git a/src/single_differences.cpp b/src/single_differences.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0ce3affcc8adff3275321b89afee5a2f5eb0e846 --- /dev/null +++ b/src/single_differences.cpp @@ -0,0 +1,61 @@ +/* + * single_differences.cpp + * + * Created on: Dec 4, 2019 + * Author: jvallve + */ + +#include "single_differences.h" + +namespace GNSSUtils +{ + +bool singleDifferences(const Observations& obs_r, const Navigation& nav_r, + const Observations& obs_k, const Navigation& nav_k, + Eigen::Vector4d& d, Eigen::Matrix4d& cov_d, + double& residual, std::list<int>& discarded_sats, + const SingleDifferencesParams& sd_params, const prcopt_t& opt) +{ + // TODO: COMPUTE RECEIVER POSITION AT t_r + Eigen::Vector3d pos_r(Eigen::Vector3d::Zero()); + + // COMPUTE SINGLE DIFFERENCES + return singleDifferences(obs_r, pos_r, + obs_k, nav_k, + d, cov_d, + residual, discarded_sats, + sd_params, opt); +} + +bool singleDifferences(const Observations& obs_r, const Eigen::Vector3d& pos_r, + const Observations& obs_k, const Navigation& nav_k, + Eigen::Vector4d& d, Eigen::Matrix4d& cov_d, + double& residual, std::list<int>& discarded_sats, + const SingleDifferencesParams& sd_params, const prcopt_t& opt) +{ + // TODO: FIND COMMON SATELLITES + Observations common_obs_r, common_obs_k; + + // TODO: COMPUTE COMMON SATELLITES POSITION + std::map<int,Eigen::Vector3d> common_sats_pos; + + // COMPUTE SINGLE DIFFERENCES + return singleDifferences(common_obs_r, pos_r, + common_obs_k, common_sats_pos, + d, cov_d, + residual, discarded_sats, + sd_params, opt); +} + +bool singleDifferences(const Observations& common_obs_r, const Eigen::Vector3d& pos_r, + const Observations& common_obs_k, const std::map<int,Eigen::Vector3d>& common_sats_pos, + Eigen::Vector4d& d, Eigen::Matrix4d& cov_d, + double& residual, std::list<int>& discarded_sats, + const SingleDifferencesParams& sd_params, const prcopt_t& opt) +{ + return true; +} + +} + +