diff --git a/CMakeLists.txt b/CMakeLists.txt
index 33c532478665f879b3658b0f173f2745d25bc8cc..83308dba04ebf89da737631e192b2bfd2417a671 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,11 +49,13 @@ SET(RTKLIB_SRC_DIR ${RTKLIB_DIR}/src)
# driver source files
SET(SOURCES
- src/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
- src/utils.cpp
src/ublox_raw.cpp)
SET(RTKLIB_SRC
@@ -88,8 +90,10 @@ SET(RTKLIB_SRC
# application header files
SET(HEADERS
- include/gnss_utils/gnss_utils.h
- include/gnss_utils/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
diff --git a/include/gnss_utils/TDCP.h b/include/gnss_utils/TDCP.h
index 439a981ad097ea568d1cda9321b31c9e1b86fa55..00f70f065ee7d76372e8f72848051bcca6e8a994 100644
--- a/include/gnss_utils/TDCP.h
+++ b/include/gnss_utils/TDCP.h
@@ -11,7 +11,8 @@
#define GNSS_UTILS_TDCP_DEBUG 0
#include <set>
-#include "gnss_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"
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index 5610a7ec08698c19a6cf84152da0773b83f55cb2..7fb34baa11663cccc738b0a127c63a3b9058203e 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -1,5 +1,5 @@
-#ifndef OBSERVATIONS_H
-#define OBSERVATIONS_H
+#ifndef INCLUDE_GNSS_UTILS_OBSERVATIONS_H_
+#define INCLUDE_GNSS_UTILS_OBSERVATIONS_H_
#include <vector>
#include <map>
@@ -7,7 +7,7 @@
#include <memory>
#include <cassert>
-#include "gnss_utils/utils.h"
+#include "gnss_utils/utils/utils.h"
extern "C" {
#include "rtklib.h"
@@ -132,4 +132,4 @@ inline bool Observations::hasSatellite(const unsigned char& i) const
}
} // namespace GNSSUtils
-#endif
+#endif // INCLUDE_GNSS_UTILS_OBSERVATIONS_H_
diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h
index c3e4cc15180559331f3b0732f4243eb684076b63..1c36ba27ab5b7eb107ee976c4c0061c4c04fae16 100644
--- a/include/gnss_utils/ublox_raw.h
+++ b/include/gnss_utils/ublox_raw.h
@@ -1,7 +1,8 @@
#ifndef UBLOX_RAW_H
#define UBLOX_RAW_H
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
namespace GNSSUtils
{
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/utils/rcv_position.h
similarity index 54%
rename from include/gnss_utils/gnss_utils.h
rename to include/gnss_utils/utils/rcv_position.h
index a16976acdec9a321d0b47fa1f4c647fab896940b..55fbd80212233df121e339b21769ccb245597a95 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/utils/rcv_position.h
@@ -1,10 +1,12 @@
-#ifndef GNSS_UTILS_H
-#define GNSS_UTILS_H
+/*
+ * transfromation.h
+ *
+ * Created on: April 3, 2020
+ * Author: Joan Vallvé, Pep MartÃ-Saumell
+ */
-#include <vector>
-#include <iostream>
-#include <memory>
-#include <string>
+#ifndef INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
+#define INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
@@ -12,6 +14,7 @@
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/transformations.h"
extern "C" {
#include "rtklib.h"
@@ -61,44 +64,5 @@ int estposOwn(const obsd_t* obs,
int* vsat,
double* resp,
char* msg);
-
-Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef);
-Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon);
-Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef);
-Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu);
-
-void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
- Eigen::Matrix3d& R_ENU_ECEF,
- Eigen::Vector3d& t_ENU_ECEF);
-void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
- Eigen::Matrix3d& R_ENU_ECEF,
- Eigen::Vector3d& t_ENU_ECEF);
-
-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
-
-#endif
+#endif // INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
\ No newline at end of file
diff --git a/include/gnss_utils/utils/sat_position.h b/include/gnss_utils/utils/sat_position.h
new file mode 100644
index 0000000000000000000000000000000000000000..2edb56c3a751b373a1ed26ddac76fc96250e2648
--- /dev/null
+++ b/include/gnss_utils/utils/sat_position.h
@@ -0,0 +1,32 @@
+/*
+ * 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
diff --git a/include/gnss_utils/utils/transformations.h b/include/gnss_utils/utils/transformations.h
new file mode 100644
index 0000000000000000000000000000000000000000..494d5ece16e83e8c870655e947588f4ceb782ec3
--- /dev/null
+++ b/include/gnss_utils/utils/transformations.h
@@ -0,0 +1,34 @@
+/*
+ * transfromation.h
+ *
+ * Created on: April 3, 2020
+ * Author: Joan Vallvé, Pep MartÃ-Saumell
+ */
+
+#ifndef INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
+#define INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
+
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+#include <eigen3/Eigen/Sparse>
+
+extern "C" {
+#include "rtklib.h"
+}
+
+namespace GNSSUtils
+{
+Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef);
+Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon);
+Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef);
+Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu);
+
+void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF);
+void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF);
+
+} // namespace GNSSUtils
+#endif // INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
\ No newline at end of file
diff --git a/include/gnss_utils/utils.h b/include/gnss_utils/utils/utils.h
similarity index 52%
rename from include/gnss_utils/utils.h
rename to include/gnss_utils/utils/utils.h
index fa24b0896b8d0f4158143aae1c6a6698e66fe8d3..faa390ae290e2281cdab9a37f60633414dae048a 100644
--- a/include/gnss_utils/utils.h
+++ b/include/gnss_utils/utils/utils.h
@@ -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
diff --git a/src/examples/gnss_utils_test.cpp b/src/examples/gnss_utils_test.cpp
index 814ee443c905e3a1fd8d327bcad4d096227a02f6..10520e09738af7396ba748bfea0b75bc6e6771be 100644
--- a/src/examples/gnss_utils_test.cpp
+++ b/src/examples/gnss_utils_test.cpp
@@ -1,6 +1,6 @@
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/transformations.h"
#include <typeinfo>
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
deleted file mode 100644
index 70bad327efe878de41efd6fd2c21d5f7f4e51528..0000000000000000000000000000000000000000
--- a/src/gnss_utils.cpp
+++ /dev/null
@@ -1,510 +0,0 @@
-#include "gnss_utils/gnss_utils.h"
-
-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;
-}
-
-Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef)
-{
- Eigen::Vector3d pos;
- ecef2pos(_ecef.data(), pos.data());
-
- return pos;
-}
-
-Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _pos)
-{
- Eigen::Vector3d ecef;
- pos2ecef(_pos.data(), ecef.data());
-
- return ecef;
-}
-
-Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef)
-{
- Eigen::Matrix3d cov_enu;
-
- /* RTKLIB transform covariance to local tangental coordinate --------------------------
- * transform ecef covariance to local tangental coordinate
- * args : double *pos I geodetic position {lat,lon} (rad)
- * double *P I covariance in ecef coordinate
- * double *Q O covariance in local tangental coordinate
- * return : none
- *-----------------------------------------------------------------------------*/
- // extern void covenu(const double *pos, const double *P, double *Q);
- covenu(_latlon.data(), _cov_ecef.data(), cov_enu.data());
-
- return cov_enu;
-}
-
-Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu)
-{
- Eigen::Matrix3d cov_ecef;
-
- /* RTKLIB transform local enu coordinate covariance to xyz-ecef -----------------------
- * transform local enu covariance to xyz-ecef coordinate
- * args : double *pos I geodetic position {lat,lon} (rad)
- * double *Q I covariance in local enu coordinate
- * double *P O covariance in xyz-ecef coordinate
- * return : none
- *-----------------------------------------------------------------------------*/
- // extern void covecef(const double *pos, const double *Q, double *P)
- covecef(_latlon.data(), _cov_enu.data(), cov_ecef.data());
-
- return cov_ecef;
-}
-
-void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
- Eigen::Matrix3d& R_ENU_ECEF,
- Eigen::Vector3d& t_ENU_ECEF)
-{
- // Convert ECEF coordinates to geodetic coordinates.
- // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
- // to geodetic coordinates," IEEE Transactions on Aerospace and
- // Electronic Systems, vol. 30, pp. 957-961, 1994.
-
- // double r = std::sqrt(_t_ECEF_ENU(0) * _t_ECEF_ENU(0) + _t_ECEF_ENU(1) * _t_ECEF_ENU(1));
- // double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
- // double F = 54 * kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) * _t_ECEF_ENU(2);
- // double G = r * r + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) - kFirstEccentricitySquared
- // * Esq; double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3); double S =
- // cbrt(1 + C + sqrt(C * C + 2 * C)); double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G); double Q = sqrt(1 + 2 *
- // kFirstEccentricitySquared * kFirstEccentricitySquared * P); double r_0 = -(P * kFirstEccentricitySquared * r) /
- // (1 + Q)
- // + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
- // - P * (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P *
- // r * r);
- // double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2)
- // * _t_ECEF_ENU(2)); double Z_0 = kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) / (kSemimajorAxis * V);
- //
- // double latitude = atan((_t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
- // double longitude = atan2(_t_ECEF_ENU(1), _t_ECEF_ENU(0));
- //
- //
- // double sLat = sin(latitude);
- // double cLat = cos(latitude);
- // double sLon = sin(longitude);
- // double cLon = cos(longitude);
- //
- // R_ENU_ECEF(0,0) = -sLon;
- // R_ENU_ECEF(0,1) = cLon;
- // R_ENU_ECEF(0,2) = 0.0;
- //
- // R_ENU_ECEF(1,0) = -sLat*cLon;
- // R_ENU_ECEF(1,1) = -sLat * sLon;
- // R_ENU_ECEF(1,2) = cLat;
- //
- // R_ENU_ECEF(2,0) = cLat * cLon;
- // R_ENU_ECEF(2,1) = cLat * sLon;
- // R_ENU_ECEF(2,2) = sLat;
- //
- // t_ENU_ECEF = -R_ENU_ECEF*_t_ECEF_ENU;
-
- Eigen::Vector3d ENU_lat_lon_alt = GNSSUtils::ecefToLatLonAlt(_t_ECEF_ENU);
-
- double sLat = sin(ENU_lat_lon_alt(0));
- double cLat = cos(ENU_lat_lon_alt(0));
- double sLon = sin(ENU_lat_lon_alt(1));
- double cLon = cos(ENU_lat_lon_alt(1));
-
- R_ENU_ECEF(0, 0) = -sLon;
- R_ENU_ECEF(0, 1) = cLon;
- R_ENU_ECEF(0, 2) = 0.0;
-
- R_ENU_ECEF(1, 0) = -sLat * cLon;
- R_ENU_ECEF(1, 1) = -sLat * sLon;
- R_ENU_ECEF(1, 2) = cLat;
-
- R_ENU_ECEF(2, 0) = cLat * cLon;
- R_ENU_ECEF(2, 1) = cLat * sLon;
- R_ENU_ECEF(2, 2) = sLat;
-
- t_ENU_ECEF = -R_ENU_ECEF * _t_ECEF_ENU;
-}
-
-void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
- Eigen::Matrix3d& R_ENU_ECEF,
- Eigen::Vector3d& t_ENU_ECEF)
-{
- double sLat = sin(_ENU_latlonalt(0));
- double cLat = cos(_ENU_latlonalt(0));
- double sLon = sin(_ENU_latlonalt(1));
- double cLon = cos(_ENU_latlonalt(1));
-
- R_ENU_ECEF(0, 0) = -sLon;
- R_ENU_ECEF(0, 1) = cLon;
- R_ENU_ECEF(0, 2) = 0.0;
-
- R_ENU_ECEF(1, 0) = -sLat * cLon;
- R_ENU_ECEF(1, 1) = -sLat * sLon;
- R_ENU_ECEF(1, 2) = cLat;
-
- R_ENU_ECEF(2, 0) = cLat * cLon;
- R_ENU_ECEF(2, 1) = cLat * sLon;
- R_ENU_ECEF(2, 2) = sLat;
-
- Eigen::Vector3d t_ECEF_ENU = latLonAltToEcef(_ENU_latlonalt);
-
- t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU;
-}
-
-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
diff --git a/src/observations.cpp b/src/observations.cpp
index ea2dc15d658ce4644e8446ae71182ce21c428d4a..e68fb8d216b9294de2d22232fe92cfb5d4a70ec7 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -1,4 +1,3 @@
-#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/observations.h"
using namespace GNSSUtils;
diff --git a/src/utils.cpp b/src/utils.cpp
deleted file mode 100644
index cd697b39951aa113fb7a160f9b75722cbdf740f7..0000000000000000000000000000000000000000
--- a/src/utils.cpp
+++ /dev/null
@@ -1,64 +0,0 @@
-#include "gnss_utils/utils.h"
-
-namespace GNSSUtils
-{
-void print(std::string& _msg)
-{
- std::string msg = GNSSUTILS_MSG + _msg;
-
- std::cout << msg << "\n";
-}
-
-void printArray(std::string _name, int* _array, int size)
-{
- std::cout << _name << ": [";
- for (int ii = 0; ii < size; ++ii)
- {
- std::cout << _array[ii];
- if (ii == size - 1)
- std::cout << "] \n";
- else
- std::cout << ",";
- }
-}
-
-void printArray(std::string _name, unsigned char* _array, int size)
-{
- std::cout << _name << ": [";
- for (int ii = 0; ii < size; ++ii)
- {
- std::cout << (int)(_array[ii]);
- if (ii == size - 1)
- std::cout << "] \n";
- else
- std::cout << ",";
- }
-}
-
-void printArray(std::string _name, double* _array, int size)
-{
- std::cout << _name << ": [";
- for (int ii = 0; ii < size; ++ii)
- {
- std::cout << _array[ii];
- if (ii == size - 1)
- std::cout << "] \n";
- else
- std::cout << ",";
- }
-}
-
-void printArray(std::string _name, float* _array, int size)
-{
- std::cout << _name << ": [";
- for (int ii = 0; ii < size; ++ii)
- {
- std::cout << _array[ii];
- if (ii == size - 1)
- std::cout << "] \n";
- else
- std::cout << ",";
- }
-}
-
-} // namespace GNSSUtils
diff --git a/src/utils/rcv_position.cpp b/src/utils/rcv_position.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..32ced1767ae1bf991cc7f92b0f1c21966072d602
--- /dev/null
+++ b/src/utils/rcv_position.cpp
@@ -0,0 +1,264 @@
+/*
+ * 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
diff --git a/src/utils/sat_position.cpp b/src/utils/sat_position.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..287fcdfc1a6a27785a4e0a3955704314c4d802db
--- /dev/null
+++ b/src/utils/sat_position.cpp
@@ -0,0 +1,63 @@
+/*
+ * 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
diff --git a/src/utils/transformations.cpp b/src/utils/transformations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c12f2db6219acdb3ac40013abd636edaead189f8
--- /dev/null
+++ b/src/utils/transformations.cpp
@@ -0,0 +1,152 @@
+#include "gnss_utils/utils/transformations.h"
+
+using namespace GNSSUtils;
+
+namespace GNSSUtils
+{
+
+Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef)
+{
+ Eigen::Vector3d pos;
+ ecef2pos(_ecef.data(), pos.data());
+
+ return pos;
+}
+
+Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon)
+{
+ Eigen::Vector3d ecef;
+ pos2ecef(_latlon.data(), ecef.data());
+
+ return ecef;
+}
+
+Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef)
+{
+ Eigen::Matrix3d cov_enu;
+
+ /* RTKLIB transform covariance to local tangental coordinate --------------------------
+ * transform ecef covariance to local tangental coordinate
+ * args : double *pos I geodetic position {lat,lon} (rad)
+ * double *P I covariance in ecef coordinate
+ * double *Q O covariance in local tangental coordinate
+ * return : none
+ *-----------------------------------------------------------------------------*/
+ // extern void covenu(const double *pos, const double *P, double *Q);
+ covenu(_latlon.data(), _cov_ecef.data(), cov_enu.data());
+
+ return cov_enu;
+}
+
+Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu)
+{
+ Eigen::Matrix3d cov_ecef;
+
+ /* RTKLIB transform local enu coordinate covariance to xyz-ecef -----------------------
+ * transform local enu covariance to xyz-ecef coordinate
+ * args : double *pos I geodetic position {lat,lon} (rad)
+ * double *Q I covariance in local enu coordinate
+ * double *P O covariance in xyz-ecef coordinate
+ * return : none
+ *-----------------------------------------------------------------------------*/
+ // extern void covecef(const double *pos, const double *Q, double *P)
+ covecef(_latlon.data(), _cov_enu.data(), cov_ecef.data());
+
+ return cov_ecef;
+}
+
+void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF)
+{
+ // Convert ECEF coordinates to geodetic coordinates.
+ // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
+ // to geodetic coordinates," IEEE Transactions on Aerospace and
+ // Electronic Systems, vol. 30, pp. 957-961, 1994.
+
+ // double r = std::sqrt(_t_ECEF_ENU(0) * _t_ECEF_ENU(0) + _t_ECEF_ENU(1) * _t_ECEF_ENU(1));
+ // double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
+ // double F = 54 * kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) * _t_ECEF_ENU(2);
+ // double G = r * r + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) - kFirstEccentricitySquared
+ // * Esq; double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3); double S =
+ // cbrt(1 + C + sqrt(C * C + 2 * C)); double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G); double Q = sqrt(1 + 2 *
+ // kFirstEccentricitySquared * kFirstEccentricitySquared * P); double r_0 = -(P * kFirstEccentricitySquared * r) /
+ // (1 + Q)
+ // + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
+ // - P * (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P *
+ // r * r);
+ // double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2)
+ // * _t_ECEF_ENU(2)); double Z_0 = kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) / (kSemimajorAxis * V);
+ //
+ // double latitude = atan((_t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
+ // double longitude = atan2(_t_ECEF_ENU(1), _t_ECEF_ENU(0));
+ //
+ //
+ // double sLat = sin(latitude);
+ // double cLat = cos(latitude);
+ // double sLon = sin(longitude);
+ // double cLon = cos(longitude);
+ //
+ // R_ENU_ECEF(0,0) = -sLon;
+ // R_ENU_ECEF(0,1) = cLon;
+ // R_ENU_ECEF(0,2) = 0.0;
+ //
+ // R_ENU_ECEF(1,0) = -sLat*cLon;
+ // R_ENU_ECEF(1,1) = -sLat * sLon;
+ // R_ENU_ECEF(1,2) = cLat;
+ //
+ // R_ENU_ECEF(2,0) = cLat * cLon;
+ // R_ENU_ECEF(2,1) = cLat * sLon;
+ // R_ENU_ECEF(2,2) = sLat;
+ //
+ // t_ENU_ECEF = -R_ENU_ECEF*_t_ECEF_ENU;
+
+ Eigen::Vector3d ENU_lat_lon_alt = GNSSUtils::ecefToLatLonAlt(_t_ECEF_ENU);
+
+ double sLat = sin(ENU_lat_lon_alt(0));
+ double cLat = cos(ENU_lat_lon_alt(0));
+ double sLon = sin(ENU_lat_lon_alt(1));
+ double cLon = cos(ENU_lat_lon_alt(1));
+
+ R_ENU_ECEF(0, 0) = -sLon;
+ R_ENU_ECEF(0, 1) = cLon;
+ R_ENU_ECEF(0, 2) = 0.0;
+
+ R_ENU_ECEF(1, 0) = -sLat * cLon;
+ R_ENU_ECEF(1, 1) = -sLat * sLon;
+ R_ENU_ECEF(1, 2) = cLat;
+
+ R_ENU_ECEF(2, 0) = cLat * cLon;
+ R_ENU_ECEF(2, 1) = cLat * sLon;
+ R_ENU_ECEF(2, 2) = sLat;
+
+ t_ENU_ECEF = -R_ENU_ECEF * _t_ECEF_ENU;
+}
+
+void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF)
+{
+ double sLat = sin(_ENU_latlonalt(0));
+ double cLat = cos(_ENU_latlonalt(0));
+ double sLon = sin(_ENU_latlonalt(1));
+ double cLon = cos(_ENU_latlonalt(1));
+
+ R_ENU_ECEF(0, 0) = -sLon;
+ R_ENU_ECEF(0, 1) = cLon;
+ R_ENU_ECEF(0, 2) = 0.0;
+
+ R_ENU_ECEF(1, 0) = -sLat * cLon;
+ R_ENU_ECEF(1, 1) = -sLat * sLon;
+ R_ENU_ECEF(1, 2) = cLat;
+
+ R_ENU_ECEF(2, 0) = cLat * cLon;
+ R_ENU_ECEF(2, 1) = cLat * sLon;
+ R_ENU_ECEF(2, 2) = sLat;
+
+ Eigen::Vector3d t_ECEF_ENU = latLonAltToEcef(_ENU_latlonalt);
+
+ t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU;
+}
+
+} // namespace GNSSUtils
\ No newline at end of file
diff --git a/src/utils/utils.cpp b/src/utils/utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5336b047d0b0901ac502de3f4d443e5868bc5168
--- /dev/null
+++ b/src/utils/utils.cpp
@@ -0,0 +1,120 @@
+#include "gnss_utils/utils/utils.h"
+
+namespace GNSSUtils
+{
+void print(std::string& _msg)
+{
+ std::string msg = GNSSUTILS_MSG + _msg;
+
+ std::cout << msg << "\n";
+}
+
+void printArray(std::string _name, int* _array, int size)
+{
+ std::cout << _name << ": [";
+ for (int ii = 0; ii < size; ++ii)
+ {
+ std::cout << _array[ii];
+ if (ii == size - 1)
+ std::cout << "] \n";
+ else
+ std::cout << ",";
+ }
+}
+
+void printArray(std::string _name, unsigned char* _array, int size)
+{
+ std::cout << _name << ": [";
+ for (int ii = 0; ii < size; ++ii)
+ {
+ std::cout << (int)(_array[ii]);
+ if (ii == size - 1)
+ std::cout << "] \n";
+ else
+ std::cout << ",";
+ }
+}
+
+void printArray(std::string _name, double* _array, int size)
+{
+ std::cout << _name << ": [";
+ for (int ii = 0; ii < size; ++ii)
+ {
+ std::cout << _array[ii];
+ if (ii == size - 1)
+ std::cout << "] \n";
+ else
+ std::cout << ",";
+ }
+}
+
+void printArray(std::string _name, float* _array, int size)
+{
+ std::cout << _name << ": [";
+ for (int ii = 0; ii < size; ++ii)
+ {
+ std::cout << _array[ii];
+ if (ii == size - 1)
+ std::cout << "] \n";
+ else
+ std::cout << ",";
+ }
+}
+
+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
diff --git a/test/gtest_navigation.cpp b/test/gtest_navigation.cpp
index 5170cf11310248899fd967b3b870f974a6368b2f..18e8c638aa4ce62dcdfaa0a702f7be18652c5c9c 100644
--- a/test/gtest_navigation.cpp
+++ b/test/gtest_navigation.cpp
@@ -1,5 +1,5 @@
#include "gtest/utils_gtest.h"
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
TEST(NavigationTests, Whatever)
{
diff --git a/test/gtest_observations.cpp b/test/gtest_observations.cpp
index fe10475eb943bd0c76e9e7e14092cd82d95a5135..80158905aba3f9ec890a6fb2d4acd8829e806245 100644
--- a/test/gtest_observations.cpp
+++ b/test/gtest_observations.cpp
@@ -1,5 +1,4 @@
#include "gtest/utils_gtest.h"
-#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/observations.h"
using namespace GNSSUtils;
diff --git a/test/gtest_transformations.cpp b/test/gtest_transformations.cpp
index cae4e24bb4bd45e99fceaa07a201fe36926b5f78..8984b1009d368b9160ae5c43b88b99d00e1b00ca 100644
--- a/test/gtest_transformations.cpp
+++ b/test/gtest_transformations.cpp
@@ -1,5 +1,5 @@
#include "gtest/utils_gtest.h"
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/sat_position.h"
// Geodetic system parameters
static double kSemimajorAxis = 6378137;