Skip to content
Snippets Groups Projects
Commit 57dcf28b authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

WIP

parent 882d0aa8
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
Hide whitespace changes
Inline Side-by-side
include/gnss_utils/gnss_utils.h
+ 14
− 9
View file @ 57dcf28b
...@@ -39,16 +39,21 @@ namespace GNSSUtils ...@@ -39,16 +39,21 @@ namespace GNSSUtils
}; };
ComputePosOutput computePos(const Observations & _observations, ComputePosOutput computePos(const Observations & _observations,
Navigation & _navigation, Navigation & _navigation,
const prcopt_t & _prcopt); const prcopt_t & _prcopt);
// ComputePosOutput computePosOwn(const Observations & _observations, ComputePosOutput computePosOwn(const Observations & _observations,
// Navigation & _navigation, Navigation & _navigation,
// const prcopt_t & _prcopt); const prcopt_t & _prcopt);
//
// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav, int pntposOwn(const obsd_t *obs, int n, const nav_t *nav,
// const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
// char *msg); 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); Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef);
} }
......
include/gnss_utils/single_differences.h 0 → 100644
+ 54
− 0
View file @ 57dcf28b
/*
* 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_ */
src/examples/CMakeLists.txt
+ 1
− 1
View file @ 57dcf28b
...@@ -5,4 +5,4 @@ INCLUDE_DIRECTORIES(../../include) ...@@ -5,4 +5,4 @@ INCLUDE_DIRECTORIES(../../include)
ADD_EXECUTABLE(gnss_utils_test gnss_utils_test.cpp) ADD_EXECUTABLE(gnss_utils_test gnss_utils_test.cpp)
# link necessary libraries # link necessary libraries
TARGET_LINK_LIBRARIES(gnss_utils_test ${gnss_utils_LIBRARIES}) TARGET_LINK_LIBRARIES(gnss_utils_test gnss_utils)
src/gnss_utils.cpp
+ 193
− 125
View file @ 57dcf28b
...@@ -53,131 +53,199 @@ namespace GNSSUtils ...@@ -53,131 +53,199 @@ namespace GNSSUtils
return output; return output;
} }
// ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations, ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations,
// GNSSUtils::Navigation & _navigation, GNSSUtils::Navigation & _navigation,
// const prcopt_t & _prcopt) const prcopt_t & _prcopt)
// { {
//
// // Remove duplicated satellites // Remove duplicated satellites
// uniqnav(&(_navigation.getNavigation())); uniqnav(&(_navigation.getNavigation()));
//
// // Define error msg // Define error msg
// char msg[128] = ""; char msg[128] = "";
//
// GNSSUtils::ComputePosOutput output; GNSSUtils::ComputePosOutput output;
// sol_t sol; sol_t sol;
// sol = {{0}}; sol = {{0}};
//
// output.pos_stat = pntposOwn(_observations.getObservations().data(), _observations.getObservations().size(), output.pos_stat = pntposOwn(_observations.getObservations().data(), _observations.getObservations().size(),
// &(_navigation.getNavigation()), &(_navigation.getNavigation()),
// &_prcopt, &sol, NULL, NULL, msg); &_prcopt, &sol, NULL, NULL, msg);
//
// if (output.pos_stat == 0) if (output.pos_stat == 0)
// { {
// std::cout << "Bad computing: " << msg << "\n"; std::cout << "Bad computing: " << msg << "\n";
// } }
//
// output.time = sol.time.time; output.time = sol.time.time;
// output.time = sol.time.sec; output.time = sol.time.sec;
// output.pos = Eigen::Vector3d(sol.rr); output.pos = Eigen::Vector3d(sol.rr);
// // std::cout << "Compute pos: " << output.pos.transpose() << "\n"; // std::cout << "Compute pos: " << output.pos.transpose() << "\n";
// output.vel = Eigen::Vector3d(&sol.rr[3]); output.vel = Eigen::Vector3d(&sol.rr[3]);
// output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5], output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5],
// sol.qr[3], sol.qr[1], sol.qr[4], sol.qr[3], sol.qr[1], sol.qr[4],
// sol.qr[5], sol.qr[3], sol.qr[2]; sol.qr[5], sol.qr[3], sol.qr[2];
//
// // XXX: segmentation fault here. // XXX: segmentation fault here.
// // if (sol.dtr != NULL) // 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.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.type = sol.type;
// output.stat = sol.stat; output.stat = sol.stat;
// output.ns = sol.ns; output.ns = sol.ns;
// output.age = sol.age; output.age = sol.age;
// output.ratio = sol.ratio; output.ratio = sol.ratio;
// output.lat_lon = ecefToLatLon(output.pos); output.lat_lon = ecefToLatLon(output.pos);
//
// return output; return output;
// } }
//
// /* single-point positioning COPIED FROM RTKLIB ------------------------------ /* single-point positioning COPIED FROM RTKLIB ------------------------------
// * compute receiver position, velocity, clock bias by single-point positioning * compute receiver position, velocity, clock bias by single-point positioning
// * with pseudorange and doppler observables * with pseudorange and doppler observables
// * args : obsd_t *obs I observation data * args : obsd_t *obs I observation data
// * int n I number of observation data * int n I number of observation data
// * nav_t *nav I navigation data * nav_t *nav I navigation data
// * prcopt_t *opt I processing options * prcopt_t *opt I processing options
// * sol_t *sol IO solution * sol_t *sol IO solution
// * double *azel IO azimuth/elevation angle (rad) (NULL: no output) * double *azel IO azimuth/elevation angle (rad) (NULL: no output)
// * ssat_t *ssat IO satellite status (NULL: no output) * ssat_t *ssat IO satellite status (NULL: no output)
// * char *msg O error message for error exit * char *msg O error message for error exit
// * return : status(1:ok,0:error) * return : status(1:ok,0:error)
// * notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and * notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and
// * receiver bias are negligible (only involving glonass-gps time offset * receiver bias are negligible (only involving glonass-gps time offset
// * and receiver bias) * and receiver bias)
// *-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav, int pntposOwn(const obsd_t *obs, int n, const nav_t *nav,
// const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
// char *msg) char *msg)
// { {
// prcopt_t opt_=*opt; prcopt_t opt_=*opt;
// double *rs,*dts,*var,*azel_,*resp; double *rs,*dts,*var,*azel_,*resp;
// int i,stat,vsat[MAXOBS]={0},svh[MAXOBS]; int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
//
// sol->stat=SOLQ_NONE; sol->stat=SOLQ_NONE;
//
// if (n<=0) {strcpy(msg,"no observation data"); return 0;} 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); trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n);
//
// sol->time=obs[0].time; msg[0]='\0'; 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); 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 (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
// #if 0 #if 0
// opt_.sateph =EPHOPT_BRDC; opt_.sateph =EPHOPT_BRDC;
// #endif #endif
// opt_.ionoopt=IONOOPT_BRDC; opt_.ionoopt=IONOOPT_BRDC;
// opt_.tropopt=TROPOPT_SAAS; opt_.tropopt=TROPOPT_SAAS;
// } }
// /* satellite positons, velocities and clocks */ /* satellite positons, velocities and clocks */
// satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh); satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);
//
// /* estimate receiver position with pseudorange */ /* estimate receiver position with pseudorange */
// stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); stat=estposOwn(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
//
// /* raim fde */ /* raim fde */
// if (!stat&&n>=6&&opt->posopt[4]) { if (!stat&&n>=6&&opt->posopt[4]) {
// stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
// } }
// /* estimate receiver velocity with doppler */ /* estimate receiver velocity with doppler */
// //if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat); //if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
//
// if (azel) { if (azel) {
// for (i=0;i<n*2;i++) azel[i]=azel_[i]; for (i=0;i<n*2;i++) azel[i]=azel_[i];
// } }
// if (ssat) { if (ssat) {
// for (i=0;i<MAXSAT;i++) { for (i=0;i<MAXSAT;i++) {
// ssat[i].vs=0; ssat[i].vs=0;
// ssat[i].azel[0]=ssat[i].azel[1]=0.0; ssat[i].azel[0]=ssat[i].azel[1]=0.0;
// ssat[i].resp[0]=ssat[i].resc[0]=0.0; ssat[i].resp[0]=ssat[i].resc[0]=0.0;
// ssat[i].snr[0]=0; ssat[i].snr[0]=0;
// } }
// for (i=0;i<n;i++) { for (i=0;i<n;i++) {
// ssat[obs[i].sat-1].azel[0]=azel_[ i*2]; 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].azel[1]=azel_[1+i*2];
// ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0]; ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
// if (!vsat[i]) continue; if (!vsat[i]) continue;
// ssat[obs[i].sat-1].vs=1; ssat[obs[i].sat-1].vs=1;
// ssat[obs[i].sat-1].resp[0]=resp[i]; ssat[obs[i].sat-1].resp[0]=resp[i];
// } }
// } }
// free(rs); free(dts); free(var); free(azel_); free(resp); free(rs); free(dts); free(var); free(azel_); free(resp);
// return stat; 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) Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef)
{ {
......
src/single_differences.cpp 0 → 100644
+ 61
− 0
View file @ 57dcf28b
/*
* 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;
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment