Skip to content
Snippets Groups Projects
Commit c5a6463e authored by Pep Martí Saumell's avatar Pep Martí Saumell
Browse files

[utils] added more util files

parent bcd073e8
No related branches found
No related tags found
3 merge requests!20new tag,!19new tag,!8Resolve "File restructuration"
Hide whitespace changes
Inline Side-by-side
Showing
with 517 additions and 432 deletions
CMakeLists.txt
+ 4
− 0
View file @ c5a6463e
......@@ -52,6 +52,8 @@ SET(SOURCES
src/utils/gnss_utils.cpp
src/utils/utils.cpp
src/utils/transformations.cpp
src/utils/rcv_position.cpp
src/utils/sat_position.cpp
src/observations.cpp
src/navigation.cpp
src/TDCP.cpp
......@@ -92,6 +94,8 @@ SET(HEADERS
include/gnss_utils/utils/gnss_utils.h
include/gnss_utils/utils/utils.h
include/gnss_utils/utils/transformations.h
include/gnss_utils/utils/rcv_position.h
include/gnss_utils/utils/sat_position.h
include/gnss_utils/observations.h
include/gnss_utils/navigation.h
include/gnss_utils/TDCP.h
......
include/gnss_utils/TDCP.h
+ 2
− 1
View file @ c5a6463e
......@@ -11,7 +11,8 @@
#define GNSS_UTILS_TDCP_DEBUG 0
#include <set>
#include "gnss_utils/utils/gnss_utils.h"
#include "gnss_utils/utils/rcv_position.h"
#include "gnss_utils/utils/sat_position.h"
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
......
include/gnss_utils/utils/gnss_utils.h
+ 1
− 67
View file @ c5a6463e
......@@ -13,6 +13,7 @@
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
#include "gnss_utils/utils/transformations.h"
#include "gnss_utils/utils/rcv_position.h"
extern "C" {
#include "rtklib.h"
......@@ -20,73 +21,6 @@ extern "C" {
namespace GNSSUtils
{
struct ComputePosOutput
{
time_t time;
double sec;
Eigen::Vector3d pos; // position (m)
Eigen::Vector3d vel; // velocity (m/s)
Eigen::Matrix3d pos_covar; // position covariance (m^2)
// {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx}
Eigen::VectorXd rcv_bias; // receiver clock bias to time systems (s)
int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
int stat; // solution status (SOLQ_???)
int ns; // number of valid satellites
double age; // age of differential (s)
double ratio; // AR ratio factor for valiation
int pos_stat; // return from pntpos
Eigen::Vector3d lat_lon; // latitude_longitude_altitude
};
ComputePosOutput computePos(const Observations& _observations, 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);
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 computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
void computeSatellitesPositions(const Observations& obs,
const Navigation& nav,
const prcopt_t& opt,
std::map<int, Eigen::Vector3d>& sats_pos);
template <typename T>
bool equalArray(const T* array1, const T* array2, const int& size1, const int& size2)
{
if (size1 != size2)
return false;
for (int ii = 0; ii < size1; ++ii)
{
if (array1[ii] != array2[ii])
return false;
}
return true;
}
bool equalTime(const gtime_t& time1, const gtime_t& time2);
bool equalObservations(const obsd_t& obs1, const obsd_t& obs2);
bool equalObservations(const obs_t& obs1, const obs_t& obs2);
} // namespace GNSSUtils
......
include/gnss_utils/utils/rcv_position.h 0 → 100644
+ 68
− 0
View file @ c5a6463e
/*
* transfromation.h
*
* Created on: April 3, 2020
* Author: Joan Vallvé, Pep Martí-Saumell
*/
#ifndef INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
#define INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Sparse>
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
#include "gnss_utils/utils/transformations.h"
extern "C" {
#include "rtklib.h"
}
namespace GNSSUtils
{
struct ComputePosOutput
{
time_t time;
double sec;
Eigen::Vector3d pos; // position (m)
Eigen::Vector3d vel; // velocity (m/s)
Eigen::Matrix3d pos_covar; // position covariance (m^2)
// {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx}
Eigen::VectorXd rcv_bias; // receiver clock bias to time systems (s)
int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
int stat; // solution status (SOLQ_???)
int ns; // number of valid satellites
double age; // age of differential (s)
double ratio; // AR ratio factor for valiation
int pos_stat; // return from pntpos
Eigen::Vector3d lat_lon; // latitude_longitude_altitude
};
ComputePosOutput computePos(const Observations& _observations, 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);
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);
} // namespace GNSSUtils
#endif // INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
\ No newline at end of file
include/gnss_utils/utils/sat_position.h 0 → 100644
+ 32
− 0
View file @ c5a6463e
/*
* sat_position.h
*
* Created on: April 3, 2020
* Author: Joan Vallvé, Pep Martí-Saumell
*/
#ifndef INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
#define INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Sparse>
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
#include "gnss_utils/utils/transformations.h"
extern "C" {
#include "rtklib.h"
}
namespace GNSSUtils
{
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
void computeSatellitesPositions(const Observations& obs,
const Navigation& nav,
const prcopt_t& opt,
std::map<int, Eigen::Vector3d>& sats_pos);
} // namespace GNSSUtils
#endif // INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
\ No newline at end of file
include/gnss_utils/utils/transformations.h
+ 2
− 7
View file @ c5a6463e
/*
* TDCP.h
* transfromation.h
*
* Created on: April 3, 2020
* Author: Pep Martí-Saumell
* Author: Joan Vallvé, Pep Martí-Saumell
*/
#ifndef INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
#define INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
// #include <vector>
// #include <iostream>
// #include <memory>
// #include <string>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Sparse>
......
include/gnss_utils/utils/utils.h
+ 24
− 0
View file @ c5a6463e
......@@ -9,6 +9,10 @@
#define ARRAY_SIZE(arr) sizeof(arr) / sizeof(arr[0])
#define GNSSUTILS_MSG "--GNSSUtils--"
extern "C" {
#include "rtklib.h"
}
namespace GNSSUtils
{
void print(std::string& _msg);
......@@ -16,6 +20,26 @@ void printArray(std::string _name, int* _array, int size);
void printArray(std::string _name, unsigned char* _array, int size);
void printArray(std::string _name, double* _array, int size);
void printArray(std::string _name, float* _array, int size);
template <typename T>
bool equalArray(const T* array1, const T* array2, const int& size1, const int& size2)
{
if (size1 != size2)
return false;
for (int ii = 0; ii < size1; ++ii)
{
if (array1[ii] != array2[ii])
return false;
}
return true;
}
bool equalTime(const gtime_t& time1, const gtime_t& time2);
bool equalObservations(const obsd_t& obs1, const obsd_t& obs2);
bool equalObservations(const obs_t& obs1, const obs_t& obs2);
} // namespace GNSSUtils
#endif
\ No newline at end of file
src/utils/gnss_utils.cpp
+ 0
− 356
View file @ c5a6463e
......@@ -2,362 +2,6 @@
namespace GNSSUtils
{
ComputePosOutput computePos(const GNSSUtils::Observations& _observations,
GNSSUtils::Navigation& _navigation,
const prcopt_t& _prcopt)
{
// Remove duplicated satellites
_navigation.uniqueNavigation();
// Define error msg
char msg[128] = "";
// Remove duplicated satellites
_navigation.uniqueNavigation();
GNSSUtils::ComputePosOutput output;
sol_t sol;
sol = { { 0 } };
output.pos_stat = pntpos(
_observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg);
if (output.pos_stat == 0)
{
std::cout << "computePos: error in computing positioning, message: " << msg << "\n";
}
output.time = sol.time.time;
output.sec = sol.time.sec;
output.pos = Eigen::Vector3d(sol.rr);
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[4], sol.qr[2];
// std::cout << "Compute pos: " << output.pos.transpose() << "\n";
// std::cout << "Covariance:\n" << output.pos_covar << "\n";
// 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 = ecefToLatLonAlt(output.pos);
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.data(), _observations.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.sec = 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 = ecefToLatLonAlt(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;
}
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
{
// ecef 2 geodetic
Eigen::Vector3d receiver_geo;
ecef2pos(receiver_ecef.data(), receiver_geo.data());
// receiver-sat vector ecef
Eigen::Vector3d receiver_sat_ecef = sat_ecef - receiver_ecef;
// receiver-sat vector enu (receiver ecef as origin)
Eigen::Vector3d receiver_sat_enu;
ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad)
receiver_sat_ecef.data(), // vector in ecef coordinate {x,y,z}
receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
// elevation
return atan2(receiver_sat_enu(2),
sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
}
void computeSatellitesPositions(const Observations& obs,
const Navigation& nav,
const prcopt_t& opt,
std::map<int, Eigen::Vector3d>& sats_pos)
{
double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()];
int svh[obs.size()];
// std::cout << "computing sats position from sats: ";
// for (auto&& obs_ref : obs.getObservations())
// std::cout << (int)obs_ref.sat << " ";
// std::cout << std::endl;
// compute positions
satposs(
obs.getObservations().front().time, obs.data(), obs.size(), &nav.getNavigation(), opt.sateph, rs, dts, var, svh);
// store positions
// std::cout << "filling sats positions: \n";
for (int i = 0; i < obs.size(); i++)
{
if (svh[i] < 0) // ephemeris unavailable
sats_pos[obs.getObservationByIdx(i).sat] = Eigen::Vector3d::Zero();
else
sats_pos[obs.getObservationByIdx(i).sat] << rs[6 * i], rs[6 * i + 1], rs[6 * i + 2];
// std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << ": " <<
// sats_pos[obs.getObservationByIdx(i).sat].transpose() << std::endl;
}
}
bool equalTime(const gtime_t& time1, const gtime_t& time2)
{
return (difftime(time1.time, time2.time) == 0.0 && time1.sec == time2.sec);
}
bool equalObservations(const obsd_t& obs1, const obsd_t& obs2)
{
if (!equalTime(obs1.time, obs2.time))
return false;
if (!equalTime(obs1.eventime, obs2.eventime))
return false;
if (obs1.timevalid != obs2.timevalid)
return false;
if (obs1.sat != obs2.sat)
return false;
if (obs1.rcv != obs2.rcv)
return false;
if (memcmp(obs1.SNR, obs2.SNR, sizeof(obs1.SNR)) != 0)
return false;
if (memcmp(obs1.LLI, obs2.LLI, sizeof(obs1.LLI)) != 0)
return false;
if (memcmp(obs1.code, obs2.code, sizeof(obs1.code)) != 0)
return false;
if (memcmp(obs1.qualL, obs2.qualL, sizeof(obs1.qualL)) != 0)
return false;
if (memcmp(obs1.qualP, obs2.qualP, sizeof(obs1.qualP)) != 0)
return false;
if (obs1.freq != obs2.freq)
return false;
if (!equalArray<double>(obs1.L, obs2.L, sizeof(obs1.L) / sizeof(obs1.L[0]), sizeof(obs2.L) / sizeof(obs2.L[0])))
return false;
if (!equalArray<double>(obs1.P, obs2.P, sizeof(obs1.P) / sizeof(obs1.P[0]), sizeof(obs2.P) / sizeof(obs2.P[0])))
return false;
if (!equalArray<float>(obs1.D, obs2.D, sizeof(obs1.D) / sizeof(obs1.D[0]), sizeof(obs2.D) / sizeof(obs2.D[0])))
return false;
return true;
}
bool equalObservations(const obs_t& obs1, const obs_t& obs2)
{
if (obs1.n != obs2.n)
return false;
if (obs1.nmax != obs2.nmax)
return false;
if (obs1.flag != obs2.flag)
return false;
if (obs1.rcvcount != obs2.rcvcount)
return false;
if (obs1.tmcount != obs2.tmcount)
return false;
if (!equalObservations(*(obs1.data), *(obs2.data)))
return false;
return true;
}
} // namespace GNSSUtils
src/utils/rcv_position.cpp 0 → 100644
+ 264
− 0
View file @ c5a6463e
/*
* transfromation.h
*
* Created on: April 3, 2020
* Author: Joan Vallvé, Pep Martí-Saumell
*/
#include "gnss_utils/utils/rcv_position.h"
using namespace GNSSUtils;
namespace GNSSUtils
{
ComputePosOutput computePos(const GNSSUtils::Observations& _observations,
GNSSUtils::Navigation& _navigation,
const prcopt_t& _prcopt)
{
// Remove duplicated satellites
_navigation.uniqueNavigation();
// Define error msg
char msg[128] = "";
// Remove duplicated satellites
_navigation.uniqueNavigation();
GNSSUtils::ComputePosOutput output;
sol_t sol;
sol = { { 0 } };
output.pos_stat = pntpos(
_observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg);
if (output.pos_stat == 0)
{
std::cout << "computePos: error in computing positioning, message: " << msg << "\n";
}
output.time = sol.time.time;
output.sec = sol.time.sec;
output.pos = Eigen::Vector3d(sol.rr);
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[4], sol.qr[2];
// std::cout << "Compute pos: " << output.pos.transpose() << "\n";
// std::cout << "Covariance:\n" << output.pos_covar << "\n";
// 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 = ecefToLatLonAlt(output.pos);
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.data(), _observations.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.sec = 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 = ecefToLatLonAlt(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;
}
} // namespace GNSSUtils
\ No newline at end of file
src/utils/sat_position.cpp 0 → 100644
+ 63
− 0
View file @ c5a6463e
/*
* sat_position.cpp
*
* Created on: April 3, 2020
* Author: Joan Vallvé, Pep Martí-Saumell
*/
#include "gnss_utils/utils/sat_position.h"
using namespace GNSSUtils;
namespace GNSSUtils
{
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
{
// ecef 2 geodetic
Eigen::Vector3d receiver_geo;
ecef2pos(receiver_ecef.data(), receiver_geo.data());
// receiver-sat vector ecef
Eigen::Vector3d receiver_sat_ecef = sat_ecef - receiver_ecef;
// receiver-sat vector enu (receiver ecef as origin)
Eigen::Vector3d receiver_sat_enu;
ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad)
receiver_sat_ecef.data(), // vector in ecef coordinate {x,y,z}
receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
// elevation
return atan2(receiver_sat_enu(2),
sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
}
void computeSatellitesPositions(const Observations& obs,
const Navigation& nav,
const prcopt_t& opt,
std::map<int, Eigen::Vector3d>& sats_pos)
{
double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()];
int svh[obs.size()];
// std::cout << "computing sats position from sats: ";
// for (auto&& obs_ref : obs.getObservations())
// std::cout << (int)obs_ref.sat << " ";
// std::cout << std::endl;
// compute positions
satposs(
obs.getObservations().front().time, obs.data(), obs.size(), &nav.getNavigation(), opt.sateph, rs, dts, var, svh);
// store positions
// std::cout << "filling sats positions: \n";
for (int i = 0; i < obs.size(); i++)
{
if (svh[i] < 0) // ephemeris unavailable
sats_pos[obs.getObservationByIdx(i).sat] = Eigen::Vector3d::Zero();
else
sats_pos[obs.getObservationByIdx(i).sat] << rs[6 * i], rs[6 * i + 1], rs[6 * i + 2];
// std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << ": " <<
// sats_pos[obs.getObservationByIdx(i).sat].transpose() << std::endl;
}
}
} // namespace GNSSUtils
\ No newline at end of file
src/utils/utils.cpp
+ 56
− 0
View file @ c5a6463e
......@@ -61,4 +61,60 @@ void printArray(std::string _name, float* _array, int size)
}
}
bool equalTime(const gtime_t& time1, const gtime_t& time2)
{
return (difftime(time1.time, time2.time) == 0.0 && time1.sec == time2.sec);
}
bool equalObservations(const obsd_t& obs1, const obsd_t& obs2)
{
if (!equalTime(obs1.time, obs2.time))
return false;
if (!equalTime(obs1.eventime, obs2.eventime))
return false;
if (obs1.timevalid != obs2.timevalid)
return false;
if (obs1.sat != obs2.sat)
return false;
if (obs1.rcv != obs2.rcv)
return false;
if (memcmp(obs1.SNR, obs2.SNR, sizeof(obs1.SNR)) != 0)
return false;
if (memcmp(obs1.LLI, obs2.LLI, sizeof(obs1.LLI)) != 0)
return false;
if (memcmp(obs1.code, obs2.code, sizeof(obs1.code)) != 0)
return false;
if (memcmp(obs1.qualL, obs2.qualL, sizeof(obs1.qualL)) != 0)
return false;
if (memcmp(obs1.qualP, obs2.qualP, sizeof(obs1.qualP)) != 0)
return false;
if (obs1.freq != obs2.freq)
return false;
if (!equalArray<double>(obs1.L, obs2.L, sizeof(obs1.L) / sizeof(obs1.L[0]), sizeof(obs2.L) / sizeof(obs2.L[0])))
return false;
if (!equalArray<double>(obs1.P, obs2.P, sizeof(obs1.P) / sizeof(obs1.P[0]), sizeof(obs2.P) / sizeof(obs2.P[0])))
return false;
if (!equalArray<float>(obs1.D, obs2.D, sizeof(obs1.D) / sizeof(obs1.D[0]), sizeof(obs2.D) / sizeof(obs2.D[0])))
return false;
return true;
}
bool equalObservations(const obs_t& obs1, const obs_t& obs2)
{
if (obs1.n != obs2.n)
return false;
if (obs1.nmax != obs2.nmax)
return false;
if (obs1.flag != obs2.flag)
return false;
if (obs1.rcvcount != obs2.rcvcount)
return false;
if (obs1.tmcount != obs2.tmcount)
return false;
if (!equalObservations(*(obs1.data), *(obs2.data)))
return false;
return true;
}
} // namespace GNSSUtils
test/gtest_transformations.cpp
+ 1
− 1
View file @ c5a6463e
#include "gtest/utils_gtest.h"
#include "gnss_utils/utils/gnss_utils.h"
#include "gnss_utils/utils/sat_position.h"
// Geodetic system parameters
static double kSemimajorAxis = 6378137;
......
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