diff --git a/.gitignore b/.gitignore
index 09480084001dca36c1cd64547557cef64a969a3f..5a52268305c24ee5e138acf3884b7e7c3050ea0e 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,4 @@
bin/
build/
-lib/
\ No newline at end of file
+lib/
+.clang-format
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000000000000000000000000000000000000..afd1a200f78dad63690a42eb88e2c5ced5b27bed
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,16 @@
+{
+ "configurations": [
+ {
+ "name": "Linux",
+ "includePath": [
+ "${workspaceFolder}/**",
+ "${workspaceFolder}/include"
+ ],
+ "defines": [],
+ "compilerPath": "/usr/bin/clang",
+ "cStandard": "c11",
+ "intelliSenseMode": "clang-x64"
+ }
+ ],
+ "version": 4
+}
\ No newline at end of file
diff --git a/include/gnss_utils/TDCP.h b/include/gnss_utils/TDCP.h
index 3580569f867b31bee90ae83d67d012a16f0b1233..439a981ad097ea568d1cda9321b31c9e1b86fa55 100644
--- a/include/gnss_utils/TDCP.h
+++ b/include/gnss_utils/TDCP.h
@@ -17,47 +17,69 @@
namespace GNSSUtils
{
- struct TDCPParams
- {
- int min_common_sats;
- int raim_n;
- double raim_min_residual;
- bool use_carrier_phase;
- bool correct_tropo;
- bool correct_iono;
- bool relinearize_jacobian;
- int max_iterations;
- double sigma_atm;
- double sigma_code;
- double sigma_carrier;
- bool use_old_nav;
- bool use_multi_freq;
- double time_window;
- };
-
- bool TDCP(const Observations& obs_r, Navigation& nav_r,
- const Observations& obs_k, const Navigation& nav_k,
- Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
- double& residual, std::set<int>& discarded_sats,
- const TDCPParams& sd_opt, const prcopt_t& opt);
-
- bool TDCP(const Observations& obs_r, const Navigation& nav_r, const Eigen::Vector3d& x_r,
- const Observations& obs_k, const Navigation& nav_k,
- Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
- double& residual, std::set<int>& discarded_sats,
- const TDCPParams& sd_opt, const prcopt_t& opt);
-
- bool TDCP(const Observations& common_obs_r, const Navigation& nav_r, const std::map<int,Eigen::Vector3d>& common_sats_pos_r, const Eigen::Vector3d& x_r,
- const Observations& common_obs_k, const Navigation& nav_k, const std::map<int,Eigen::Vector3d>& common_sats_pos_k,
- Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
- double& residual, std::set<int>& discarded_sats,
- const TDCPParams& sd_opt);
-
- void filterCommonSatellites(Observations& common_obs_r, std::map<int,Eigen::Vector3d>& common_sats_pos_r,
- Observations& common_obs_k, std::map<int,Eigen::Vector3d>& common_sats_pos_k,
- std::set<int>& discarded_sats, const Eigen::Vector3d& x_r,
- const TDCPParams& sd_params, const prcopt_t& opt);
-
-}
+struct TDCPParams
+{
+ int min_common_sats;
+ int raim_n;
+ double raim_min_residual;
+ bool use_carrier_phase;
+ bool correct_tropo;
+ bool correct_iono;
+ bool relinearize_jacobian;
+ int max_iterations;
+ double sigma_atm;
+ double sigma_code;
+ double sigma_carrier;
+ bool use_old_nav;
+ bool use_multi_freq;
+ double time_window;
+};
+
+bool TDCP(const Observations& obs_r,
+ Navigation& nav_r,
+ const Observations& obs_k,
+ const Navigation& nav_k,
+ Eigen::Vector4d& d,
+ Eigen::Matrix4d& cov_d,
+ double& residual,
+ std::set<int>& discarded_sats,
+ const TDCPParams& sd_opt,
+ const prcopt_t& opt);
+
+bool TDCP(const Observations& obs_r,
+ const Navigation& nav_r,
+ const Eigen::Vector3d& x_r,
+ const Observations& obs_k,
+ const Navigation& nav_k,
+ Eigen::Vector4d& d,
+ Eigen::Matrix4d& cov_d,
+ double& residual,
+ std::set<int>& discarded_sats,
+ const TDCPParams& sd_opt,
+ const prcopt_t& opt);
+
+bool TDCP(const Observations& common_obs_r,
+ const Navigation& nav_r,
+ const std::map<int, Eigen::Vector3d>& common_sats_pos_r,
+ const Eigen::Vector3d& x_r,
+ const Observations& common_obs_k,
+ const Navigation& nav_k,
+ const std::map<int, Eigen::Vector3d>& common_sats_pos_k,
+ Eigen::Vector4d& d,
+ Eigen::Matrix4d& cov_d,
+ double& residual,
+ std::set<int>& discarded_sats,
+ const TDCPParams& sd_opt);
+
+void filterCommonSatellites(Observations& common_obs_r,
+ std::map<int, Eigen::Vector3d>& common_sats_pos_r,
+ Observations& common_obs_k,
+ std::map<int, Eigen::Vector3d>& common_sats_pos_k,
+ std::set<int>& discarded_sats,
+ const Eigen::Vector3d& x_r,
+ const TDCPParams& sd_params,
+ const prcopt_t& opt);
+
+} // namespace GNSSUtils
#endif /* INCLUDE_GNSS_UTILS_TDCP_H_ */
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index ec75472312f3add20a8730ce79abace251d5e674..a16976acdec9a321d0b47fa1f4c647fab896940b 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -1,7 +1,6 @@
#ifndef GNSS_UTILS_H
#define GNSS_UTILS_H
-
#include <vector>
#include <iostream>
#include <memory>
@@ -14,62 +13,92 @@
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
-extern "C"
-{
- #include "rtklib.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);
-
- 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);
+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);
+
+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
diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h
index ba4c3f594f6fa59e852575fb87d9d6c49b4cf4da..20f65a0ffb1e111a5ba68f5a1a4f22693e73b426 100644
--- a/include/gnss_utils/navigation.h
+++ b/include/gnss_utils/navigation.h
@@ -5,364 +5,361 @@
#include <iostream>
#include <memory>
-extern "C"
-{
- #include "rtklib.h"
+extern "C" {
+#include "rtklib.h"
}
namespace GNSSUtils
{
-
class Navigation;
-typedef std::shared_ptr<Navigation> NavigationPtr;
+typedef std::shared_ptr<Navigation> NavigationPtr;
typedef std::shared_ptr<const Navigation> NavigationConstPtr;
class Navigation
{
- public:
- // Constructor & Destructor
- Navigation();
- Navigation(const Navigation& nav);
- ~Navigation();
-
- // Public objects
-
- // Public methods
- void setNavigation(nav_t nav);
- void clearNavigation();
- void loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
-
- const nav_t & getNavigation() const;
- nav_t & getNavigation();
-
- void uniqueNavigation(); //remove duplicated ephemerides and update carrier phase wave lengths
-
- /****************** Array memory management ******************/
- bool addEphemeris(const eph_t &eph);
- bool addGlonassEphemeris(const geph_t& geph);
- bool addSbasEphemeris(const seph_t& seph);
- bool addPreciseEphemeris(const peph_t& peph);
- bool addPreciseClock(const pclk_t& pclk);
- bool addAlmanac(const alm_t& alm);
- bool addTec(const tec_t& tec);
- bool addFcb(const fcbd_t& fcb);
-
- void addSbasMessage(const sbsmsg_t &sbas_msg);
-
- void copyAllArrays(const nav_t& nav);
- void copyEphemeris(const nav_t& nav);
- void copyAlmanac(const nav_t& nav);
- void copyIonUtc(const nav_t &nav);
- void copySbasCorrections(const nav_t &nav);
-
- void freeNavigationArrays();
- void freeEphemeris();
- void freeGlonassEphemeris();
- void freeSbasEphemeris();
- void freePreciseEphemeris();
- void freeAlmanac();
- void freePreciseClock();
- void freeTecData();
- void freeFcbData();
-
- void print();
-
- //////////////////////////////// nav UTILS //////////////////////////////////////
- template <typename T>
- static bool addToArray(const T &new_element, T *&array, int &n, int &nmax);
- template <typename T>
- static bool copyArray(const T *array_in, const int &n_in, T *&array_out, int &n_out, int &nmax_out);
- template <typename T>
- static void freeArray(T *&array, int &n, int &nmax);
- static void freeEph(nav_t &nav);
- static void freeGeph(nav_t &nav);
- static void freeSeph(nav_t &nav);
- static void freePeph(nav_t &nav);
- static void freeAlm(nav_t &nav);
- static void freePclk(nav_t &nav);
- static void freeTec(nav_t &nav);
- static void freeFcb(nav_t &nav);
- static void freeNavArrays(nav_t &nav);
-
- private:
-
- // rtklib-like attribute to represent the different navigation msgs for a given epoch
- nav_t nav_;
-
- // Private methods
+public:
+ // Constructor & Destructor
+ Navigation();
+ Navigation(const Navigation& nav);
+ ~Navigation();
+
+ // Public objects
+
+ // Public methods
+ void setNavigation(nav_t nav);
+ void clearNavigation();
+ void
+ loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
+
+ const nav_t& getNavigation() const;
+ nav_t& getNavigation();
+
+ void uniqueNavigation(); // remove duplicated ephemerides and update carrier phase wave lengths
+
+ /****************** Array memory management ******************/
+ bool addEphemeris(const eph_t& eph);
+ bool addGlonassEphemeris(const geph_t& geph);
+ bool addSbasEphemeris(const seph_t& seph);
+ bool addPreciseEphemeris(const peph_t& peph);
+ bool addPreciseClock(const pclk_t& pclk);
+ bool addAlmanac(const alm_t& alm);
+ bool addTec(const tec_t& tec);
+ bool addFcb(const fcbd_t& fcb);
+
+ void addSbasMessage(const sbsmsg_t& sbas_msg);
+
+ void copyAllArrays(const nav_t& nav);
+ void copyEphemeris(const nav_t& nav);
+ void copyAlmanac(const nav_t& nav);
+ void copyIonUtc(const nav_t& nav);
+ void copySbasCorrections(const nav_t& nav);
+
+ void freeNavigationArrays();
+ void freeEphemeris();
+ void freeGlonassEphemeris();
+ void freeSbasEphemeris();
+ void freePreciseEphemeris();
+ void freeAlmanac();
+ void freePreciseClock();
+ void freeTecData();
+ void freeFcbData();
+
+ void print();
+
+ //////////////////////////////// nav UTILS //////////////////////////////////////
+ template <typename T>
+ static bool addToArray(const T& new_element, T*& array, int& n, int& nmax);
+ template <typename T>
+ static bool copyArray(const T* array_in, const int& n_in, T*& array_out, int& n_out, int& nmax_out);
+ template <typename T>
+ static void freeArray(T*& array, int& n, int& nmax);
+ static void freeEph(nav_t& nav);
+ static void freeGeph(nav_t& nav);
+ static void freeSeph(nav_t& nav);
+ static void freePeph(nav_t& nav);
+ static void freeAlm(nav_t& nav);
+ static void freePclk(nav_t& nav);
+ static void freeTec(nav_t& nav);
+ static void freeFcb(nav_t& nav);
+ static void freeNavArrays(nav_t& nav);
+
+private:
+ // rtklib-like attribute to represent the different navigation msgs for a given epoch
+ nav_t nav_;
+
+ // Private methods
};
-inline void Navigation::uniqueNavigation() //remove duplicated ephemerides and update wave lengths
+inline void Navigation::uniqueNavigation() // remove duplicated ephemerides and update wave lengths
{
- uniqnav(&nav_);
+ uniqnav(&nav_);
}
-inline const nav_t & Navigation::getNavigation() const
+inline const nav_t& Navigation::getNavigation() const
{
return nav_;
}
-inline nav_t & Navigation::getNavigation()
+inline nav_t& Navigation::getNavigation()
{
return nav_;
}
-inline bool Navigation::addEphemeris(const eph_t &eph)
+inline bool Navigation::addEphemeris(const eph_t& eph)
{
- return addToArray<eph_t>(eph, nav_.eph, nav_.n, nav_.nmax);
- // // "inspired" from RTKLIB rinex.c
- // eph_t *nav_eph;
- //
- // if (nav_.nmax<=nav_.n) {
- // nav_.nmax+=1024;
- // if (!(nav_eph=(eph_t *)realloc(nav_.eph,sizeof(eph_t)*nav_.nmax))) {
- // printf("addEphemeris malloc error: n=%d\n",nav_.nmax);
- // free(nav_.eph); nav_.eph=NULL; nav_.n=nav_.nmax=0;
- // return false;
- // }
- // nav_.eph=nav_eph;
- // }
- // nav_.eph[nav_.n++]=eph;
- // return true;
+ return addToArray<eph_t>(eph, nav_.eph, nav_.n, nav_.nmax);
+ // // "inspired" from RTKLIB rinex.c
+ // eph_t *nav_eph;
+ //
+ // if (nav_.nmax<=nav_.n) {
+ // nav_.nmax+=1024;
+ // if (!(nav_eph=(eph_t *)realloc(nav_.eph,sizeof(eph_t)*nav_.nmax))) {
+ // printf("addEphemeris malloc error: n=%d\n",nav_.nmax);
+ // free(nav_.eph); nav_.eph=NULL; nav_.n=nav_.nmax=0;
+ // return false;
+ // }
+ // nav_.eph=nav_eph;
+ // }
+ // nav_.eph[nav_.n++]=eph;
+ // return true;
}
inline bool Navigation::addGlonassEphemeris(const geph_t& geph)
{
- return addToArray<geph_t>(geph, nav_.geph, nav_.ng, nav_.ngmax);
-// // "inspired" from RTKLIB rinex.c
-// geph_t *nav_geph;
-//
-// if (nav_.ngmax<=nav_.ng) {
-// nav_.ngmax+=1024;
-// if (!(nav_geph=(geph_t *)realloc(nav_.geph,sizeof(geph_t)*nav_.ngmax))) {
-// printf("addGLONASSEphemeris malloc error: n=%d\n",nav_.ngmax);
-// free(nav_.geph); nav_.geph=NULL; nav_.ng=nav_.ngmax=0;
-// return false;
-// }
-// nav_.geph=nav_geph;
-// }
-// nav_.geph[nav_.ng++]=geph;
-// return true;
+ return addToArray<geph_t>(geph, nav_.geph, nav_.ng, nav_.ngmax);
+ // // "inspired" from RTKLIB rinex.c
+ // geph_t *nav_geph;
+ //
+ // if (nav_.ngmax<=nav_.ng) {
+ // nav_.ngmax+=1024;
+ // if (!(nav_geph=(geph_t *)realloc(nav_.geph,sizeof(geph_t)*nav_.ngmax))) {
+ // printf("addGLONASSEphemeris malloc error: n=%d\n",nav_.ngmax);
+ // free(nav_.geph); nav_.geph=NULL; nav_.ng=nav_.ngmax=0;
+ // return false;
+ // }
+ // nav_.geph=nav_geph;
+ // }
+ // nav_.geph[nav_.ng++]=geph;
+ // return true;
}
inline bool Navigation::addSbasEphemeris(const seph_t& seph)
{
- return addToArray<seph_t>(seph, nav_.seph, nav_.ns, nav_.nsmax);
-// // "inspired" from RTKLIB rinex.c
-// seph_t *nav_seph;
-//
-// if (nav_.nsmax<=nav_.ns) {
-// nav_.nsmax+=1024;
-// if (!(nav_seph=(seph_t *)realloc(nav_.seph,sizeof(seph_t)*nav_.nsmax))) {
-// printf("addSBASEphemeris malloc error: n=%d\n",nav_.nsmax);
-// free(nav_.seph); nav_.seph=NULL; nav_.ns=nav_.nsmax=0;
-// return false;
-// }
-// nav_.seph=nav_seph;
-// }
-// nav_.seph[nav_.ns++]=seph;
-// return true;
+ return addToArray<seph_t>(seph, nav_.seph, nav_.ns, nav_.nsmax);
+ // // "inspired" from RTKLIB rinex.c
+ // seph_t *nav_seph;
+ //
+ // if (nav_.nsmax<=nav_.ns) {
+ // nav_.nsmax+=1024;
+ // if (!(nav_seph=(seph_t *)realloc(nav_.seph,sizeof(seph_t)*nav_.nsmax))) {
+ // printf("addSBASEphemeris malloc error: n=%d\n",nav_.nsmax);
+ // free(nav_.seph); nav_.seph=NULL; nav_.ns=nav_.nsmax=0;
+ // return false;
+ // }
+ // nav_.seph=nav_seph;
+ // }
+ // nav_.seph[nav_.ns++]=seph;
+ // return true;
}
inline bool Navigation::addPreciseEphemeris(const peph_t& peph)
{
- return addToArray<peph_t>(peph, nav_.peph, nav_.ne, nav_.nemax);
+ return addToArray<peph_t>(peph, nav_.peph, nav_.ne, nav_.nemax);
}
inline bool Navigation::addPreciseClock(const pclk_t& pclk)
{
- return addToArray<pclk_t>(pclk, nav_.pclk, nav_.nc, nav_.ncmax);
+ return addToArray<pclk_t>(pclk, nav_.pclk, nav_.nc, nav_.ncmax);
}
inline bool Navigation::addAlmanac(const alm_t& alm)
{
- return addToArray<alm_t>(alm, nav_.alm, nav_.na, nav_.namax);
-// // "inspired" from RTKLIB rinex.c
-// alm_t *nav_alm;
-//
-// if (nav_.namax<=nav_.na) {
-// nav_.namax+=1024;
-// if (!(nav_alm=(alm_t *)realloc(nav_.alm,sizeof(alm_t)*nav_.namax))) {
-// printf("addAlmanac malloc error: n=%d\n",nav_.namax);
-// free(nav_.alm); nav_.alm=NULL; nav_.na=nav_.namax=0;
-// return false;
-// }
-// nav_.alm=nav_alm;
-// }
-// nav_.alm[nav_.na++]=alm;
-// return true;
+ return addToArray<alm_t>(alm, nav_.alm, nav_.na, nav_.namax);
+ // // "inspired" from RTKLIB rinex.c
+ // alm_t *nav_alm;
+ //
+ // if (nav_.namax<=nav_.na) {
+ // nav_.namax+=1024;
+ // if (!(nav_alm=(alm_t *)realloc(nav_.alm,sizeof(alm_t)*nav_.namax))) {
+ // printf("addAlmanac malloc error: n=%d\n",nav_.namax);
+ // free(nav_.alm); nav_.alm=NULL; nav_.na=nav_.namax=0;
+ // return false;
+ // }
+ // nav_.alm=nav_alm;
+ // }
+ // nav_.alm[nav_.na++]=alm;
+ // return true;
}
inline bool Navigation::addTec(const tec_t& tec)
{
- return addToArray<tec_t>(tec, nav_.tec, nav_.nt, nav_.ntmax);
+ return addToArray<tec_t>(tec, nav_.tec, nav_.nt, nav_.ntmax);
}
inline bool Navigation::addFcb(const fcbd_t& fcb)
{
- return addToArray<fcbd_t>(fcb, nav_.fcb, nav_.nf, nav_.nfmax);
+ return addToArray<fcbd_t>(fcb, nav_.fcb, nav_.nf, nav_.nfmax);
}
-inline void Navigation::addSbasMessage(const sbsmsg_t &sbas_msg)
+inline void Navigation::addSbasMessage(const sbsmsg_t& sbas_msg)
{
- sbsupdatecorr(&sbas_msg, &nav_);
+ sbsupdatecorr(&sbas_msg, &nav_);
}
-
inline void Navigation::freeEphemeris()
{
- freeEph(nav_);
+ freeEph(nav_);
}
inline void Navigation::freeGlonassEphemeris()
{
- freeGeph(nav_);
+ freeGeph(nav_);
}
inline void Navigation::freePreciseEphemeris()
{
- freePeph(nav_);
+ freePeph(nav_);
}
inline void Navigation::freeSbasEphemeris()
{
- freeSeph(nav_);
+ freeSeph(nav_);
}
inline void Navigation::freePreciseClock()
{
- freePclk(nav_);
+ freePclk(nav_);
}
inline void Navigation::freeTecData()
{
- freeTec(nav_);
+ freeTec(nav_);
}
inline void Navigation::freeFcbData()
{
- freeFcb(nav_);
+ freeFcb(nav_);
}
inline void Navigation::freeAlmanac()
{
- freeAlm(nav_);
+ freeAlm(nav_);
}
//////////////////////////////// nav UTILS //////////////////////////////////////
template <typename T>
-inline bool Navigation::addToArray(const T &new_element, T *&array, int &n, int &nmax)
-{
- //std::cout << "addToArray: n = " << n << " nmax = " << nmax << "\n";
- // "inspired" from RTKLIB rinex.c
- T *array_ref;
- if (nmax <= n)
+inline bool Navigation::addToArray(const T& new_element, T*& array, int& n, int& nmax)
+{
+ // std::cout << "addToArray: n = " << n << " nmax = " << nmax << "\n";
+ // "inspired" from RTKLIB rinex.c
+ T* array_ref;
+ if (nmax <= n)
+ {
+ // std::cout << "addToArray: nmax <= n\n";
+ nmax += 1024;
+ if (!(array_ref = (T*)realloc(array, sizeof(T) * nmax)))
{
- //std::cout << "addToArray: nmax <= n\n";
- nmax += 1024;
- if (!(array_ref = (T*)realloc(array, sizeof(T) * nmax)))
- {
- printf("addToArray malloc error: n=%d\n", nmax);
- free(array);
- array = NULL;
- n = nmax = 0;
- return false;
- }
- //std::cout << "addToArray: assigning reallocated array\n";
- array = array_ref;
+ printf("addToArray malloc error: n=%d\n", nmax);
+ free(array);
+ array = NULL;
+ n = nmax = 0;
+ return false;
}
- //std::cout << "addToArray: adding element " << n << "\n";
- array[n++] = new_element;
- //std::cout << "addToArray: added!\n";
- return true;
+ // std::cout << "addToArray: assigning reallocated array\n";
+ array = array_ref;
+ }
+ // std::cout << "addToArray: adding element " << n << "\n";
+ array[n++] = new_element;
+ // std::cout << "addToArray: added!\n";
+ return true;
}
-template<typename T>
-inline bool Navigation::copyArray(const T *array_in, const int &n_in, T *&array_out, int &n_out, int &nmax_out)
+template <typename T>
+inline bool Navigation::copyArray(const T* array_in, const int& n_in, T*& array_out, int& n_out, int& nmax_out)
{
- //std::cout << "copyArray: " << n_in << " elements\n";
- if (array_in == NULL)
- return false;
+ // std::cout << "copyArray: " << n_in << " elements\n";
+ if (array_in == NULL)
+ return false;
- //std::cout << "copyArray: array in not null\n";
+ // std::cout << "copyArray: array in not null\n";
- for (int i = 0; i<n_in; i++)
+ for (int i = 0; i < n_in; i++)
+ {
+ // std::cout << "copyArray: adding element " << i << "\n";
+ if (!addToArray<T>(array_in[i], array_out, n_out, nmax_out))
{
- //std::cout << "copyArray: adding element " << i << "\n";
- if (!addToArray<T>(array_in[i], array_out, n_out, nmax_out))
- {
- //std::cout << "copyArray: failed to add..\n";
- return false;
- }
- //std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n";
+ // std::cout << "copyArray: failed to add..\n";
+ return false;
}
+ // std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n";
+ }
- //std::cout << "copyArray: all copied\n";
+ // std::cout << "copyArray: all copied\n";
- return true;
+ return true;
}
-template<typename T>
-void Navigation::freeArray(T *&array, int &n, int &nmax)
+template <typename T>
+void Navigation::freeArray(T*& array, int& n, int& nmax)
{
- if (array != NULL)
- free(array);
- array = NULL;
- n = nmax = 0;
+ if (array != NULL)
+ free(array);
+ array = NULL;
+ n = nmax = 0;
}
-inline void Navigation::freeEph(nav_t &nav)
+inline void Navigation::freeEph(nav_t& nav)
{
- freeArray<eph_t>(nav.eph, nav.n, nav.nmax);
+ freeArray<eph_t>(nav.eph, nav.n, nav.nmax);
}
-inline void Navigation::freeGeph(nav_t &nav)
+inline void Navigation::freeGeph(nav_t& nav)
{
- freeArray<geph_t>(nav.geph, nav.ng, nav.ngmax);
+ freeArray<geph_t>(nav.geph, nav.ng, nav.ngmax);
}
-inline void Navigation::freeSeph(nav_t &nav)
+inline void Navigation::freeSeph(nav_t& nav)
{
- freeArray<seph_t>(nav.seph, nav.ns, nav.nsmax);
+ freeArray<seph_t>(nav.seph, nav.ns, nav.nsmax);
}
-inline void Navigation::freePeph(nav_t &nav)
+inline void Navigation::freePeph(nav_t& nav)
{
- freeArray<peph_t>(nav.peph, nav.ne, nav.nemax);
+ freeArray<peph_t>(nav.peph, nav.ne, nav.nemax);
}
-inline void Navigation::freeAlm(nav_t &nav)
+inline void Navigation::freeAlm(nav_t& nav)
{
- freeArray<alm_t>(nav.alm, nav.na, nav.namax);
+ freeArray<alm_t>(nav.alm, nav.na, nav.namax);
}
-inline void Navigation::freePclk(nav_t &nav)
+inline void Navigation::freePclk(nav_t& nav)
{
- freeArray<pclk_t>(nav.pclk, nav.nc, nav.ncmax);
+ freeArray<pclk_t>(nav.pclk, nav.nc, nav.ncmax);
}
-inline void Navigation::freeTec(nav_t &nav)
+inline void Navigation::freeTec(nav_t& nav)
{
- freeArray<tec_t>(nav.tec, nav.nt, nav.ntmax);
+ freeArray<tec_t>(nav.tec, nav.nt, nav.ntmax);
}
-inline void Navigation::freeFcb(nav_t &nav)
+inline void Navigation::freeFcb(nav_t& nav)
{
- freeArray<fcbd_t>(nav.fcb, nav.nf, nav.nfmax);
+ freeArray<fcbd_t>(nav.fcb, nav.nf, nav.nfmax);
}
-inline void Navigation::freeNavArrays(nav_t &nav)
+inline void Navigation::freeNavArrays(nav_t& nav)
{
- // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
- freeEph(nav);
- freeGeph(nav);
- freeSeph(nav);
- freePeph(nav);
- freePclk(nav);
- freeAlm(nav);
- freeTec(nav);
- freeFcb(nav);
+ // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
+ freeEph(nav);
+ freeGeph(nav);
+ freeSeph(nav);
+ freePeph(nav);
+ freePclk(nav);
+ freeAlm(nav);
+ freeTec(nav);
+ freeFcb(nav);
}
-}
+} // namespace GNSSUtils
#endif
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index f0715dc58893ee4fe3e3b48868089cbc5c480923..5610a7ec08698c19a6cf84152da0773b83f55cb2 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -9,127 +9,127 @@
#include "gnss_utils/utils.h"
-extern "C"
-{
- #include "rtklib.h"
+extern "C" {
+#include "rtklib.h"
}
namespace GNSSUtils
{
-
class Observations;
-typedef std::shared_ptr<Observations> ObservationsPtr;
+typedef std::shared_ptr<Observations> ObservationsPtr;
typedef std::shared_ptr<const Observations> ObservationsConstPtr;
class Observations
{
- public:
- // Constructor & Destructor
- Observations();
- Observations(const Observations& obs);
- ~Observations();
+public:
+ // Constructor & Destructor
+ Observations();
+ Observations(const Observations& obs);
+ ~Observations();
- // Public objects
+ // Public objects
- // Public methods
+ // Public methods
+ /* - Observations - */
- /* - Observations - */
+ void clearObservations();
- void clearObservations();
+ void addObservation(const obsd_t& obs);
+ void
+ loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
- void addObservation(const obsd_t & obs);
- void loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt=0.0, const char* opt="");
+ void removeObservationByIdx(const int& _idx);
+ void removeObservationBySat(const int& _sat);
- void removeObservationByIdx(const int& _idx);
- void removeObservationBySat(const int& _sat);
+ std::vector<obsd_t>& getObservations();
+ const std::vector<obsd_t>& getObservations() const;
- std::vector<obsd_t>& getObservations();
- const std::vector<obsd_t> & getObservations() const;
+ obsd_t& getObservationBySat(const unsigned char& sat_number);
+ const obsd_t& getObservationBySat(const unsigned char& sat_number) const;
- obsd_t& getObservationBySat(const unsigned char& sat_number);
- const obsd_t& getObservationBySat(const unsigned char& sat_number) const;
+ obsd_t& getObservationByIdx(const int& idx);
+ const obsd_t& getObservationByIdx(const int& idx) const;
- obsd_t& getObservationByIdx(const int& idx);
- const obsd_t& getObservationByIdx(const int& idx) const;
+ obsd_t* data();
+ const obsd_t* data() const;
- obsd_t* data();
- const obsd_t* data() const;
+ size_t size() const;
- size_t size() const;
+ bool hasSatellite(const unsigned char& i) const;
- bool hasSatellite(const unsigned char& i) const;
+ void print(obsd_t& _obs);
+ void printBySat(const int& _sat);
+ void printByIdx(const int& _idx);
+ void print();
- void print(obsd_t & _obs);
- void printBySat(const int& _sat);
- void printByIdx(const int& _idx);
- void print();
+ static void findCommonObservations(const Observations& obs_1,
+ const Observations& obs_2,
+ Observations& common_obs_1,
+ Observations& common_obs_2);
- static void findCommonObservations(const Observations& obs_1, const Observations& obs_2,
- Observations& common_obs_1, Observations& common_obs_2);
+private:
+ // Private objects
+ std::map<unsigned char, int> sat_2_idx_; //< key: corresponding sat number, value: idx in obs_ vector
+ std::vector<unsigned char> idx_2_sat_; //< key: idx in obs_ vector, value: corresponding sat number
+ std::vector<obsd_t> obs_; //< vector of RTKLIB observations for a given epoch
- private:
- // Private objects
- std::map<unsigned char,int> sat_2_idx_; //< key: corresponding sat number, value: idx in obs_ vector
- std::vector<unsigned char> idx_2_sat_; //< key: idx in obs_ vector, value: corresponding sat number
- std::vector<obsd_t> obs_; //< vector of RTKLIB observations for a given epoch
-
- // Private methods
+ // Private methods
};
inline const std::vector<obsd_t>& Observations::getObservations() const
{
- return this->obs_;
+ return this->obs_;
}
inline std::vector<obsd_t>& Observations::getObservations()
{
- return this->obs_;
+ return this->obs_;
}
inline obsd_t& Observations::getObservationBySat(const unsigned char& sat_number)
{
- assert(sat_2_idx_.count(sat_number) != 0);
- return obs_.at(sat_2_idx_.at(sat_number));
+ assert(sat_2_idx_.count(sat_number) != 0);
+ return obs_.at(sat_2_idx_.at(sat_number));
}
inline const obsd_t& Observations::getObservationBySat(const unsigned char& sat_number) const
{
- assert(sat_2_idx_.count(sat_number) != 0);
- return obs_.at(sat_2_idx_.at(sat_number));
+ assert(sat_2_idx_.count(sat_number) != 0);
+ return obs_.at(sat_2_idx_.at(sat_number));
}
inline obsd_t& Observations::getObservationByIdx(const int& idx)
{
- assert(obs_.size() > idx);
- return obs_.at(idx);
+ assert(obs_.size() > idx);
+ return obs_.at(idx);
}
inline const obsd_t& Observations::getObservationByIdx(const int& idx) const
{
- assert(obs_.size() > idx);
- return obs_.at(idx);
+ assert(obs_.size() > idx);
+ return obs_.at(idx);
}
inline obsd_t* Observations::data()
{
- return obs_.data();
+ return obs_.data();
}
inline const obsd_t* Observations::data() const
{
- return obs_.data();
+ return obs_.data();
}
inline size_t Observations::size() const
{
- return obs_.size();
+ return obs_.size();
}
inline bool Observations::hasSatellite(const unsigned char& i) const
{
- return sat_2_idx_.count(i) != 0;
+ return sat_2_idx_.count(i) != 0;
}
-}
+} // namespace GNSSUtils
#endif
diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h
index d14dc5b13603018c72d3c4a5f901bd9fc9348677..c3e4cc15180559331f3b0732f4243eb684076b63 100644
--- a/include/gnss_utils/ublox_raw.h
+++ b/include/gnss_utils/ublox_raw.h
@@ -5,12 +5,23 @@
namespace GNSSUtils
{
-
-enum RawDataType : int {NO = 0, OBS = 1, NAV_EPH = 2, NAV_SBAS = 3, NAV_ALM = 9, NAV_ANT = 5, NAV_DGPS = 7, NAV_SSR = 10, NAV_LEX = 31, ERROR = -1};
+enum RawDataType : int
+{
+ NO = 0,
+ OBS = 1,
+ NAV_EPH = 2,
+ NAV_SBAS = 3,
+ NAV_ALM = 9,
+ NAV_ANT = 5,
+ NAV_DGPS = 7,
+ NAV_SSR = 10,
+ NAV_LEX = 31,
+ ERROR = -1
+};
class UBloxRaw
{
- public:
+public:
UBloxRaw();
~UBloxRaw();
@@ -21,26 +32,25 @@ class UBloxRaw
RawDataType getRawDataType() const;
- private:
+private:
raw_t raw_data_;
Observations obs_;
- Navigation nav_;
+ Navigation nav_;
RawDataType raw_data_type_;
-
- void updateObservations();
+ void updateObservations();
};
inline const GNSSUtils::Observations& UBloxRaw::getObservations()
{
- return obs_;
+ return obs_;
}
inline const GNSSUtils::Navigation& UBloxRaw::getNavigation()
{
- return nav_;
+ return nav_;
}
inline RawDataType UBloxRaw::getRawDataType() const
@@ -48,7 +58,6 @@ inline RawDataType UBloxRaw::getRawDataType() const
return raw_data_type_;
}
-}
-
+} // namespace GNSSUtils
#endif
diff --git a/include/gnss_utils/utils.h b/include/gnss_utils/utils.h
index 4d19e40ab78baa88c2d15a78184f7dafa23e357a..fa24b0896b8d0f4158143aae1c6a6698e66fe8d3 100644
--- a/include/gnss_utils/utils.h
+++ b/include/gnss_utils/utils.h
@@ -6,18 +6,16 @@
#include <memory>
#include <string>
-#define ARRAY_SIZE(arr) sizeof(arr)/sizeof(arr[0])
+#define ARRAY_SIZE(arr) sizeof(arr) / sizeof(arr[0])
#define GNSSUTILS_MSG "--GNSSUtils--"
namespace GNSSUtils
{
-
-
- void print(std::string & _msg);
- 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);
-}
+void print(std::string& _msg);
+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);
+} // namespace GNSSUtils
#endif
\ No newline at end of file
diff --git a/src/TDCP.cpp b/src/TDCP.cpp
index 033e4925f159d66f96edff9d197f1b946f4cfd2e..66e5a10997152c4544c7b6d1e5eaefa2cfa6028f 100644
--- a/src/TDCP.cpp
+++ b/src/TDCP.cpp
@@ -9,548 +9,571 @@
namespace GNSSUtils
{
-
-bool TDCP(const Observations& obs_r, Navigation& nav_r,
- const Observations& obs_k, const Navigation& nav_k,
- Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
- double& residual, std::set<int>& discarded_sats,
- const TDCPParams& sd_params, const prcopt_t& opt)
+bool TDCP(const Observations& obs_r,
+ Navigation& nav_r,
+ const Observations& obs_k,
+ const Navigation& nav_k,
+ Eigen::Vector4d& d,
+ Eigen::Matrix4d& cov_d,
+ double& residual,
+ std::set<int>& discarded_sats,
+ const TDCPParams& sd_params,
+ const prcopt_t& opt)
{
- // COMPUTE SINGLE DIFFERENCES
- return TDCP(obs_r, nav_r, computePos(obs_r,nav_r,opt).pos,
- obs_k, nav_k,
- d, cov_d,
- residual, discarded_sats,
- sd_params, opt);
+ // COMPUTE SINGLE DIFFERENCES
+ return TDCP(obs_r,
+ nav_r,
+ computePos(obs_r, nav_r, opt).pos,
+ obs_k,
+ nav_k,
+ d,
+ cov_d,
+ residual,
+ discarded_sats,
+ sd_params,
+ opt);
}
-bool TDCP(const Observations& obs_r, const Navigation& nav_r, const Eigen::Vector3d& x_r,
- const Observations& obs_k, const Navigation& nav_k,
- Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
- double& residual, std::set<int>& discarded_sats,
- const TDCPParams& sd_params, const prcopt_t& opt)
+bool TDCP(const Observations& obs_r,
+ const Navigation& nav_r,
+ const Eigen::Vector3d& x_r,
+ const Observations& obs_k,
+ const Navigation& nav_k,
+ Eigen::Vector4d& d,
+ Eigen::Matrix4d& cov_d,
+ double& residual,
+ std::set<int>& discarded_sats,
+ const TDCPParams& sd_params,
+ const prcopt_t& opt)
{
- // FIND COMMON SATELLITES OBSERVATIONS
- Observations common_obs_r, common_obs_k;
- Observations::findCommonObservations(obs_r, obs_k, common_obs_r, common_obs_k);
- int n_common_sats = common_obs_r.getObservations().size();
-
- // COMPUTE COMMON SATELLITES POSITION
- std::map<int, Eigen::Vector3d> common_sats_pos_r, common_sats_pos_k;
- computeSatellitesPositions(common_obs_r, nav_r, opt, common_sats_pos_r);
- computeSatellitesPositions(common_obs_k, sd_params.use_old_nav ? nav_r : nav_k, opt, common_sats_pos_k);
-
- // FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR)
- filterCommonSatellites(common_obs_r, common_sats_pos_r,
- common_obs_k, common_sats_pos_k,
- discarded_sats, x_r,
- sd_params, opt);
-
- // COMPUTE SINGLE DIFFERENCES
- return TDCP(common_obs_r, nav_r, common_sats_pos_r, x_r,
- common_obs_k, nav_k, common_sats_pos_k,
- d, cov_d,
- residual, discarded_sats,
- sd_params);
+ // FIND COMMON SATELLITES OBSERVATIONS
+ Observations common_obs_r, common_obs_k;
+ Observations::findCommonObservations(obs_r, obs_k, common_obs_r, common_obs_k);
+ int n_common_sats = common_obs_r.getObservations().size();
+
+ // COMPUTE COMMON SATELLITES POSITION
+ std::map<int, Eigen::Vector3d> common_sats_pos_r, common_sats_pos_k;
+ computeSatellitesPositions(common_obs_r, nav_r, opt, common_sats_pos_r);
+ computeSatellitesPositions(common_obs_k, sd_params.use_old_nav ? nav_r : nav_k, opt, common_sats_pos_k);
+
+ // FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR)
+ filterCommonSatellites(
+ common_obs_r, common_sats_pos_r, common_obs_k, common_sats_pos_k, discarded_sats, x_r, sd_params, opt);
+
+ // COMPUTE SINGLE DIFFERENCES
+ return TDCP(common_obs_r,
+ nav_r,
+ common_sats_pos_r,
+ x_r,
+ common_obs_k,
+ nav_k,
+ common_sats_pos_k,
+ d,
+ cov_d,
+ residual,
+ discarded_sats,
+ sd_params);
}
-bool TDCP(const Observations& common_obs_r, const Navigation& nav_r, const std::map<int,Eigen::Vector3d>& common_sats_pos_r, const Eigen::Vector3d& x_r,
- const Observations& common_obs_k, const Navigation& nav_k, const std::map<int,Eigen::Vector3d>& common_sats_pos_k,
- Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
- double& residual, std::set<int>& discarded_sats,
- const TDCPParams& sd_params)
+bool TDCP(const Observations& common_obs_r,
+ const Navigation& nav_r,
+ const std::map<int, Eigen::Vector3d>& common_sats_pos_r,
+ const Eigen::Vector3d& x_r,
+ const Observations& common_obs_k,
+ const Navigation& nav_k,
+ const std::map<int, Eigen::Vector3d>& common_sats_pos_k,
+ Eigen::Vector4d& d,
+ Eigen::Matrix4d& cov_d,
+ double& residual,
+ std::set<int>& discarded_sats,
+ const TDCPParams& sd_params)
{
- assert(common_obs_r.size() == common_obs_k.size());
- assert(common_obs_r.size() == common_sats_pos_r.size());
- assert(common_obs_k.size() == common_sats_pos_k.size());
+ assert(common_obs_r.size() == common_obs_k.size());
+ assert(common_obs_r.size() == common_sats_pos_r.size());
+ assert(common_obs_k.size() == common_sats_pos_k.size());
+
+ double tr(common_obs_r.getObservations().front().time.time + common_obs_r.getObservations().front().time.sec);
+ double tk(common_obs_k.getObservations().front().time.time + common_obs_k.getObservations().front().time.sec);
- double tr(common_obs_r.getObservations().front().time.time + common_obs_r.getObservations().front().time.sec);
- double tk(common_obs_k.getObservations().front().time.time + common_obs_k.getObservations().front().time.sec);
+ int required_n_sats(std::max(sd_params.min_common_sats + sd_params.raim_n, 4 + sd_params.raim_n));
+ int n_common_sats = common_sats_pos_k.size();
- int required_n_sats(std::max(sd_params.min_common_sats + sd_params.raim_n, 4 + sd_params.raim_n));
- int n_common_sats = common_sats_pos_k.size();
+ if (n_common_sats < required_n_sats)
+ {
+#if GNSS_UTILS_TDCP_DEBUG == 1
+ printf("TDCP: NOT ENOUGH COMMON SATS");
+ printf("TDCP: %i common sats available, %i sats are REQUIRED. [ SKIPPING TDCP ]", n_common_sats, required_n_sats);
+#endif
+ return false;
+ }
- if(n_common_sats < required_n_sats)
+ // MULTI-FREQUENCY
+ std::map<int, std::pair<int, int>> row_2_sat_freq;
+ int row = 0;
+ for (auto obs_idx = 0; obs_idx < common_obs_k.size(); obs_idx++)
+ {
+ auto&& obs_r = common_obs_r.getObservationByIdx(obs_idx);
+ auto&& obs_k = common_obs_k.getObservationByIdx(obs_idx);
+
+ const int& sat_number = obs_k.sat;
+
+ if (sd_params.use_carrier_phase)
{
- #if GNSS_UTILS_TDCP_DEBUG == 1
- printf("TDCP: NOT ENOUGH COMMON SATS");
- printf("TDCP: %i common sats available, %i sats are REQUIRED. [ SKIPPING TDCP ]",
- n_common_sats,
- required_n_sats);
- #endif
- return false;
+ // L1/E1
+ if (std::abs(obs_r.L[0]) > 1e-12 and std::abs(obs_k.L[0]) > 1e-12)
+ {
+ row_2_sat_freq[row] = std::make_pair(sat_number, 0);
+ row++;
+ }
+ if (!sd_params.use_multi_freq)
+ continue;
+
+ // L2 (GPS/GLO/QZS)
+ int sys = satsys(sat_number, NULL);
+ if (NFREQ >= 2 and (sys & (SYS_GPS | SYS_GLO | SYS_QZS)) and std::abs(obs_r.L[1]) > 1e-12 and
+ std::abs(obs_k.L[1]) > 1e-12)
+ {
+ row_2_sat_freq[row] = std::make_pair(sat_number, 1);
+ row++;
+ }
+ // E5 (GAL)
+ else if (NFREQ >= 3 and (sys & SYS_GAL) and std::abs(obs_r.L[2]) > 1e-12 and std::abs(obs_k.L[2]) > 1e-12)
+ {
+ row_2_sat_freq[row] = std::make_pair(sat_number, 2);
+ row++;
+ }
}
-
- // MULTI-FREQUENCY
- std::map<int,std::pair<int,int>> row_2_sat_freq;
- int row = 0;
- for (auto obs_idx = 0; obs_idx < common_obs_k.size(); obs_idx++)
+ else
+ // L1/E1
+ if (std::abs(obs_r.P[0]) > 1e-12 and std::abs(obs_k.P[0]) > 1e-12)
{
- auto&& obs_r = common_obs_r.getObservationByIdx(obs_idx);
- auto&& obs_k = common_obs_k.getObservationByIdx(obs_idx);
+ row_2_sat_freq[row] = std::make_pair(sat_number, 0);
+ row++;
+ }
+ }
+ int n_differences = row_2_sat_freq.size();
- const int& sat_number = obs_k.sat;
+ // Initial guess
+ Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
- if(sd_params.use_carrier_phase)
- {
- // L1/E1
- if (std::abs(obs_r.L[0]) > 1e-12 and
- std::abs(obs_k.L[0]) > 1e-12)
- {
- row_2_sat_freq[row] = std::make_pair(sat_number,0);
- row++;
- }
- if (!sd_params.use_multi_freq)
- continue;
-
- // L2 (GPS/GLO/QZS)
- int sys=satsys(sat_number,NULL);
- if (NFREQ >= 2 and (sys&(SYS_GPS|SYS_GLO|SYS_QZS)) and
- std::abs(obs_r.L[1]) > 1e-12 and
- std::abs(obs_k.L[1]) > 1e-12)
- {
- row_2_sat_freq[row] = std::make_pair(sat_number,1);
- row++;
- }
- // E5 (GAL)
- else if (NFREQ >= 3 and (sys&SYS_GAL) and
- std::abs(obs_r.L[2]) > 1e-12 and
- std::abs(obs_k.L[2]) > 1e-12)
- {
- row_2_sat_freq[row] = std::make_pair(sat_number,2);
- row++;
- }
- }
- else
- // L1/E1
- if (std::abs(obs_r.P[0]) > 1e-12 and
- std::abs(obs_k.P[0]) > 1e-12)
- {
- row_2_sat_freq[row] = std::make_pair(sat_number,0);
- row++;
- }
+#if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
+ std::cout << "d initial guess: " << d_0.transpose() << std::endl;
+ std::cout << "common sats: ";
+ for (auto row_sat_freq_it : row_2_sat_freq)
+ std::cout << row_sat_freq_it.second.first << " ";
+ std::cout << std::endl;
+ std::cout << "discarded_sats: ";
+ for (auto sat : discarded_sats)
+ std::cout << sat << " ";
+ std::cout << std::endl;
+#endif
+
+ // FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
+ Eigen::Matrix<double, Eigen::Dynamic, 4> A(Eigen::Matrix<double, Eigen::Dynamic, 4>::Zero(n_differences, 4));
+ Eigen::VectorXd r(Eigen::VectorXd::Zero(n_differences));
+ Eigen::VectorXd drho_m(Eigen::VectorXd::Zero(n_differences));
+ Eigen::Matrix<double, 3, Eigen::Dynamic> s_k(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
+ Eigen::Matrix<double, 3, Eigen::Dynamic> s_r(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
+ int sat_i = 0;
+
+ for (auto row_sat_freq_it : row_2_sat_freq)
+ {
+ const int& row = row_sat_freq_it.first;
+ const int& sat_number = row_sat_freq_it.second.first;
+ const int& freq = row_sat_freq_it.second.second;
+
+ auto obs_r = common_obs_r.getObservationBySat(sat_number);
+ auto obs_k = common_obs_k.getObservationBySat(sat_number);
+
+ // excluded satellite
+ if (discarded_sats.count(sat_number) != 0)
+ {
+#if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "\tdiscarded sat" << std::endl;
+#endif
+ continue;
}
- int n_differences = row_2_sat_freq.size();
- // Initial guess
- Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
+ // Satellite's positions
+ s_r.col(row) = common_sats_pos_r.at(sat_number);
+ s_k.col(row) = common_sats_pos_k.at(sat_number);
+ nav_k.getNavigation().ion_gps;
- #if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
- std::cout << "d initial guess: " << d_0.transpose() << std::endl;
- std::cout << "common sats: ";
- for (auto row_sat_freq_it : row_2_sat_freq)
- std::cout << row_sat_freq_it.second.first << " ";
- std::cout << std::endl;
- std::cout << "discarded_sats: ";
- for (auto sat : discarded_sats)
- std::cout << sat << " ";
- std::cout << std::endl;
- #endif
+ if (sd_params.use_carrier_phase) // TODO: add iono and tropo corrections (optionally)
+ drho_m(row) = (obs_k.L[freq] * nav_k.getNavigation().lam[sat_number - 1][freq]) -
+ (obs_r.L[freq] * nav_r.getNavigation().lam[sat_number - 1][freq]);
+ else
+ drho_m(row) = obs_k.P[freq] - obs_r.P[freq];
- // FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
- Eigen::Matrix<double, Eigen::Dynamic, 4> A(Eigen::Matrix<double, Eigen::Dynamic, 4>::Zero(n_differences,4));
- Eigen::VectorXd r(Eigen::VectorXd::Zero(n_differences));
- Eigen::VectorXd drho_m(Eigen::VectorXd::Zero(n_differences));
- Eigen::Matrix<double, 3, Eigen::Dynamic> s_k(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3,n_differences));
- Eigen::Matrix<double, 3, Eigen::Dynamic> s_r(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3,n_differences));
- int sat_i = 0;
+#if GNSS_UTILS_TDCP_DEBUG == 1
+ // std::cout << "\tsat " << sat_number << " frequency " << freq << " wavelength = " <<
+ // nav_r.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\tpositions:\n\t\tt_r: " <<
+ // s_r.col(row).transpose() << "\n\t\tt_k: " << s_k.col(row).transpose() << std::endl; std::cout << "\tobs_r.L: "
+ // << obs_r.L[freq] << std::endl; std::cout << "\tobs_k.L: " << obs_k.L[freq] << std::endl; std::cout <<
+ // "\tnav_r.getNavigation().lam[sat_number-1][freq]: " << nav_r.getNavigation().lam[sat_number-1][freq] <<
+ // std::endl; std::cout << "\tnav_k.getNavigation().lam[sat_number-1][freq]: " <<
+ // nav_k.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\trho_r: " << obs_r.L[freq] *
+ // nav_r.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\trho_k: " << obs_k.L[freq] *
+ // nav_k.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\tdrho_m: " << drho_m(row) <<
+ // std::endl;
+#endif
- for (auto row_sat_freq_it : row_2_sat_freq)
+ if (!sd_params.relinearize_jacobian)
{
- const int& row = row_sat_freq_it.first;
- const int& sat_number = row_sat_freq_it.second.first;
- const int& freq = row_sat_freq_it.second.second;
+ // Unit vectors from receivers to satellites
+ Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
+
+ // Fill Jacobian matrix
+ A(row, 0) = u_k(0);
+ A(row, 1) = u_k(1);
+ A(row, 2) = u_k(2);
+ A(row, 3) = -1.0;
+ }
+ }
- auto obs_r = common_obs_r.getObservationBySat(sat_number);
- auto obs_k = common_obs_k.getObservationBySat(sat_number);
+ // LEAST SQUARES SOLUTION =======================================================================
+ for (int j = 0; j < sd_params.max_iterations; j++)
+ {
+ // fill A and r
+ for (int row = 0; row < A.rows(); row++)
+ {
+ // Evaluate r
+ r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
+
+ // Evaluate A
+ if (sd_params.relinearize_jacobian)
+ {
+ // Unit vectors from rcvrs to sats
+ Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
+
+ // Fill Jacobian matrix
+ A(row, 0) = u_k(0);
+ A(row, 1) = u_k(1);
+ A(row, 2) = u_k(2);
+ A(row, 3) = -1.0;
+ }
+ }
- // excluded satellite
- if (discarded_sats.count(sat_number) != 0)
- {
- #if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "\tdiscarded sat" << std::endl;
- #endif
- continue;
- }
+ // Solve
+ cov_d = (A.transpose() * A).inverse();
+ delta_d = -cov_d * A.transpose() * r;
+ d += delta_d;
- // Satellite's positions
- s_r.col(row) = common_sats_pos_r.at(sat_number);
- s_k.col(row) = common_sats_pos_k.at(sat_number);
- nav_k.getNavigation().ion_gps;
-
- if(sd_params.use_carrier_phase) // TODO: add iono and tropo corrections (optionally)
- drho_m(row) = (obs_k.L[freq] * nav_k.getNavigation().lam[sat_number-1][freq]) -
- (obs_r.L[freq] * nav_r.getNavigation().lam[sat_number-1][freq]);
- else
- drho_m(row) = obs_k.P[freq] - obs_r.P[freq];
-
- #if GNSS_UTILS_TDCP_DEBUG == 1
- //std::cout << "\tsat " << sat_number << " frequency " << freq << " wavelength = " << nav_r.getNavigation().lam[sat_number-1][freq] << std::endl;
- //std::cout << "\tpositions:\n\t\tt_r: " << s_r.col(row).transpose() << "\n\t\tt_k: " << s_k.col(row).transpose() << std::endl;
- //std::cout << "\tobs_r.L: " << obs_r.L[freq] << std::endl;
- //std::cout << "\tobs_k.L: " << obs_k.L[freq] << std::endl;
- //std::cout << "\tnav_r.getNavigation().lam[sat_number-1][freq]: " << nav_r.getNavigation().lam[sat_number-1][freq] << std::endl;
- //std::cout << "\tnav_k.getNavigation().lam[sat_number-1][freq]: " << nav_k.getNavigation().lam[sat_number-1][freq] << std::endl;
- //std::cout << "\trho_r: " << obs_r.L[freq] * nav_r.getNavigation().lam[sat_number-1][freq] << std::endl;
- //std::cout << "\trho_k: " << obs_k.L[freq] * nav_k.getNavigation().lam[sat_number-1][freq] << std::endl;
- //std::cout << "\tdrho_m: " << drho_m(row) << std::endl;
- #endif
-
- if(!sd_params.relinearize_jacobian)
- {
- // Unit vectors from receivers to satellites
- Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
-
- // Fill Jacobian matrix
- A(row, 0) = u_k(0);
- A(row, 1) = u_k(1);
- A(row, 2) = u_k(2);
- A(row, 3) = -1.0;
- }
+ // wrong solution
+ if (d.array().isNaN().any())
+ {
+ std::cout << "TDCP: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
+ return false;
}
- // LEAST SQUARES SOLUTION =======================================================================
- for (int j = 0; j < sd_params.max_iterations; j++)
+ // residual
+ // residual = sqrt((r - A * delta_d).squaredNorm() / A.rows());
+ residual = (r + A * delta_d).norm();
+ // std::cout << "residual = " << residual << std::endl;
+ // std::cout << "TDCP2: r-A*delta_d = " << (r + A * delta_d).transpose() << std::endl;
+
+#if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl;
+ std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
+ printf("Ref distance = %7.3f m\n", d_0.norm());
+ printf("Computed distance = %7.3f m\n", d.head<3>().norm());
+ printf("TDCP: residual = %13.10f\n", residual);
+ printf("TDCP: row = %i\n", r.rows());
+ std::cout << "TDCP: drho = " << r.transpose() << std::endl;
+ std::cout << "TDCP: drho_m = " << drho_m.transpose() << std::endl;
+ std::cout << "TDCP: H = \n" << A << std::endl;
+ printf("TDCP: dT = %8.3f secs\n", d(3));
+#endif
+
+ // RAIM ====================================== (only at first iteration) RAIM WITH SATELLITES, not ROWS
+ int input_discarded_sats = discarded_sats.size();
+ if (j == 0 and sd_params.raim_n > 0 and residual > sd_params.raim_min_residual)
{
- // fill A and r
- for (int row = 0; row < A.rows(); row++)
+ int worst_sat_row = -1;
+ double best_residual = 1e12;
+ Eigen::Vector4d best_d;
+ int n_removed_rows = 1;
+
+ // remove some satellites
+ while (discarded_sats.size() - input_discarded_sats < sd_params.raim_n)
+ {
+ auto A_raim = A;
+ auto r_raim = r;
+
+ // solve removing each satellite
+ for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
{
- // Evaluate r
- r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
+ int sat_removed = row_2_sat_freq.at(row_removed).first;
+
+ // Multi-freq: some rows for the same satellite
+ n_removed_rows = 1;
+ if (sd_params.use_multi_freq)
+ while (row_removed + n_removed_rows < A_raim.rows() and
+ row_2_sat_freq.at(row_removed + n_removed_rows).first == sat_removed)
+ n_removed_rows++;
+
+ // remove satellite rows
+ for (auto r = 0; r < n_removed_rows; r++)
+ {
+ A_raim.row(row_removed + r) = Eigen::Vector4d::Zero().transpose();
+ r_raim(row_removed + r) = 0; // not necessary
+ }
+
+ // solve
+ Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
+ Eigen::Vector4d d_raim = d_0 + delta_d_raim;
+
+ // no NaN
+ if (!d_raim.array().isNaN().any())
+ {
+ // residual
+ residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
+ // residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
- // Evaluate A
- if (sd_params.relinearize_jacobian)
+#if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "RAIM excluding sat " << sat_removed << " - row " << row_removed << "(n_rows "
+ << n_removed_rows << ")" << std::endl;
+ std::cout << "Displacement vector =" << d_raim.head<3>().transpose() << std::endl;
+ std::cout << "Displacement update =" << delta_d_raim.head<3>().transpose() << std::endl;
+ printf("Ref distance = %7.3f m\n", d_0.norm());
+ printf("Computed distance = %7.3f m\n", d_raim.head<3>().norm());
+ printf("TDCP: residual = %13.10f\n", residual);
+ std::cout << "TDCP: drho = " << r_raim.transpose() << std::endl;
+ std::cout << "TDCP: H = \n" << A_raim << std::endl;
+ printf("TDCP: dT = %8.3f secs\n", d_raim(3));
+#endif
+ // store if best residual
+ if (residual < best_residual)
{
- // Unit vectors from rcvrs to sats
- Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
-
- // Fill Jacobian matrix
- A(row, 0) = u_k(0);
- A(row, 1) = u_k(1);
- A(row, 2) = u_k(2);
- A(row, 3) = -1.0;
+ worst_sat_row = row_removed;
+ best_residual = residual;
+ best_d = d_raim;
}
+ }
+ // restore initial A and r
+ for (auto row = 0; row < n_removed_rows; row++)
+ {
+ A_raim.row(row_removed + row) = A.row(row_removed + row);
+ r_raim(row_removed + row) = r(row_removed + row);
+ }
+ row_removed += (n_removed_rows - 1);
}
- // Solve
- cov_d = (A.transpose()*A).inverse();
- delta_d = -cov_d*A.transpose()*r;
- d += delta_d;
+ // No successful RAIM solution
+ if (worst_sat_row == -1)
+ {
+ printf("TDCP: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
+ return false;
+ }
- // wrong solution
- if(d.array().isNaN().any())
+ // store removed sat
+ discarded_sats.insert(row_2_sat_freq[worst_sat_row].first);
+
+ // decrease n_common_sats
+ n_differences -= n_removed_rows;
+ n_common_sats--;
+
+ // Remove selected satellite from problem
+ std::cout << "resizing problem..." << std::endl;
+ auto A_aux = A;
+ auto r_aux = r;
+ auto drho_m_aux = drho_m;
+ auto s_r_aux = s_r;
+ auto s_k_aux = s_k;
+ A.conservativeResize(n_differences, Eigen::NoChange);
+ r.conservativeResize(n_differences);
+ drho_m.conservativeResize(n_differences);
+ s_r.conservativeResize(Eigen::NoChange, n_differences);
+ s_k.conservativeResize(Eigen::NoChange, n_differences);
+
+ int back_elements_changing = A_aux.rows() - worst_sat_row - n_removed_rows;
+ if (back_elements_changing != 0) // if last satelite removed, conservative resize did the job
{
- std::cout << "TDCP: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
- return false;
+ A.bottomRows(back_elements_changing) = A_aux.bottomRows(back_elements_changing);
+ s_r.rightCols(back_elements_changing) = s_r_aux.rightCols(back_elements_changing);
+ s_k.rightCols(back_elements_changing) = s_k_aux.rightCols(back_elements_changing);
+ r.tail(back_elements_changing) = r_aux.tail(back_elements_changing);
+ drho_m.tail(back_elements_changing) = drho_m_aux.tail(back_elements_changing);
}
- // residual
- //residual = sqrt((r - A * delta_d).squaredNorm() / A.rows());
- residual = (r + A * delta_d).norm();
- //std::cout << "residual = " << residual << std::endl;
- //std::cout << "TDCP2: r-A*delta_d = " << (r + A * delta_d).transpose() << std::endl;
+ // apply the RAIM solution
+ d = best_d;
+ cov_d = (A.transpose() * A).inverse();
#if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl;
- std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
- printf("Ref distance = %7.3f m\n", d_0.norm());
- printf("Computed distance = %7.3f m\n", d.head<3>().norm());
- printf("TDCP: residual = %13.10f\n", residual);
- printf("TDCP: row = %i\n", r.rows());
- std::cout << "TDCP: drho = " << r.transpose() << std::endl;
- std::cout << "TDCP: drho_m = " << drho_m.transpose() << std::endl;
- std::cout << "TDCP: H = \n" << A << std::endl;
- printf("TDCP: dT = %8.3f secs\n", d(3));
+ std::cout << "TDCP After RAIM iteration" << std::endl;
+ std::cout << "\tExcluded sats : ";
+ for (auto dsat : discarded_sats)
+ std::cout << (int)dsat << " ";
+ std::cout << std::endl;
+ std::cout << "\tCommon sats : " << n_common_sats << std::endl;
+ std::cout << "\tRows : " << n_differences << std::endl;
+ std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tClock offset = " << d(3) << std::endl;
+ std::cout << "\tResidual = " << best_residual << std::endl;
#endif
-
-
- // RAIM ====================================== (only at first iteration) RAIM WITH SATELLITES, not ROWS
- int input_discarded_sats = discarded_sats.size();
- if(j==0 and sd_params.raim_n > 0 and residual > sd_params.raim_min_residual)
- {
- int worst_sat_row = -1;
- double best_residual = 1e12;
- Eigen::Vector4d best_d;
- int n_removed_rows = 1;
-
- // remove some satellites
- while(discarded_sats.size() - input_discarded_sats < sd_params.raim_n)
- {
-
- auto A_raim = A;
- auto r_raim = r;
-
- // solve removing each satellite
- for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
- {
- int sat_removed = row_2_sat_freq.at(row_removed).first;
-
- // Multi-freq: some rows for the same satellite
- n_removed_rows = 1;
- if (sd_params.use_multi_freq)
- while (row_removed + n_removed_rows < A_raim.rows() and row_2_sat_freq.at(row_removed+n_removed_rows).first == sat_removed)
- n_removed_rows++;
-
- // remove satellite rows
- for (auto r = 0; r < n_removed_rows; r++)
- {
- A_raim.row(row_removed+r) = Eigen::Vector4d::Zero().transpose();
- r_raim(row_removed+r) = 0; // not necessary
- }
-
- // solve
- Eigen::Vector4d delta_d_raim = -(A_raim.transpose()*A_raim).inverse()*A_raim.transpose()*r_raim;
- Eigen::Vector4d d_raim = d_0+delta_d_raim;
-
-
- // no NaN
- if (!d_raim.array().isNaN().any())
- {
- // residual
- residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows()-1);
- //residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
+ }
#if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "RAIM excluding sat " << sat_removed << " - row " << row_removed << "(n_rows " << n_removed_rows << ")" << std::endl;
- std::cout << "Displacement vector =" << d_raim.head<3>().transpose() << std::endl;
- std::cout << "Displacement update =" << delta_d_raim.head<3>().transpose() << std::endl;
- printf("Ref distance = %7.3f m\n", d_0.norm());
- printf("Computed distance = %7.3f m\n", d_raim.head<3>().norm());
- printf("TDCP: residual = %13.10f\n", residual);
- std::cout << "TDCP: drho = " << r_raim.transpose() << std::endl;
- std::cout << "TDCP: H = \n" << A_raim << std::endl;
- printf("TDCP: dT = %8.3f secs\n", d_raim(3));
+ std::cout << "TDCP After RAIM " << std::endl;
+ std::cout << "\tExcluded sats : ";
+ for (auto dsat : discarded_sats)
+ std::cout << (int)dsat << " ";
+ std::cout << std::endl;
+ std::cout << "\tCommon sats : " << n_common_sats << std::endl;
+ std::cout << "\tRows : " << n_differences << std::endl;
+ std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tClock offset = " << d(3) << std::endl;
+ std::cout << "\tResidual = " << best_residual << std::endl;
#endif
- // store if best residual
- if (residual < best_residual)
- {
- worst_sat_row = row_removed;
- best_residual = residual;
- best_d = d_raim;
- }
- }
- // restore initial A and r
- for (auto row = 0; row < n_removed_rows; row++)
- {
- A_raim.row(row_removed+row) = A.row(row_removed+row);
- r_raim(row_removed+row) = r(row_removed+row);
- }
- row_removed += (n_removed_rows-1);
- }
-
- // No successful RAIM solution
- if(worst_sat_row == -1)
- {
- printf("TDCP: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
- return false;
- }
-
-
- // store removed sat
- discarded_sats.insert(row_2_sat_freq[worst_sat_row].first);
-
- // decrease n_common_sats
- n_differences-=n_removed_rows;
- n_common_sats--;
-
- // Remove selected satellite from problem
- std::cout << "resizing problem..." << std::endl;
- auto A_aux = A;
- auto r_aux = r;
- auto drho_m_aux = drho_m;
- auto s_r_aux = s_r;
- auto s_k_aux = s_k;
- A.conservativeResize(n_differences, Eigen::NoChange);
- r.conservativeResize(n_differences);
- drho_m.conservativeResize(n_differences);
- s_r.conservativeResize(Eigen::NoChange, n_differences);
- s_k.conservativeResize(Eigen::NoChange, n_differences);
-
- int back_elements_changing = A_aux.rows() - worst_sat_row - n_removed_rows;
- if (back_elements_changing != 0) //if last satelite removed, conservative resize did the job
- {
- A .bottomRows(back_elements_changing) = A_aux .bottomRows(back_elements_changing);
- s_r .rightCols(back_elements_changing) = s_r_aux .rightCols(back_elements_changing);
- s_k .rightCols(back_elements_changing) = s_k_aux .rightCols(back_elements_changing);
- r .tail(back_elements_changing) = r_aux .tail(back_elements_changing);
- drho_m .tail(back_elements_changing) = drho_m_aux.tail(back_elements_changing);
- }
-
- // apply the RAIM solution
- d = best_d;
- cov_d = (A.transpose()*A).inverse();
-
- #if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "TDCP After RAIM iteration" << std::endl;
- std::cout << "\tExcluded sats : ";
- for (auto dsat : discarded_sats)
- std::cout << (int)dsat << " ";
- std::cout << std::endl;
- std::cout << "\tCommon sats : " << n_common_sats << std::endl;
- std::cout << "\tRows : " << n_differences << std::endl;
- std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
- std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
- std::cout << "\tClock offset = " << d(3) << std::endl;
- std::cout << "\tResidual = " << best_residual << std::endl;
- #endif
- }
-
- #if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "TDCP After RAIM " << std::endl;
- std::cout << "\tExcluded sats : ";
- for (auto dsat : discarded_sats)
- std::cout << (int)dsat << " ";
- std::cout << std::endl;
- std::cout << "\tCommon sats : " << n_common_sats << std::endl;
- std::cout << "\tRows : " << n_differences << std::endl;
- std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
- std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
- std::cout << "\tClock offset = " << d(3) << std::endl;
- std::cout << "\tResidual = " << best_residual << std::endl;
- #endif
- }
-
- #if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "TDCP iteration " << j << std::endl;
- std::cout << "\tExcluded sats : ";
- for (auto dsat : discarded_sats)
- std::cout << (int)dsat << " ";
- std::cout << std::endl;
- std::cout << "\tCommon sats : " << n_common_sats << std::endl;
- std::cout << "\tRows : " << n_differences << std::endl;
- std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
- std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
- std::cout << "\tClock offset = " << d(3) << std::endl;
- std::cout << "\tResidual = " << residual << std::endl;
- #endif
}
- // weight covariance with the measurement noise (proportional to time)
- double sq_sigma_TDCP = (tk-tr) * sd_params.sigma_atm * sd_params.sigma_atm +
- 2 * (sd_params.use_carrier_phase ?
- sd_params.sigma_carrier * sd_params.sigma_carrier :
- sd_params.sigma_code * sd_params.sigma_code);
- cov_d *= sq_sigma_TDCP;
- //residual = (r - A * d).norm() / A.rows();
-
- // Computing error on measurement space
- for(int row = 0; row < n_differences; row++)
- {
- r(row) = drho_m(row)
- - (s_k.col(row) - x_r - d.head(3)).norm()
- + (s_r.col(row) - x_r).norm()
- - d(3);
- }
- residual = sqrt(r.squaredNorm() / r.size());
-
- return true;
+#if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "TDCP iteration " << j << std::endl;
+ std::cout << "\tExcluded sats : ";
+ for (auto dsat : discarded_sats)
+ std::cout << (int)dsat << " ";
+ std::cout << std::endl;
+ std::cout << "\tCommon sats : " << n_common_sats << std::endl;
+ std::cout << "\tRows : " << n_differences << std::endl;
+ std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tClock offset = " << d(3) << std::endl;
+ std::cout << "\tResidual = " << residual << std::endl;
+#endif
+ }
+
+ // weight covariance with the measurement noise (proportional to time)
+ double sq_sigma_TDCP = (tk - tr) * sd_params.sigma_atm * sd_params.sigma_atm +
+ 2 * (sd_params.use_carrier_phase ? sd_params.sigma_carrier * sd_params.sigma_carrier :
+ sd_params.sigma_code * sd_params.sigma_code);
+ cov_d *= sq_sigma_TDCP;
+ // residual = (r - A * d).norm() / A.rows();
+
+ // Computing error on measurement space
+ for (int row = 0; row < n_differences; row++)
+ {
+ r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
+ }
+ residual = sqrt(r.squaredNorm() / r.size());
+
+ return true;
}
-void filterCommonSatellites(Observations& common_obs_r, std::map<int,Eigen::Vector3d>& common_sats_pos_r,
- Observations& common_obs_k, std::map<int,Eigen::Vector3d>& common_sats_pos_k,
- std::set<int>& discarded_sats, const Eigen::Vector3d& x_r,
- const TDCPParams& sd_params, const prcopt_t& opt)
+void filterCommonSatellites(Observations& common_obs_r,
+ std::map<int, Eigen::Vector3d>& common_sats_pos_r,
+ Observations& common_obs_k,
+ std::map<int, Eigen::Vector3d>& common_sats_pos_k,
+ std::set<int>& discarded_sats,
+ const Eigen::Vector3d& x_r,
+ const TDCPParams& sd_params,
+ const prcopt_t& opt)
{
- assert(&common_obs_r != &common_obs_k);
- assert(&common_sats_pos_r != &common_sats_pos_k);
- assert(common_obs_r.size() == common_obs_k.size());
- assert(common_obs_r.size() == common_sats_pos_r.size());
- assert(common_obs_r.size() == common_sats_pos_k.size());
-
- //std::cout << "filterCommonSatellites: initial size: " << common_obs_k.size() << std::endl;
+ assert(&common_obs_r != &common_obs_k);
+ assert(&common_sats_pos_r != &common_sats_pos_k);
+ assert(common_obs_r.size() == common_obs_k.size());
+ assert(common_obs_r.size() == common_sats_pos_r.size());
+ assert(common_obs_r.size() == common_sats_pos_k.size());
- std::set<int> remove_sats;
+ // std::cout << "filterCommonSatellites: initial size: " << common_obs_k.size() << std::endl;
- for (int obs_i = 0; obs_i < common_obs_r.size(); obs_i++)
- {
- auto&& obs_r = common_obs_r.getObservationByIdx(obs_i);
- auto&& obs_k = common_obs_k.getObservationByIdx(obs_i);
- assert(obs_r.sat == obs_k.sat && "common satellites observations have to be ordered equally");
- const int& sat_number = obs_r.sat;
+ std::set<int> remove_sats;
- // already discarded sats
- if (discarded_sats.count(sat_number) != 0)
- {
- remove_sats.insert(sat_number);
- continue;
- }
+ for (int obs_i = 0; obs_i < common_obs_r.size(); obs_i++)
+ {
+ auto&& obs_r = common_obs_r.getObservationByIdx(obs_i);
+ auto&& obs_k = common_obs_k.getObservationByIdx(obs_i);
+ assert(obs_r.sat == obs_k.sat && "common satellites observations have to be ordered equally");
+ const int& sat_number = obs_r.sat;
- // wrong data (satellite is not included in the discarded list)
- if (sd_params.use_carrier_phase and (std::abs(obs_r.L[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
- {
- // std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.L[0] << " " << obs_k.L[0] << std::endl;
- remove_sats.insert(sat_number);
- continue;
- }
- if (!sd_params.use_carrier_phase and (std::abs(obs_r.P[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
- {
- // std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.P[0] << " " << obs_k.P[0] << std::endl;
- remove_sats.insert(sat_number);
- continue;
- }
-
- // bad satellite position (satellite is not included in the discarded list)
- if (common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3) or
- common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3))
- {
- // std::cout << "Discarding sat " << sat_number << ": bad satellite position: \n\t" << common_sats_pos_r[sat_number].transpose() << "\n\t" << common_sats_pos_k[sat_number].transpose() << std::endl;
- remove_sats.insert(sat_number);
- continue;
- }
-
- // constellation
- int sys=satsys(obs_r.sat,NULL);
- if (!(sys & opt.navsys))
- {
- // std::cout << "Discarding sat " << sat_number << ": not selected constellation " << sys << " - mask: " << opt.navsys << std::endl;
- discarded_sats.insert(sat_number);
- remove_sats.insert(sat_number);
- continue;
- }
-
- // check both elevations
- double elevation = std::min(computeSatElevation(x_r, common_sats_pos_r[sat_number]),
- computeSatElevation(x_r, common_sats_pos_k[sat_number]));
- if (elevation < opt.elmin)
- {
- // std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << opt.elmin << std::endl;
- discarded_sats.insert(sat_number);
- remove_sats.insert(sat_number);
- continue;
- }
+ // already discarded sats
+ if (discarded_sats.count(sat_number) != 0)
+ {
+ remove_sats.insert(sat_number);
+ continue;
+ }
- // snr TODO: multifrequency (2nd param and 3rd idx)
- if (testsnr(0, 0, elevation, obs_r.SNR[0] * 0.25, &opt.snrmask) == 1)
- {
- // std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl;
- discarded_sats.insert(sat_number);
- remove_sats.insert(sat_number);
- continue;
- }
+ // wrong data (satellite is not included in the discarded list)
+ if (sd_params.use_carrier_phase and (std::abs(obs_r.L[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
+ {
+ // std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.L[0] << " " <<
+ // obs_k.L[0] << std::endl;
+ remove_sats.insert(sat_number);
+ continue;
+ }
+ if (!sd_params.use_carrier_phase and (std::abs(obs_r.P[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
+ {
+ // std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.P[0] << " " <<
+ // obs_k.P[0] << std::endl;
+ remove_sats.insert(sat_number);
+ continue;
}
- // remove sats
- //std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
- for (auto sat : remove_sats)
+ // bad satellite position (satellite is not included in the discarded list)
+ if (common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3) or
+ common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3))
{
- assert(common_obs_r.hasSatellite(sat));
- assert(common_obs_k.hasSatellite(sat));
- assert(common_sats_pos_r.count(sat));
- assert(common_sats_pos_k.count(sat));
- common_obs_r.removeObservationBySat(sat);
- common_obs_k.removeObservationBySat(sat);
- common_sats_pos_r.erase(sat);
- common_sats_pos_k.erase(sat);
+ // std::cout << "Discarding sat " << sat_number << ": bad satellite position: \n\t" <<
+ // common_sats_pos_r[sat_number].transpose() << "\n\t" << common_sats_pos_k[sat_number].transpose() << std::endl;
+ remove_sats.insert(sat_number);
+ continue;
}
- assert(common_obs_r.size() == common_obs_k.size());
- assert(common_obs_r.size() == common_sats_pos_r.size());
- assert(common_obs_r.size() == common_sats_pos_k.size());
+ // constellation
+ int sys = satsys(obs_r.sat, NULL);
+ if (!(sys & opt.navsys))
+ {
+ // std::cout << "Discarding sat " << sat_number << ": not selected constellation " << sys << " - mask: " <<
+ // opt.navsys << std::endl;
+ discarded_sats.insert(sat_number);
+ remove_sats.insert(sat_number);
+ continue;
+ }
- //std::cout << "final size: " << common_obs_k.size() << std::endl;
-}
+ // check both elevations
+ double elevation = std::min(computeSatElevation(x_r, common_sats_pos_r[sat_number]),
+ computeSatElevation(x_r, common_sats_pos_k[sat_number]));
+ if (elevation < opt.elmin)
+ {
+ // std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << opt.elmin <<
+ // std::endl;
+ discarded_sats.insert(sat_number);
+ remove_sats.insert(sat_number);
+ continue;
+ }
+ // snr TODO: multifrequency (2nd param and 3rd idx)
+ if (testsnr(0, 0, elevation, obs_r.SNR[0] * 0.25, &opt.snrmask) == 1)
+ {
+ // std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl;
+ discarded_sats.insert(sat_number);
+ remove_sats.insert(sat_number);
+ continue;
+ }
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(common_obs_r.hasSatellite(sat));
+ assert(common_obs_k.hasSatellite(sat));
+ assert(common_sats_pos_r.count(sat));
+ assert(common_sats_pos_k.count(sat));
+ common_obs_r.removeObservationBySat(sat);
+ common_obs_k.removeObservationBySat(sat);
+ common_sats_pos_r.erase(sat);
+ common_sats_pos_k.erase(sat);
+ }
+
+ assert(common_obs_r.size() == common_obs_k.size());
+ assert(common_obs_r.size() == common_sats_pos_r.size());
+ assert(common_obs_r.size() == common_sats_pos_k.size());
+
+ // std::cout << "final size: " << common_obs_k.size() << std::endl;
}
-
+} // namespace GNSSUtils
diff --git a/src/examples/gnss_utils_test.cpp b/src/examples/gnss_utils_test.cpp
index fb0792acec09adc79a13e7ea13c1914c96226018..814ee443c905e3a1fd8d327bcad4d096227a02f6 100644
--- a/src/examples/gnss_utils_test.cpp
+++ b/src/examples/gnss_utils_test.cpp
@@ -8,39 +8,43 @@
#include <iostream>
-extern "C"
-{
- #include "../deps/RTKLIB/src/rinex.c"
+extern "C" {
+#include "../deps/RTKLIB/src/rinex.c"
}
using namespace GNSSUtils;
-int main(int argc, char *argv[])
+int main(int argc, char* argv[])
{
/* header */
std::cout << "--------------------------" << std::endl;
std::cout << "GNSS Utils Library Example" << std::endl;
std::cout << "--------------------------" << std::endl;
-
// create Observations object
Observations observations;
// create Navigation object
Navigation navigation;
+ gtime_t t_start{ 0, 0 }; // no limit
+ gtime_t t_end{ 0, 0 }; // no limit
+ double dt = 0.0; // no limit
+ const char* opt = ""; // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
- gtime_t t_start{0,0}; // no limit
- gtime_t t_end{0,0}; // no limit
- double dt = 0.0; // no limit
- const char* opt = "-SYS=G"; // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
+ // RTKLIB trace
+ // char str_file[80];
+ // snprintf(str_file, sizeof str_file, "../src/examples/trace");
// load observations from RINEX file
- observations.loadFromRinex("../src/examples/raw_201805171357.obs", t_start, t_end, dt, opt);
+ observations.loadFromRinex("../src/examples/sample_data.obs", t_start, t_end, dt, opt);
observations.print();
+ // Trace close
+ // traceclose();
+
// load navigation from RINEX file
- navigation.loadFromRinex("../src/examples/raw_201805171357.nav", t_start, t_end, dt, opt);
+ navigation.loadFromRinex("../src/examples/sample_data.nav", t_start, t_end, dt, opt);
navigation.print();
/* Set processing options */
@@ -51,26 +55,26 @@ int main(int argc, char *argv[])
std::cout << "------------------" << std::endl;
prcopt_t prcopt = prcopt_default;
- prcopt.mode = PMODE_SINGLE;
- prcopt.soltype = 0;
- prcopt.nf = 1;
- prcopt.navsys = SYS_GPS;
- prcopt.elmin = 0.525; // 60 degrees = 1.05 rad
- prcopt.sateph = EPHOPT_BRDC;
- prcopt.ionoopt = IONOOPT_OFF;
- prcopt.tropopt = TROPOPT_OFF;
+ prcopt.mode = PMODE_SINGLE;
+ prcopt.soltype = 0;
+ prcopt.nf = 1;
+ prcopt.navsys = SYS_GPS;
+ prcopt.elmin = 0.525; // 60 degrees = 1.05 rad
+ prcopt.sateph = EPHOPT_BRDC;
+ prcopt.ionoopt = IONOOPT_OFF;
+ prcopt.tropopt = TROPOPT_OFF;
prcopt.dynamics = 0;
prcopt.tidecorr = 0;
prcopt.sbascorr = SBSOPT_FCORR;
- prcopt.ru[0] = 0;//4789374.0336;
- prcopt.ru[1] = 0;//177048.3292;
- prcopt.ru[2] = 0;//4194542.6444;
+ prcopt.ru[0] = 0; // 4789374.0336;
+ prcopt.ru[1] = 0; // 177048.3292;
+ prcopt.ru[2] = 0; // 4194542.6444;
std::cout << "Processing options defined" << std::endl;
- Eigen::Vector3d lla_gt(41.383293114*M_PI/180.0, 2.116101115*M_PI/180.0, -91.6641);
+ Eigen::Vector3d lla_gt(41.383293114 * M_PI / 180.0, 2.116101115 * M_PI / 180.0, -91.6641);
- //Compute spp
+ // Compute spp
/* header */
std::cout << "-----------" << std::endl;
@@ -79,9 +83,9 @@ int main(int argc, char *argv[])
int stat;
- sol_t solb={{0}};
+ sol_t solb = { { 0 } };
- char msg[128]="";
+ char msg[128] = "";
stat = pntpos(observations.data(), observations.size(), &navigation.getNavigation(), &prcopt, &solb, NULL, NULL, msg);
@@ -89,6 +93,9 @@ int main(int argc, char *argv[])
std::cout << "sol.stat: " << int(solb.stat) << std::endl;
std::cout << "Position: " << solb.rr[0] << ", " << solb.rr[1] << ", " << solb.rr[2] << std::endl;
std::cout << "Position (GT): " << latLonAltToEcef(lla_gt).transpose() << std::endl;
- std::cout << "Position LLA: " << ecefToLatLonAlt(Eigen::Vector3d(solb.rr[0],solb.rr[1],solb.rr[2])).transpose() << std::endl;
+ std::cout << "Position LLA: " << ecefToLatLonAlt(Eigen::Vector3d(solb.rr[0], solb.rr[1], solb.rr[2])).transpose()
+ << std::endl;
std::cout << "Position LLA (GT): " << lla_gt.transpose() << std::endl;
+
+ traceclose();
}
diff --git a/src/examples/sample_data.nav b/src/examples/sample_data.nav
new file mode 100644
index 0000000000000000000000000000000000000000..5c4c418dc876fd45e7fddca388b4cc98b0202441
--- /dev/null
+++ b/src/examples/sample_data.nav
@@ -0,0 +1,406 @@
+ 3.04 N: GNSS NAV DATA M: MIXED RINEX VERSION / TYPE
+sbf2rin-13.4.3 20200205 113410 UTC PGM / RUN BY / DATE
+GPSA 9.3132E-09 -1.4901E-08 -5.9605E-08 1.1921E-07 IONOSPHERIC CORR
+GPSB 9.6256E+04 -1.4746E+05 -1.3107E+05 9.1750E+05 IONOSPHERIC CORR
+GAL 2.9250E+01 3.8281E-01 3.2959E-03 0.0000E+00 IONOSPHERIC CORR
+GPUT -9.3132257462E-10 8.881784197E-16 503808 2083 TIME SYSTEM CORR
+GAUT 9.3132257462E-10 0.000000000E+00 345600 2083 TIME SYSTEM CORR
+GAGP 1.2514647096E-09-5.329070518E-15 345600 2083 TIME SYSTEM CORR
+ 18 LEAP SECONDS
+ END OF HEADER
+G02 2019 12 12 12 00 00-3.648423589766E-04-7.503331289627E-12 0.000000000000E+00
+ 4.700000000000E+01-1.208437500000E+02 4.378039505776E-09-1.891689129337E+00
+ -6.053596735001E-06 1.953727600630E-02 9.275972843170E-06 5.153594808578E+03
+ 3.888000000000E+05 3.371387720108E-07-2.768163608668E-01 2.346932888031E-07
+ 9.575379914494E-01 1.915312500000E+02-1.691294764827E+00-7.654961717025E-09
+ 2.000083311498E-11 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00-1.769512891769E-08 4.700000000000E+01
+ 3.868920000000E+05 4.000000000000E+00
+G04 2019 12 12 12 00 00-1.993030309677E-05-5.002220859751E-12 0.000000000000E+00
+ 1.570000000000E+02-1.387500000000E+01 4.959135139313E-09-2.339868507292E+00
+ -8.419156074524E-07 1.249722088687E-03 5.329027771950E-06 5.150576126099E+03
+ 3.888000000000E+05 5.587935447693E-09 1.915372002909E+00 1.303851604462E-08
+ 9.594040575671E-01 2.769062500000E+02-1.281079249179E+00-8.132124449911E-09
+ -3.685867816904E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 4.096000000000E+03 6.300000000000E+01-4.190951585770E-09 6.690000000000E+02
+ 3.873960000000E+05 4.000000000000E+00
+G06 2019 12 12 12 00 00-1.512947492301E-04-1.114131009672E-11 0.000000000000E+00
+ 1.000000000000E+02-1.239062500000E+02 3.927663602954E-09-1.824134113525E+00
+ -6.347894668579E-06 1.658447668888E-03 9.786337614059E-06 5.153719285965E+03
+ 3.888000000000E+05-1.490116119385E-08-2.122724576342E-01 5.401670932770E-08
+ 9.781077241470E-01 2.010000000000E+02-1.200768673201E+00-7.640318249923E-09
+ 1.214336296267E-11 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00 4.656612873077E-09 1.000000000000E+02
+ 3.858180000000E+05 4.000000000000E+00
+G07 2019 12 12 12 00 00-1.683332957327E-04-8.185452315956E-12 0.000000000000E+00
+ 5.700000000000E+01 3.909375000000E+01 4.250534194668E-09-2.882882750792E+00
+ 1.888722181320E-06 1.320131728426E-02 1.172721385956E-05 5.153718780518E+03
+ 3.888000000000E+05-7.450580596924E-09 2.943673467473E+00-1.341104507446E-07
+ 9.545190186113E-01 1.482812500000E+02-2.422380694519E+00-7.540671242082E-09
+ -4.639478967207E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00-1.117587089539E-08 5.700000000000E+01
+ 3.858180000000E+05 4.000000000000E+00
+G09 2019 12 12 12 00 00-1.062308438122E-04-9.436007530894E-12 0.000000000000E+00
+ 7.800000000000E+01-1.206250000000E+01 5.168786729286E-09 4.992301981873E-01
+ -7.078051567078E-07 1.692383317277E-03 5.021691322327E-06 5.153534730911E+03
+ 3.888000000000E+05-2.793967723846E-08 1.868752621939E+00 1.490116119385E-08
+ 9.520568016730E-01 2.776875000000E+02 1.684705661839E+00-8.283559329210E-09
+ -2.914407111040E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00 1.396983861923E-09 7.800000000000E+01
+ 3.857520000000E+05 4.000000000000E+00
+G13 2019 12 12 12 00 00-2.714386209846E-05 2.387423592154E-12 0.000000000000E+00
+ 8.700000000000E+01-1.987500000000E+01 4.779841956746E-09-8.793798521920E-01
+ -1.072883605957E-06 4.130274290219E-03 5.709007382393E-06 5.153656044006E+03
+ 3.888000000000E+05-6.332993507385E-08 2.009100851183E+00 9.313225746155E-08
+ 9.674158381471E-01 2.706562500000E+02 1.183260297004E+00-8.083193840326E-09
+ -5.078782980268E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00-1.117587089539E-08 8.700000000000E+01
+ 3.870420000000E+05 4.000000000000E+00
+G23 2019 12 12 12 00 00-1.500290818512E-04 2.387423592154E-12 0.000000000000E+00
+ 3.000000000000E+01-1.009375000000E+01 5.335222233421E-09-1.452196357053E+00
+ -4.973262548447E-07 1.326873130165E-02 4.552304744720E-06 5.153694892883E+03
+ 3.888000000000E+05 2.980232238770E-07 1.861752114203E+00 4.470348358154E-08
+ 9.428683609718E-01 2.778750000000E+02-2.215284453592E+00-8.261415549690E-09
+ -2.478674675321E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00-2.048909664154E-08 3.000000000000E+01
+ 3.858420000000E+05 4.000000000000E+00
+G30 2019 12 12 12 00 00-1.042019575834E-04-8.640199666843E-12 0.000000000000E+00
+ 2.500000000000E+01 4.368750000000E+01 4.663408535398E-09-2.906933186921E+00
+ 2.210959792137E-06 4.164319136180E-03 1.142919063568E-05 5.153722436905E+03
+ 3.888000000000E+05-2.793967723846E-08 2.968865819418E+00-3.725290298462E-09
+ 9.399056183160E-01 1.480312500000E+02-2.921787544453E+00-7.888185717455E-09
+ -4.778770483544E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00
+ 2.000000000000E+00 0.000000000000E+00 3.725290298462E-09 2.500000000000E+01
+ 3.858180000000E+05 4.000000000000E+00
+R03 2019 12 12 11 15 00 6.369315087795E-06 0.000000000000E+00 3.858000000000E+05
+ 9.652274414063E+03 3.457994461060E-01 9.313225746155E-10 0.000000000000E+00
+ 1.708528710938E+04 2.202743530273E+00-1.862645149231E-09 5.000000000000E+00
+ 1.629986035156E+04-2.522974967957E+00-2.793967723846E-09 0.000000000000E+00
+R03 2019 12 12 11 45 00 6.371177732944E-06 0.000000000000E+00 3.870000000000E+05
+ 1.043946044922E+04 4.914197921753E-01 9.313225746155E-10 0.000000000000E+00
+ 2.039817236328E+04 1.453030586243E+00-1.862645149231E-09 5.000000000000E+00
+ 1.118693066406E+04-3.121288299561E+00-1.862645149231E-09 0.000000000000E+00
+R05 2019 12 12 11 15 00 4.108343273401E-05 9.094947017729E-13 3.862800000000E+05
+ 5.985222167969E+03 1.195173263550E-01-1.862645149231E-09 0.000000000000E+00
+ -1.828012353516E+04 2.295486450195E+00 1.862645149231E-09 1.000000000000E+00
+ 1.676849218750E+04 2.457766532898E+00-1.862645149231E-09 0.000000000000E+00
+R05 2019 12 12 11 45 00 4.108529537916E-05 9.094947017729E-13 3.871800000000E+05
+ 6.598400390625E+03 5.781021118164E-01-9.313225746155E-10 0.000000000000E+00
+ -1.370180371094E+04 2.745733261108E+00 9.313225746155E-10 1.000000000000E+00
+ 2.048785058594E+04 1.648019790649E+00-1.862645149231E-09 0.000000000000E+00
+R14 2019 12 12 11 15 00 4.597380757332E-05 0.000000000000E+00 3.858000000000E+05
+ 2.135816357422E+04-6.865911483765E-01 3.725290298462E-09 0.000000000000E+00
+ 1.261080761719E+04-4.694700241089E-01-0.000000000000E+00-7.000000000000E+00
+ 5.991423828125E+03 3.442845344543E+00-2.793967723846E-09 0.000000000000E+00
+R14 2019 12 12 11 45 00 4.597473889589E-05 0.000000000000E+00 3.870000000000E+05
+ 1.936734765625E+04-1.519350051880E+00 1.862645149231E-09 0.000000000000E+00
+ 1.162212011719E+04-5.869359970093E-01-9.313225746155E-10-7.000000000000E+00
+ 1.187716162109E+04 3.054467201233E+00-2.793967723846E-09 0.000000000000E+00
+R15 2019 12 12 11 15 00 1.051910221577E-04 0.000000000000E+00 3.860100000000E+05
+ 2.249286962891E+04 1.470921516418E+00 5.587935447693E-09 0.000000000000E+00
+ 4.179755859375E+03 4.950771331787E-01 9.313225746155E-10 0.000000000000E+00
+ -1.125261865234E+04 3.118181228638E+00-1.862645149231E-09 0.000000000000E+00
+R15 2019 12 12 11 45 00 1.051910221577E-04 0.000000000000E+00 3.870000000000E+05
+ 2.451276953125E+04 7.425718307495E-01 4.656612873077E-09 0.000000000000E+00
+ 4.643450683594E+03 4.951381683350E-02 0.000000000000E+00 0.000000000000E+00
+ -5.276882812500E+03 3.478320121765E+00-2.793967723846E-09 0.000000000000E+00
+R19 2019 12 12 11 15 00-6.853323429823E-05-1.818989403546E-12 3.859500000000E+05
+ 2.008951708984E+04 2.060537338257E-01 2.793967723846E-09 0.000000000000E+00
+ -1.541425244141E+04-4.377956390381E-01 2.793967723846E-09 3.000000000000E+00
+ 3.049126953125E+03-3.578741073608E+00-1.862645149231E-09 0.000000000000E+00
+R19 2019 12 12 11 45 00-6.853695958853E-05-1.818989403546E-12 3.870000000000E+05
+ 1.978871044922E+04-5.135507583618E-01 3.725290298462E-09 0.000000000000E+00
+ -1.571828076172E+04 1.333026885986E-01 1.862645149231E-09 3.000000000000E+00
+ -3.427280273438E+03-3.570507049561E+00-1.862645149231E-09 0.000000000000E+00
+R13 2019 12 12 11 45 00-3.215298056602E-05-0.000000000000E+00 3.876000000000E+05
+ 3.100402832031E+03-2.939124107361E+00-1.862645149231E-09 0.000000000000E+00
+ 1.186301953125E+04-8.695030212402E-01-9.313225746155E-10-2.000000000000E+00
+ 2.235348193359E+04 8.685455322266E-01-1.862645149231E-09 0.000000000000E+00
+R20 2019 12 12 11 45 00-3.986386582255E-04-0.000000000000E+00 3.877800000000E+05
+ 9.837636718750E+03 1.428291320801E+00-0.000000000000E+00 0.000000000000E+00
+ -1.795626220703E+04-1.534766197205E+00 1.862645149231E-09 2.000000000000E+00
+ 1.525312060547E+04-2.726587295532E+00-1.862645149231E-09 0.000000000000E+00
+E01 2019 12 12 11 10 00-7.507699192502E-04-7.986500349944E-12 0.000000000000E+00
+ 3.000000000000E+00 2.013125000000E+02 2.449744898851E-09 7.981291655576E-01
+ 9.473413228989E-06 1.731918891892E-04 9.533017873764E-06 5.440622966766E+03
+ 3.858000000000E+05 3.166496753693E-08-2.605880888987E+00-1.117587089539E-08
+ 9.855193908613E-01 1.463750000000E+02-1.069210219000E+00-5.272719629937E-09
+ 1.914365455291E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09
+ 3.864650000000E+05
+E01 2019 12 12 11 20 00-7.507746340707E-04-7.972289495228E-12 0.000000000000E+00
+ 4.000000000000E+00 2.002500000000E+02 2.449744898851E-09 8.736282654153E-01
+ 9.419396519661E-06 1.731686061248E-04 9.588897228241E-06 5.440622581482E+03
+ 3.864000000000E+05 3.725290298462E-08-2.605884063520E+00-1.862645149231E-09
+ 9.855195488564E-01 1.452500000000E+02-1.070324887968E+00-5.265933632987E-09
+ 1.896507568581E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09
+ 3.870650000000E+05
+E09 2019 12 12 11 10 00 6.223833654076E-03-1.216449163621E-11 0.000000000000E+00
+ 3.000000000000E+00 7.218750000000E+00 3.823373544569E-09-3.442271110123E-02
+ 3.892928361893E-07 2.964780433103E-04 3.403052687645E-06 5.440603132248E+03
+ 3.858000000000E+05-9.313225746155E-09 1.575307486314E+00-6.146728992462E-08
+ 9.586413959478E-01 2.625000000000E+02 2.794206052069E-01-5.815242228181E-09
+ 8.571785620706E-12 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-4.656612873077E-10-6.984919309616E-10
+ 3.869030000000E+05
+E12 2019 12 12 10 40 00 6.055364210624E-03-1.870148480521E-11 0.000000000000E+00
+ 0.000000000000E+00-2.075312500000E+02 2.603322724555E-09-2.414195903395E+00
+ -9.659677743912E-06 2.996901748702E-04 6.495043635368E-06 5.440614477158E+03
+ 3.840000000000E+05-2.421438694000E-08-5.219517721978E-01 5.774199962616E-08
+ 9.864495053986E-01 2.151250000000E+02-9.348391204646E-01-5.376652530588E-09
+ -2.257236880119E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.327134668827E-08-1.327134668827E-08
+ 3.858050000000E+05
+E19 2019 12 12 11 00 00-2.469634637237E-06 2.273736754432E-13 0.000000000000E+00
+ 2.000000000000E+00 1.103125000000E+01 3.765871149364E-09-1.685770007887E-01
+ 4.824250936508E-07 1.212444622070E-04 3.688037395477E-06 5.440600940704E+03
+ 3.852000000000E+05-5.401670932770E-08 1.580615112564E+00 5.215406417847E-08
+ 9.583897608719E-01 2.595937500000E+02 1.996512379669E+00-5.770954669140E-09
+ 1.160762636137E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09-5.820766091347E-09
+ 3.858660000000E+05
+E19 2019 12 12 11 10 00-2.469576429576E-06 2.273736754432E-13 0.000000000000E+00
+ 3.000000000000E+00 1.168750000000E+01 3.765156833895E-09-9.388288915917E-02
+ 5.066394805908E-07 1.211694907397E-04 3.693625330925E-06 5.440600774765E+03
+ 3.858000000000E+05-5.960464477539E-08 1.580611582543E+00 4.097819328308E-08
+ 9.583898398694E-01 2.594687500000E+02 1.996204499781E+00-5.764168672191E-09
+ 1.207193141583E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09-5.820766091347E-09
+ 3.864650000000E+05
+E21 2019 12 12 11 10 00-5.709171527997E-04-2.131628207280E-12 0.000000000000E+00
+ 3.000000000000E+00 2.180312500000E+02 2.333311477503E-09 1.537835498027E+00
+ 1.022964715958E-05 9.841390419751E-05 9.685754776001E-06 5.440636165619E+03
+ 3.858000000000E+05-2.980232238770E-08-2.608584029515E+00 4.842877388000E-08
+ 9.850865499746E-01 1.424062500000E+02-1.025413854454E+00-5.217717338871E-09
+ 1.871506527187E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10
+ 3.867350000000E+05
+E21 2019 12 12 11 20 00-5.709183751605E-04-2.131628207280E-12 0.000000000000E+00
+ 4.000000000000E+00 2.213125000000E+02 2.317953694933E-09 1.607657241034E+00
+ 1.038052141666E-05 9.810912888497E-05 9.816139936447E-06 5.440637243271E+03
+ 3.864000000000E+05-4.097819328308E-08-2.608587130902E+00 4.470348358154E-08
+ 9.850867050439E-01 1.395312500000E+02-1.020851268976E+00-5.213788603794E-09
+ 1.800074980348E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10
+ 3.870650000000E+05
+E01 2019 12 12 11 10 00-7.507689879276E-04-7.986500349944E-12 0.000000000000E+00
+ 3.000000000000E+00 2.013125000000E+02 2.449744898851E-09 7.981291655576E-01
+ 9.473413228989E-06 1.731918891892E-04 9.533017873764E-06 5.440622966766E+03
+ 3.858000000000E+05 3.166496753693E-08-2.605880888987E+00-1.117587089539E-08
+ 9.855193908613E-01 1.463750000000E+02-1.069210219000E+00-5.272719629937E-09
+ 1.914365455291E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00
+ 3.865400000000E+05
+E01 2019 12 12 11 20 00-7.507736445405E-04-7.972289495228E-12 0.000000000000E+00
+ 4.000000000000E+00 2.002500000000E+02 2.449744898851E-09 8.736282654153E-01
+ 9.419396519661E-06 1.731686061248E-04 9.588897228241E-06 5.440622581482E+03
+ 3.864000000000E+05 3.725290298462E-08-2.605884063520E+00-1.862645149231E-09
+ 9.855195488564E-01 1.452500000000E+02-1.070324887968E+00-5.265933632987E-09
+ 1.896507568581E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00
+ 3.871400000000E+05
+E12 2019 12 12 10 40 00 6.055364909116E-03-1.871569565992E-11 0.000000000000E+00
+ 0.000000000000E+00-2.075312500000E+02 2.603322724555E-09-2.414195903395E+00
+ -9.659677743912E-06 2.996901748702E-04 6.495043635368E-06 5.440614477158E+03
+ 3.840000000000E+05-2.421438694000E-08-5.219517721978E-01 5.774199962616E-08
+ 9.864495053986E-01 2.151250000000E+02-9.348391204646E-01-5.376652530588E-09
+ -2.257236880119E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.327134668827E-08 0.000000000000E+00
+ 3.858400000000E+05
+E19 2019 12 12 11 00 00-2.468528691679E-06 2.273736754432E-13 0.000000000000E+00
+ 2.000000000000E+00 1.103125000000E+01 3.765871149364E-09-1.685770007887E-01
+ 4.824250936508E-07 1.212444622070E-04 3.688037395477E-06 5.440600940704E+03
+ 3.852000000000E+05-5.401670932770E-08 1.580615112564E+00 5.215406417847E-08
+ 9.583897608719E-01 2.595937500000E+02 1.996512379669E+00-5.770954669140E-09
+ 1.160762636137E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09 0.000000000000E+00
+ 3.859400000000E+05
+E19 2019 12 12 11 10 00-2.468470484018E-06 2.273736754432E-13 0.000000000000E+00
+ 3.000000000000E+00 1.168750000000E+01 3.765156833895E-09-9.388288915917E-02
+ 5.066394805908E-07 1.211694907397E-04 3.693625330925E-06 5.440600774765E+03
+ 3.858000000000E+05-5.960464477539E-08 1.580611582543E+00 4.097819328308E-08
+ 9.583898398694E-01 2.594687500000E+02 1.996204499781E+00-5.764168672191E-09
+ 1.207193141583E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09 0.000000000000E+00
+ 3.865400000000E+05
+E21 2019 12 12 11 10 00-5.709161632694E-04-2.117417352565E-12 0.000000000000E+00
+ 3.000000000000E+00 2.180312500000E+02 2.333311477503E-09 1.537835498027E+00
+ 1.022964715958E-05 9.841390419751E-05 9.685754776001E-06 5.440636165619E+03
+ 3.858000000000E+05-2.980232238770E-08-2.608584029515E+00 4.842877388000E-08
+ 9.850865499746E-01 1.424062500000E+02-1.025413854454E+00-5.217717338871E-09
+ 1.871506527187E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00
+ 3.867900000000E+05
+E21 2019 12 12 11 20 00-5.709173856303E-04-2.131628207280E-12 0.000000000000E+00
+ 4.000000000000E+00 2.213125000000E+02 2.317953694933E-09 1.607657241034E+00
+ 1.038052141666E-05 9.810912888497E-05 9.816139936447E-06 5.440637243271E+03
+ 3.864000000000E+05-4.097819328308E-08-2.608587130902E+00 4.470348358154E-08
+ 9.850867050439E-01 1.395312500000E+02-1.020851268976E+00-5.213788603794E-09
+ 1.800074980348E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00
+ 3.871400000000E+05
+E04 2019 12 12 11 10 00-4.236891982146E-04-7.531752999057E-12 0.000000000000E+00
+ 3.000000000000E+00 9.781250000000E+00 3.935521073107E-09-4.009111796513E-01
+ 5.532056093216E-07 8.811207953840E-05 3.596767783165E-06 5.440604427338E+03
+ 3.858000000000E+05 9.499490261078E-08 1.582511472789E+00 3.166496753693E-08
+ 9.526713574660E-01 2.577187500000E+02 1.440283619102E+00-5.908817554540E-09
+ 7.071723137082E-11 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.355104804039E-09-5.820766091347E-09
+ 3.875670000000E+05
+E04 2019 12 12 11 10 00-4.236878594384E-04-7.531752999057E-12 0.000000000000E+00
+ 3.000000000000E+00 9.781250000000E+00 3.935521073107E-09-4.009111796513E-01
+ 5.532056093216E-07 8.811207953840E-05 3.596767783165E-06 5.440604427338E+03
+ 3.858000000000E+05 9.499490261078E-08 1.582511472789E+00 3.166496753693E-08
+ 9.526713574660E-01 2.577187500000E+02 1.440283619102E+00-5.908817554540E-09
+ 7.071723137082E-11 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-5.355104804039E-09 0.000000000000E+00
+ 3.875900000000E+05
+E11 2019 12 12 11 00 00 3.295102505945E-04 1.526245796413E-10 5.204170427930E-18
+ 2.000000000000E+00-2.123750000000E+02 2.665468170305E-09 2.480654590844E+00
+ -9.858980774879E-06 1.651302445680E-04 7.150694727898E-06 5.440609029770E+03
+ 3.852000000000E+05-3.352761268616E-08-5.219456059981E-01 2.048909664154E-08
+ 9.864349859366E-01 1.975937500000E+02-2.012781103955E-01-5.400939256513E-09
+ -2.103659054415E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.583248376846E-08-1.699663698673E-08
+ 3.876330000000E+05
+E21 2019 12 12 11 30 00-5.709196557291E-04-2.131628207280E-12 0.000000000000E+00
+ 5.000000000000E+00 2.240625000000E+02 2.303667385565E-09 1.677767938158E+00
+ 1.050904393196E-05 9.777839295566E-05 9.970739483833E-06 5.440638303757E+03
+ 3.870000000000E+05-5.401670932770E-08-2.608590226436E+00 3.725290298462E-08
+ 9.850868571874E-01 1.362812500000E+02-1.016577701982E+00-5.210574184187E-09
+ 1.721500278825E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10
+ 3.876650000000E+05
+E11 2019 12 12 11 00 00 3.295124042779E-04 1.526387904960E-10 5.204170427930E-18
+ 2.000000000000E+00-2.123750000000E+02 2.665468170305E-09 2.480654590844E+00
+ -9.858980774879E-06 1.651302445680E-04 7.150694727898E-06 5.440609029770E+03
+ 3.852000000000E+05-3.352761268616E-08-5.219456059981E-01 2.048909664154E-08
+ 9.864349859366E-01 1.975937500000E+02-2.012781103955E-01-5.400939256513E-09
+ -2.103659054415E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.583248376846E-08 0.000000000000E+00
+ 3.876800000000E+05
+E01 2019 12 12 11 30 00-7.507792906836E-04-7.972289495228E-12 0.000000000000E+00
+ 5.000000000000E+00 1.994687500000E+02 2.447601952446E-09 9.486982138657E-01
+ 9.378418326378E-06 1.731649972498E-04 9.616836905479E-06 5.440622346878E+03
+ 3.870000000000E+05 3.911554813385E-08-2.605887229274E+00 7.450580596924E-09
+ 9.855197039257E-01 1.446875000000E+02-1.071010335308E+00-5.259504793772E-09
+ 1.871506527187E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09
+ 3.877250000000E+05
+E21 2019 12 12 11 30 00-5.709186661988E-04-2.117417352565E-12 0.000000000000E+00
+ 5.000000000000E+00 2.240625000000E+02 2.303667385565E-09 1.677767938158E+00
+ 1.050904393196E-05 9.777839295566E-05 9.970739483833E-06 5.440638303757E+03
+ 3.870000000000E+05-5.401670932770E-08-2.608590226436E+00 3.725290298462E-08
+ 9.850868571874E-01 1.362812500000E+02-1.016577701982E+00-5.210574184187E-09
+ 1.721500278825E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00
+ 3.877400000000E+05
+E01 2019 12 12 11 30 00-7.507783011533E-04-7.972289495228E-12 0.000000000000E+00
+ 5.000000000000E+00 1.994687500000E+02 2.447601952446E-09 9.486982138657E-01
+ 9.378418326378E-06 1.731649972498E-04 9.616836905479E-06 5.440622346878E+03
+ 3.870000000000E+05 3.911554813385E-08-2.605887229274E+00 7.450580596924E-09
+ 9.855197039257E-01 1.446875000000E+02-1.071010335308E+00-5.259504793772E-09
+ 1.871506527187E-10 2.580000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00
+ 3.878200000000E+05
+E36 2019 12 12 10 30 00 6.253067986108E-04-4.803268893738E-12 0.000000000000E+00
+ 1.270000000000E+02-2.396562500000E+02 2.521890761159E-09 3.060908164528E+00
+ -1.121312379837E-05 3.076605498791E-04 7.288530468941E-06 5.440609262466E+03
+ 3.834000000000E+05 2.980232238770E-08-5.264936383676E-01 5.587935447693E-08
+ 9.896539850266E-01 1.944062500000E+02-1.782833699345E+00-5.428440402046E-09
+ -2.457245211269E-10 5.170000000000E+02 2.083000000000E+03
+ 3.120000000000E+00 0.000000000000E+00 3.725290298462E-09 4.423782229424E-09
+ 3.878450000000E+05
+S36 2019 12 12 11 36 48 0.000000000000E+00 0.000000000000E+00 3.874820000000E+05
+ 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.530000000000E+02
+S25 2019 12 11 23 59 44 0.000000000000E+00 0.000000000000E+00 3.874840000000E+05
+ 4.053076976000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ -1.162201120000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00
+S23 2019 12 12 11 37 04 0.000000000000E+00 0.000000000000E+00 3.874940000000E+05
+ 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.460000000000E+02
+S36 2019 12 12 11 38 56 0.000000000000E+00 0.000000000000E+00 3.875460000000E+05
+ 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.540000000000E+02
+S23 2019 12 12 11 39 12 0.000000000000E+00 0.000000000000E+00 3.875580000000E+05
+ 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.470000000000E+02
+S23 2019 12 12 11 41 20 0.000000000000E+00 0.000000000000E+00 3.876860000000E+05
+ 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.480000000000E+02
+S36 2019 12 12 11 41 04 0.000000000000E+00 0.000000000000E+00 3.877380000000E+05
+ 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.550000000000E+02
+S36 2019 12 12 11 43 12 0.000000000000E+00 0.000000000000E+00 3.878020000000E+05
+ 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00
+S23 2019 12 12 11 43 28 0.000000000000E+00 0.000000000000E+00 3.878780000000E+05
+ 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01
+ 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04
+ 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.490000000000E+02
+C05 2019 12 12 11 00 00-2.244751667604E-04-5.808153957787E-11 0.000000000000E+00
+ 1.000000000000E+00-4.986718750000E+02 1.785788670980E-12-5.169766818568E-01
+ -1.630326732993E-05 6.773804780096E-04 2.061109989882E-05 6.493346771240E+03
+ 3.852000000000E+05 1.271255314350E-07-1.766797704845E-01 3.259629011154E-09
+ 1.180676987021E-01-6.241562500000E+02-1.608095995036E+00 1.192549674481E-09
+ 7.575315542299E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00-8.000000000000E-10-9.200000000000E-09
+ 3.858276000000E+05 0.000000000000E+00
+C10 2019 12 12 11 00 00-4.537283675745E-04-3.685762806072E-11 0.000000000000E+00
+ 1.000000000000E+00 9.611718750000E+02 9.743262988869E-10-3.040647920928E+00
+ 3.135204315186E-05 6.156359100714E-03 7.578637450933E-06 6.493785018921E+03
+ 3.852000000000E+05-1.676380634308E-08-2.379959881122E+00-4.004687070847E-08
+ 9.058375010021E-01-1.493750000000E+01-2.570309406835E+00-2.115088101909E-09
+ 4.782342060886E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00 5.000000000000E-09 2.400000000000E-09
+ 3.858180000000E+05 0.000000000000E+00
+C23 2019 12 12 11 00 00-8.696540025994E-04 2.136069099379E-12 0.000000000000E+00
+ 1.000000000000E+00 1.034062500000E+02 3.498002848716E-09 1.631067082891E+00
+ 5.179084837437E-06 2.014992060140E-04 1.188227906823E-05 5.282628129959E+03
+ 3.852000000000E+05 3.725290298462E-08-3.016985033742E+00 6.565824151039E-08
+ 9.546959600159E-01 1.175156250000E+02-1.126531243634E+00-6.488127399406E-09
+ -1.864363372504E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00 2.430000000000E-08 2.430000000000E-08
+ 3.858180000000E+05 1.000000000000E+00
+C30 2019 12 12 11 00 00 2.388220746070E-04 6.104450278599E-12 0.000000000000E+00
+ 1.000000000000E+00-1.393437500000E+02 3.927663602954E-09-2.732742750025E+00
+ -6.837770342827E-06 3.649030113593E-04 1.750886440277E-06 5.282614929199E+03
+ 3.852000000000E+05-3.864988684654E-08-9.521190897989E-01-5.587935447693E-09
+ 9.644793109832E-01 3.275781250000E+02-4.157902561103E-01-7.082080711374E-09
+ -2.339383158984E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00-1.040000000000E-08-1.040000000000E-08
+ 3.858180000000E+05 1.000000000000E+00
+C32 2019 12 12 11 00 00-8.566775359213E-04 2.992273095970E-12 0.000000000000E+00
+ 1.000000000000E+00 2.875000000000E+01 4.228033257413E-09 2.867304095205E+00
+ 1.456588506699E-06 1.842749770731E-04 6.427057087421E-06 5.282614164352E+03
+ 3.852000000000E+05 1.862645149231E-09 1.148173467908E+00 6.705522537231E-08
+ 9.608588315814E-01 2.297500000000E+02-1.050918258529E+00-6.857785654299E-09
+ 5.228789228631E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00-9.900000000000E-09-9.900000000000E-09
+ 3.857580000000E+05 1.000000000000E+00
+C37 2019 12 12 11 00 00-8.880557725206E-04-5.954881032721E-11 0.000000000000E+00
+ 1.000000000000E+00 1.086562500000E+02 3.521932416908E-09 8.066563687499E-01
+ 5.388632416725E-06 7.037379546091E-04 1.127412542701E-05 5.282630052567E+03
+ 3.852000000000E+05-2.142041921616E-08-3.020306267399E+00 1.536682248116E-08
+ 9.560827215058E-01 1.306718750000E+02-1.080448983277E+00-6.580988410297E-09
+ -2.328668426958E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00-1.390000000000E-08-1.390000000000E-08
+ 3.858180000000E+05 1.000000000000E+00
+C20 2019 12 12 11 00 00-4.067538538948E-04-5.008171655163E-11 0.000000000000E+00
+ 1.000000000000E+00 2.850000000000E+01 4.229461888350E-09 1.463145840983E+00
+ 1.523178070784E-06 5.460308166221E-04 6.177462637424E-06 5.282614208221E+03
+ 3.852000000000E+05 3.445893526077E-08 1.153432718382E+00-1.210719347000E-08
+ 9.622213291121E-01 2.341406250000E+02-4.480160490039E-01-6.852785446020E-09
+ 4.753769442150E-10 0.000000000000E+00 7.270000000000E+02
+ 2.000000000000E+00 0.000000000000E+00 2.090000000000E-08 2.090000000000E-08
+ 3.875880000000E+05 1.000000000000E+00
diff --git a/src/examples/sample_data.obs b/src/examples/sample_data.obs
new file mode 100644
index 0000000000000000000000000000000000000000..d773d3b4d57556afcd38b64ef4167981a00de992
--- /dev/null
+++ b/src/examples/sample_data.obs
@@ -0,0 +1,58 @@
+ 3.04 OBSERVATION DATA M RINEX VERSION / TYPE
+sbf2rin-13.4.3 20200205 113408 UTC PGM / RUN BY / DATE
+SEPT MARKER NAME
+Unknown MARKER NUMBER
+Unknown Unknown OBSERVER / AGENCY
+3021420 SEPT ASTERX-M2 4.4.0 REC # / TYPE / VERS
+Unknown Unknown ANT # / TYPE
+ 4789398.3686 176958.8129 4194502.0999 APPROX POSITION XYZ
+ 0.0000 0.0000 0.0000 ANTENNA: DELTA H/E/N
+G 7 X1 C1C L1C C2W L2W C2L L2L SYS / # / OBS TYPES
+E 7 X1 C1C L1C C5Q L5Q C7Q L7Q SYS / # / OBS TYPES
+S 3 X1 C1C L1C SYS / # / OBS TYPES
+R 5 X1 C1C L1C C2C L2C SYS / # / OBS TYPES
+C 5 X1 C2I L2I C7I L7I SYS / # / OBS TYPES
+SEPTENTRIO RECEIVERS OUTPUT ALIGNED CARRIER PHASES. COMMENT
+NO FURTHER PHASE SHIFT APPLIED IN THE RINEX ENCODER. COMMENT
+G L1C SYS / PHASE SHIFT
+G L2W SYS / PHASE SHIFT
+G L2L 0.00000 SYS / PHASE SHIFT
+E L1C 0.00000 SYS / PHASE SHIFT
+E L5Q 0.00000 SYS / PHASE SHIFT
+E L7Q 0.00000 SYS / PHASE SHIFT
+S L1C SYS / PHASE SHIFT
+R L1C SYS / PHASE SHIFT
+R L2C SYS / PHASE SHIFT
+C L2I SYS / PHASE SHIFT
+C L7I SYS / PHASE SHIFT
+ 0.050 INTERVAL
+ 2019 12 12 11 37 42.0000000 GPS TIME OF FIRST OBS
+ 2019 12 12 11 45 12.9000000 GPS TIME OF LAST OBS
+ 36 # OF SATELLITES
+ C1C 0.000 C2C 0.000 GLONASS COD/PHS/BIS
+ 8 R03 5 R05 1 R13 -2 R14 -7 R15 0 R19 3 R20 2 R21 4 GLONASS SLOT / FRQ #
+ END OF HEADER
+> 2019 12 12 11 37 42.0000000 0 23
+C05 16.000 40138840.659 6 209013478.40606 40138832.256 6 161622441.87406
+C10 17.000 39633104.355 5 206379985.90205 39633100.852 6 159586077.69206
+C23 18.000 22865676.301 8 119067585.90908
+C32 19.000 22153454.599 8 115358921.19508
+C37 20.000 23690566.611 7 123363010.77907
+E01 11.000 26311074.676 7 138265636.53007 26311074.470 5 103250320.19005 26311070.977 7 105943790.57907
+E09 12.000 25723493.482 5
+E12 13.000 24803095.627 5 130341141.55605 24803092.768 4 97332661.71504 24803092.407 6 99871767.93506
+E19 14.000 24540665.652 6 128962068.93606 24540664.837 5 96302837.53105 24540661.694 6 98815075.63606
+E21 15.000 25609802.251 7 134580416.73507 25609802.381 7 100498367.38007 25609799.519 7 103120055.01507
+G02 1.000 22032430.127 5
+G06 2.000 22147435.410 6 116385570.45206 22147437.095 4 90690045.24704 22147437.422 7 90690046.16907
+G07 3.000 20902411.481 8 109842911.60108 20902406.002 6 85591857.15006 20902406.930 7 85591857.16507
+G09 4.000 21908403.525 6 115129430.37406 21908399.544 1 89711233.32401 21908399.320 5
+G23 5.000 24104714.314 6 126671129.49306 24104706.816 2 98704739.67602
+G30 6.000 21404145.095 8 112479542.93308 21404142.953 6 87646388.26706 21404143.916 7 87646383.27307
+R03 7.000 21945110.617 7 117474090.63807 21945109.720 6 91368580.68606
+R14 8.000 20214975.526 8 107757316.39008 20214976.579 7 83811176.41707
+R15 9.000 22699008.378 5 121296675.96005 22699009.712 5 94341922.34705
+R19 10.000 22853592.745 4 22853590.888 4
+S23 21.000 38309228.895 7 201316353.13407
+S25 22.000 37834172.957 6 198818604.39206
+S36 23.000 37630702.258 7 197750698.01307
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index 6747ab860d2cbd80bc83ff28b06378d0c45323c4..70bad327efe878de41efd6fd2c21d5f7f4e51528 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -2,21 +2,22 @@
namespace GNSSUtils
{
-
- ComputePosOutput computePos(const GNSSUtils::Observations & _observations,
- GNSSUtils::Navigation & _navigation,
- const prcopt_t & _prcopt)
- {
+ComputePosOutput computePos(const GNSSUtils::Observations& _observations,
+ GNSSUtils::Navigation& _navigation,
+ const prcopt_t& _prcopt)
+{
+ // Remove duplicated satellites
+ _navigation.uniqueNavigation();
- // Remove duplicated satellites
- _navigation.uniqueNavigation();
+ // Define error msg
+ char msg[128] = "";
- // Define error msg
- char msg[128] = "";
+ // Remove duplicated satellites
+ _navigation.uniqueNavigation();
- GNSSUtils::ComputePosOutput output;
- sol_t sol;
- sol = {{0}};
+ GNSSUtils::ComputePosOutput output;
+ sol_t sol;
+ sol = { { 0 } };
output.pos_stat = pntpos(_observations.data(), _observations.size(),
&(_navigation.getNavigation()),
@@ -50,392 +51,460 @@ namespace GNSSUtils
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;
- }
+// 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 ecefToLatLonAlt(const Eigen::Vector3d& _ecef)
{
- Eigen::Vector3d pos;
- ecef2pos(_ecef.data(), pos.data());
+ Eigen::Vector3d pos;
+ ecef2pos(_ecef.data(), pos.data());
- return pos;
+ return pos;
}
-Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d & _pos)
+Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _pos)
{
- Eigen::Vector3d ecef;
- pos2ecef(_pos.data(), ecef.data());
+ Eigen::Vector3d ecef;
+ pos2ecef(_pos.data(), ecef.data());
- return ecef;
+ return ecef;
}
-
-Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_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 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 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;
+ 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)
+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;
+ // 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)
+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));
+ 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(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(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;
+ 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);
+ Eigen::Vector3d t_ECEF_ENU = latLonAltToEcef(_ENU_latlonalt);
- t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU;
+ 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());
+ // 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()];
- // receiver-sat vector ecef
- Eigen::Vector3d receiver_sat_ecef = sat_ecef-receiver_ecef;
+ // std::cout << "computing sats position from sats: ";
+ // for (auto&& obs_ref : obs.getObservations())
+ // std::cout << (int)obs_ref.sat << " ";
+ // std::cout << std::endl;
- // 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}
+ // compute positions
+ satposs(
+ obs.getObservations().front().time, obs.data(), obs.size(), &nav.getNavigation(), opt.sateph, rs, dts, var, svh);
- // elevation
- return atan2(receiver_sat_enu(2), sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
+ // 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;
+ }
}
-void computeSatellitesPositions(const Observations& obs,
- const Navigation& nav,
- const prcopt_t& opt,
- std::map<int,Eigen::Vector3d>& sats_pos)
+bool equalTime(const gtime_t& time1, const gtime_t& time2)
{
- 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;
- }
+ 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/navigation.cpp b/src/navigation.cpp
index a3e3dac38d96b00f539d712b76e5f9f8465dba14..398d0a86f275b64f4d469a2bce0ddbdf3dba6172 100644
--- a/src/navigation.cpp
+++ b/src/navigation.cpp
@@ -4,33 +4,35 @@ using namespace GNSSUtils;
Navigation::Navigation()
{
- // array initialization
- nav_.n = nav_.nmax = nav_.ng = nav_.ngmax = nav_.ns = nav_.nsmax = nav_.ne = nav_.nemax = nav_.nc = nav_.ncmax = nav_.na = nav_.namax = nav_.nt = nav_.ntmax = nav_.nf = nav_.nfmax = 0;
- nav_.eph = NULL;
- nav_.geph = NULL;
- nav_.seph = NULL;
- nav_.peph = NULL;
- nav_.pclk = NULL;
- nav_.alm = NULL;
- nav_.tec = NULL;
- nav_.fcb = NULL;
- clearNavigation();
+ // array initialization
+ nav_.n = nav_.nmax = nav_.ng = nav_.ngmax = nav_.ns = nav_.nsmax = nav_.ne = nav_.nemax = nav_.nc = nav_.ncmax =
+ nav_.na = nav_.namax = nav_.nt = nav_.ntmax = nav_.nf = nav_.nfmax = 0;
+ nav_.eph = NULL;
+ nav_.geph = NULL;
+ nav_.seph = NULL;
+ nav_.peph = NULL;
+ nav_.pclk = NULL;
+ nav_.alm = NULL;
+ nav_.tec = NULL;
+ nav_.fcb = NULL;
+ clearNavigation();
}
Navigation::Navigation(const Navigation& nav)
{
- // array initialization
- nav_.n = nav_.nmax = nav_.ng = nav_.ngmax = nav_.ns = nav_.nsmax = nav_.ne = nav_.nemax = nav_.nc = nav_.ncmax = nav_.na = nav_.namax = nav_.nt = nav_.ntmax = nav_.nf = nav_.nfmax = 0;
- nav_.eph = NULL;
- nav_.geph = NULL;
- nav_.seph = NULL;
- nav_.peph = NULL;
- nav_.pclk = NULL;
- nav_.alm = NULL;
- nav_.tec = NULL;
- nav_.fcb = NULL;
- clearNavigation();
- setNavigation(nav.getNavigation());
+ // array initialization
+ nav_.n = nav_.nmax = nav_.ng = nav_.ngmax = nav_.ns = nav_.nsmax = nav_.ne = nav_.nemax = nav_.nc = nav_.ncmax =
+ nav_.na = nav_.namax = nav_.nt = nav_.ntmax = nav_.nf = nav_.nfmax = 0;
+ nav_.eph = NULL;
+ nav_.geph = NULL;
+ nav_.seph = NULL;
+ nav_.peph = NULL;
+ nav_.pclk = NULL;
+ nav_.alm = NULL;
+ nav_.tec = NULL;
+ nav_.fcb = NULL;
+ clearNavigation();
+ setNavigation(nav.getNavigation());
}
Navigation::~Navigation()
@@ -40,225 +42,224 @@ Navigation::~Navigation()
void Navigation::setNavigation(nav_t _nav)
{
- freeNavigationArrays();
-
- copyAllArrays(_nav);
- // int n,nmax; /* number of broadcast ephemeris */
- // int ng,ngmax; /* number of glonass ephemeris */
- // int ns,nsmax; /* number of sbas ephemeris */
- // int ne,nemax; /* number of precise ephemeris */
- // int nc,ncmax; /* number of precise clock */
- // int na,namax; /* number of almanac data */
- // int nt,ntmax; /* number of tec grid data */
- // int nf,nfmax; /* number of satellite fcb data */
- // eph_t *eph; /* GPS/QZS/GAL ephemeris */
- // geph_t *geph; /* GLONASS ephemeris */
- // seph_t *seph; /* SBAS ephemeris */
- // peph_t *peph; /* precise ephemeris */
- // pclk_t *pclk; /* precise clock */
- // alm_t *alm; /* almanac data */
- // tec_t *tec; /* tec grid data */
- // fcbd_t *fcb; /* satellite fcb data */
- // double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */
+ freeNavigationArrays();
- copyIonUtc(_nav);
- // double utc_gps[4]; /* GPS delta-UTC parameters {A0,A1,T,W} */
- // double utc_glo[4]; /* GLONASS UTC GPS time parameters */
- // double utc_gal[4]; /* Galileo UTC GPS time parameters */
- // double utc_qzs[4]; /* QZS UTC GPS time parameters */
- // double utc_cmp[4]; /* BeiDou UTC parameters */
- // double utc_irn[4]; /* IRNSS UTC parameters */
- // double utc_sbs[4]; /* SBAS UTC parameters */
- // double ion_gps[8]; /* GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
- // double ion_gal[4]; /* Galileo iono model parameters {ai0,ai1,ai2,0} */
- // double ion_qzs[8]; /* QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
- // double ion_cmp[8]; /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
- // double ion_irn[8]; /* IRNSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
- // int leaps; /* leap seconds (s) */
+ copyAllArrays(_nav);
+ // int n,nmax; /* number of broadcast ephemeris */
+ // int ng,ngmax; /* number of glonass ephemeris */
+ // int ns,nsmax; /* number of sbas ephemeris */
+ // int ne,nemax; /* number of precise ephemeris */
+ // int nc,ncmax; /* number of precise clock */
+ // int na,namax; /* number of almanac data */
+ // int nt,ntmax; /* number of tec grid data */
+ // int nf,nfmax; /* number of satellite fcb data */
+ // eph_t *eph; /* GPS/QZS/GAL ephemeris */
+ // geph_t *geph; /* GLONASS ephemeris */
+ // seph_t *seph; /* SBAS ephemeris */
+ // peph_t *peph; /* precise ephemeris */
+ // pclk_t *pclk; /* precise clock */
+ // alm_t *alm; /* almanac data */
+ // tec_t *tec; /* tec grid data */
+ // fcbd_t *fcb; /* satellite fcb data */
+ // double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */
- copySbasCorrections(_nav);
- // sbssat_t sbssat; /* SBAS satellite corrections */
- // sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */
+ copyIonUtc(_nav);
+ // double utc_gps[4]; /* GPS delta-UTC parameters {A0,A1,T,W} */
+ // double utc_glo[4]; /* GLONASS UTC GPS time parameters */
+ // double utc_gal[4]; /* Galileo UTC GPS time parameters */
+ // double utc_qzs[4]; /* QZS UTC GPS time parameters */
+ // double utc_cmp[4]; /* BeiDou UTC parameters */
+ // double utc_irn[4]; /* IRNSS UTC parameters */
+ // double utc_sbs[4]; /* SBAS UTC parameters */
+ // double ion_gps[8]; /* GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+ // double ion_gal[4]; /* Galileo iono model parameters {ai0,ai1,ai2,0} */
+ // double ion_qzs[8]; /* QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+ // double ion_cmp[8]; /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+ // double ion_irn[8]; /* IRNSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+ // int leaps; /* leap seconds (s) */
+ copySbasCorrections(_nav);
+ // sbssat_t sbssat; /* SBAS satellite corrections */
+ // sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */
- // ********** other not copied nav_t content: ***********
- // erp_t erp; /* earth rotation parameters */
- // double cbias[MAXSAT][3]; /* satellite dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
- // double rbias[MAXRCV][2][3]; /* receiver dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
- // double wlbias[MAXSAT]; /* wide-lane bias (cycle) */
- // double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */
- // SHOULD WE COPY THIS???????? char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */
- // pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */
- // dgps_t dgps[MAXSAT]; /* DGPS corrections */
- // ssr_t ssr[MAXSAT]; /* SSR corrections */
- // lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */
- // lexion_t lexion; /* LEX ionosphere correction */
- // pppcorr_t pppcorr; /* ppp corrections */
+ // ********** other not copied nav_t content: ***********
+ // erp_t erp; /* earth rotation parameters */
+ // double cbias[MAXSAT][3]; /* satellite dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
+ // double rbias[MAXRCV][2][3]; /* receiver dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
+ // double wlbias[MAXSAT]; /* wide-lane bias (cycle) */
+ // double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */
+ // SHOULD WE COPY THIS???????? char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */
+ // pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */
+ // dgps_t dgps[MAXSAT]; /* DGPS corrections */
+ // ssr_t ssr[MAXSAT]; /* SSR corrections */
+ // lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */
+ // lexion_t lexion; /* LEX ionosphere correction */
+ // pppcorr_t pppcorr; /* ppp corrections */
}
void Navigation::clearNavigation()
{
- freeNavigationArrays();
- memset (nav_.utc_gps, 0, sizeof (nav_.utc_gps));
- memset (nav_.utc_glo, 0, sizeof (nav_.utc_glo));
- memset (nav_.utc_gal, 0, sizeof (nav_.utc_gal));
- memset (nav_.utc_qzs, 0, sizeof (nav_.utc_qzs));
- memset (nav_.utc_cmp, 0, sizeof (nav_.utc_cmp));
- memset (nav_.utc_irn, 0, sizeof (nav_.utc_irn));
- memset (nav_.utc_sbs, 0, sizeof (nav_.utc_sbs));
- memset (nav_.ion_gps, 0, sizeof (nav_.ion_gps));
- memset (nav_.ion_gal, 0, sizeof (nav_.ion_gal));
- memset (nav_.ion_qzs, 0, sizeof (nav_.ion_qzs));
- memset (nav_.ion_cmp, 0, sizeof (nav_.ion_cmp));
- memset (nav_.ion_irn, 0, sizeof (nav_.ion_irn));
- nav_.leaps = 0;
- sbssat_t sbssat_zero = {0};
- nav_.sbssat = sbssat_zero;
+ freeNavigationArrays();
+ memset(nav_.utc_gps, 0, sizeof(nav_.utc_gps));
+ memset(nav_.utc_glo, 0, sizeof(nav_.utc_glo));
+ memset(nav_.utc_gal, 0, sizeof(nav_.utc_gal));
+ memset(nav_.utc_qzs, 0, sizeof(nav_.utc_qzs));
+ memset(nav_.utc_cmp, 0, sizeof(nav_.utc_cmp));
+ memset(nav_.utc_irn, 0, sizeof(nav_.utc_irn));
+ memset(nav_.utc_sbs, 0, sizeof(nav_.utc_sbs));
+ memset(nav_.ion_gps, 0, sizeof(nav_.ion_gps));
+ memset(nav_.ion_gal, 0, sizeof(nav_.ion_gal));
+ memset(nav_.ion_qzs, 0, sizeof(nav_.ion_qzs));
+ memset(nav_.ion_cmp, 0, sizeof(nav_.ion_cmp));
+ memset(nav_.ion_irn, 0, sizeof(nav_.ion_irn));
+ nav_.leaps = 0;
+ sbssat_t sbssat_zero = { 0 };
+ nav_.sbssat = sbssat_zero;
}
void Navigation::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt)
{
- auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, NULL, &nav_, NULL);
- if (stat == 0)
- {
- std::cout << "Navigation file loaded." << std::endl;
- std::cout << "GPS satellites in navigation file: " << nav_.n << std::endl;
- std::cout << "GLONASS satellites in navigation file: " << nav_.ng << std::endl;
- std::cout << "SBAS satellites in navigation file: " << nav_.ns << std::endl;
- std::cout << "Almanac satellites in navigation file: " << nav_.na << std::endl;
- uniqueNavigation();
- }
- else
- std::cout << "Couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << std::endl;
+ auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, NULL, &nav_, NULL);
+ if (stat == 1)
+ {
+ std::cout << "Navigation file loaded." << std::endl;
+ std::cout << "GPS satellites in navigation file: " << nav_.n << std::endl;
+ std::cout << "GLONASS satellites in navigation file: " << nav_.ng << std::endl;
+ std::cout << "SBAS satellites in navigation file: " << nav_.ns << std::endl;
+ std::cout << "Almanac satellites in navigation file: " << nav_.na << std::endl;
+ uniqueNavigation();
+ }
+ else
+ std::cout << "Navigation: couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << std::endl;
}
void Navigation::copyAllArrays(const nav_t& nav)
{
- //GPS/QZS/GAL ephemeris
- copyArray<eph_t>(nav.eph, nav.n, nav_.eph, nav_.n, nav_.nmax);
- //GLONASS ephemeris
- copyArray<geph_t>(nav.geph, nav.ng, nav_.geph, nav_.ng, nav_.ngmax);
- //SBAS ephemeris
- copyArray<seph_t>(nav.seph, nav.ns, nav_.seph, nav_.ns, nav_.nsmax);
- //precise ephemeris
- copyArray<peph_t>(nav.peph, nav.ne, nav_.peph, nav_.ne, nav_.nemax);
- //precise clock
- copyArray<pclk_t>(nav.pclk, nav.nc, nav_.pclk, nav_.nc, nav_.ncmax);
- //almanac data
- copyArray<alm_t>(nav.alm, nav.na, nav_.alm, nav_.na, nav_.namax);
- //tec grid data
- copyArray<tec_t>(nav.tec, nav.nt, nav_.tec, nav_.nt, nav_.ntmax);
- //satellite fcb data
- copyArray<fcbd_t>(nav.fcb, nav.nf, nav_.fcb, nav_.nf, nav_.nfmax);
+ // GPS/QZS/GAL ephemeris
+ copyArray<eph_t>(nav.eph, nav.n, nav_.eph, nav_.n, nav_.nmax);
+ // GLONASS ephemeris
+ copyArray<geph_t>(nav.geph, nav.ng, nav_.geph, nav_.ng, nav_.ngmax);
+ // SBAS ephemeris
+ copyArray<seph_t>(nav.seph, nav.ns, nav_.seph, nav_.ns, nav_.nsmax);
+ // precise ephemeris
+ copyArray<peph_t>(nav.peph, nav.ne, nav_.peph, nav_.ne, nav_.nemax);
+ // precise clock
+ copyArray<pclk_t>(nav.pclk, nav.nc, nav_.pclk, nav_.nc, nav_.ncmax);
+ // almanac data
+ copyArray<alm_t>(nav.alm, nav.na, nav_.alm, nav_.na, nav_.namax);
+ // tec grid data
+ copyArray<tec_t>(nav.tec, nav.nt, nav_.tec, nav_.nt, nav_.ntmax);
+ // satellite fcb data
+ copyArray<fcbd_t>(nav.fcb, nav.nf, nav_.fcb, nav_.nf, nav_.nfmax);
- uniqueNavigation();
+ uniqueNavigation();
}
void Navigation::copyEphemeris(const nav_t& nav)
{
- copyArray<eph_t>(nav.eph, nav.n, nav_.eph, nav_.n, nav_.nmax);
-// if (nav.eph != NULL)
-// {
-// for (int ii = 0; ii < nav.n; ++ii)
-// {
-// addEphemeris(nav.eph[ii]);
-// }
-// }
+ copyArray<eph_t>(nav.eph, nav.n, nav_.eph, nav_.n, nav_.nmax);
+ // if (nav.eph != NULL)
+ // {
+ // for (int ii = 0; ii < nav.n; ++ii)
+ // {
+ // addEphemeris(nav.eph[ii]);
+ // }
+ // }
- copyArray<geph_t>(nav.geph, nav.ng, nav_.geph, nav_.ng, nav_.ngmax);
-// if (nav.geph != NULL)
-// {
-// for (int ii = 0; ii < nav.ng; ++ii)
-// {
-// addGLONASSEphemeris(nav.geph[ii]);
-// }
-// }
+ copyArray<geph_t>(nav.geph, nav.ng, nav_.geph, nav_.ng, nav_.ngmax);
+ // if (nav.geph != NULL)
+ // {
+ // for (int ii = 0; ii < nav.ng; ++ii)
+ // {
+ // addGLONASSEphemeris(nav.geph[ii]);
+ // }
+ // }
- copyArray<seph_t>(nav.seph, nav.ns, nav_.seph, nav_.ns, nav_.nsmax);
-// if (nav.seph != NULL)
-// {
-// for (int ii = 0; ii < nav.ns; ++ii)
-// {
-// addSBASEphemeris(nav.seph[ii]);
-// }
-// }
+ copyArray<seph_t>(nav.seph, nav.ns, nav_.seph, nav_.ns, nav_.nsmax);
+ // if (nav.seph != NULL)
+ // {
+ // for (int ii = 0; ii < nav.ns; ++ii)
+ // {
+ // addSBASEphemeris(nav.seph[ii]);
+ // }
+ // }
uniqueNavigation();
}
void Navigation::copyAlmanac(const nav_t& nav)
{
- copyArray<alm_t>(nav.alm, nav.na, nav_.alm, nav_.na, nav_.namax);
-// if (nav.alm != NULL)
-// {
-// for (int ii = 0; ii < nav.na; ++ii)
-// {
-// addAlmanac(nav.alm[ii]);
-// }
-// }
+ copyArray<alm_t>(nav.alm, nav.na, nav_.alm, nav_.na, nav_.namax);
+ // if (nav.alm != NULL)
+ // {
+ // for (int ii = 0; ii < nav.na; ++ii)
+ // {
+ // addAlmanac(nav.alm[ii]);
+ // }
+ // }
}
-void Navigation::copyIonUtc(const nav_t &nav)
+void Navigation::copyIonUtc(const nav_t& nav)
{
- std::copy(nav.utc_gps, nav.utc_gps + 4, nav_.utc_gps);
- std::copy(nav.utc_gal, nav.utc_gal + 4, nav_.utc_gal);
- std::copy(nav.utc_qzs, nav.utc_qzs + 4, nav_.utc_qzs);
- std::copy(nav.utc_glo, nav.utc_glo + 4, nav_.utc_glo);
- std::copy(nav.utc_cmp, nav.utc_cmp + 4, nav_.utc_cmp);
- std::copy(nav.utc_irn, nav.utc_irn + 4, nav_.utc_irn);
- std::copy(nav.utc_sbs, nav.utc_sbs + 4, nav_.utc_sbs);
+ std::copy(nav.utc_gps, nav.utc_gps + 4, nav_.utc_gps);
+ std::copy(nav.utc_gal, nav.utc_gal + 4, nav_.utc_gal);
+ std::copy(nav.utc_qzs, nav.utc_qzs + 4, nav_.utc_qzs);
+ std::copy(nav.utc_glo, nav.utc_glo + 4, nav_.utc_glo);
+ std::copy(nav.utc_cmp, nav.utc_cmp + 4, nav_.utc_cmp);
+ std::copy(nav.utc_irn, nav.utc_irn + 4, nav_.utc_irn);
+ std::copy(nav.utc_sbs, nav.utc_sbs + 4, nav_.utc_sbs);
- std::copy(nav.ion_gps, nav.ion_gps + 8, nav_.ion_gps);
- std::copy(nav.ion_gal, nav.ion_gal + 4, nav_.ion_gal);
- std::copy(nav.ion_qzs, nav.ion_qzs + 8, nav_.ion_qzs);
- std::copy(nav.ion_cmp, nav.ion_cmp + 4, nav_.ion_cmp);
- std::copy(nav.ion_irn, nav.ion_irn + 8, nav_.ion_irn);
+ std::copy(nav.ion_gps, nav.ion_gps + 8, nav_.ion_gps);
+ std::copy(nav.ion_gal, nav.ion_gal + 4, nav_.ion_gal);
+ std::copy(nav.ion_qzs, nav.ion_qzs + 8, nav_.ion_qzs);
+ std::copy(nav.ion_cmp, nav.ion_cmp + 4, nav_.ion_cmp);
+ std::copy(nav.ion_irn, nav.ion_irn + 8, nav_.ion_irn);
- nav_.leaps = nav.leaps;
+ nav_.leaps = nav.leaps;
}
-void Navigation::copySbasCorrections(const nav_t &nav)
+void Navigation::copySbasCorrections(const nav_t& nav)
{
- nav_.sbssat.iodp = nav.sbssat.iodp;
- nav_.sbssat.nsat = nav.sbssat.nsat;
- nav_.sbssat.tlat = nav.sbssat.tlat;
- for (int i=0; i < MAXSAT; i++)
- {
- nav_.sbssat.sat[i].sat = nav.sbssat.sat[i].sat;
- nav_.sbssat.sat[i].fcorr = nav.sbssat.sat[i].fcorr;
- nav_.sbssat.sat[i].lcorr.daf0 = nav.sbssat.sat[i].lcorr.daf0;
- nav_.sbssat.sat[i].lcorr.daf1 = nav.sbssat.sat[i].lcorr.daf1;
- nav_.sbssat.sat[i].lcorr.iode = nav.sbssat.sat[i].lcorr.iode;
- nav_.sbssat.sat[i].lcorr.t0 = nav.sbssat.sat[i].lcorr.t0;
- std::copy(nav.sbssat.sat[i].lcorr.dpos, nav.sbssat.sat[i].lcorr.dpos + 3, nav_.sbssat.sat[i].lcorr.dpos);
- std::copy(nav.sbssat.sat[i].lcorr.dvel, nav.sbssat.sat[i].lcorr.dvel + 3, nav_.sbssat.sat[i].lcorr.dvel);
- }
- for (int i=0; i < MAXBAND+1; i++)
- {
- nav_.sbsion[i].iodi = nav.sbsion[i].iodi;
- nav_.sbsion[i].nigp = nav.sbsion[i].nigp;
- std::copy(nav.sbsion[i].igp, nav.sbsion[i].igp + MAXNIGP, nav_.sbsion[i].igp);
- }
+ nav_.sbssat.iodp = nav.sbssat.iodp;
+ nav_.sbssat.nsat = nav.sbssat.nsat;
+ nav_.sbssat.tlat = nav.sbssat.tlat;
+ for (int i = 0; i < MAXSAT; i++)
+ {
+ nav_.sbssat.sat[i].sat = nav.sbssat.sat[i].sat;
+ nav_.sbssat.sat[i].fcorr = nav.sbssat.sat[i].fcorr;
+ nav_.sbssat.sat[i].lcorr.daf0 = nav.sbssat.sat[i].lcorr.daf0;
+ nav_.sbssat.sat[i].lcorr.daf1 = nav.sbssat.sat[i].lcorr.daf1;
+ nav_.sbssat.sat[i].lcorr.iode = nav.sbssat.sat[i].lcorr.iode;
+ nav_.sbssat.sat[i].lcorr.t0 = nav.sbssat.sat[i].lcorr.t0;
+ std::copy(nav.sbssat.sat[i].lcorr.dpos, nav.sbssat.sat[i].lcorr.dpos + 3, nav_.sbssat.sat[i].lcorr.dpos);
+ std::copy(nav.sbssat.sat[i].lcorr.dvel, nav.sbssat.sat[i].lcorr.dvel + 3, nav_.sbssat.sat[i].lcorr.dvel);
+ }
+ for (int i = 0; i < MAXBAND + 1; i++)
+ {
+ nav_.sbsion[i].iodi = nav.sbsion[i].iodi;
+ nav_.sbsion[i].nigp = nav.sbsion[i].nigp;
+ std::copy(nav.sbsion[i].igp, nav.sbsion[i].igp + MAXNIGP, nav_.sbsion[i].igp);
+ }
}
void Navigation::freeNavigationArrays()
{
- // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
- freeEphemeris();
- //std::cout << "freeing glonass ephemeris...\n";
- freeGlonassEphemeris();
- //std::cout << "freeing sbas ephemeris...\n";
- freeSbasEphemeris();
- //std::cout << "freeing precise ephemeris...\n";
- freePreciseEphemeris();
- //std::cout << "freeing precise clock...\n";
- freePreciseClock();
- //std::cout << "freeing almanac...\n";
- freeAlmanac();
- //std::cout << "freeing tec...\n";
- freeTecData();
- //std::cout << "freeing fcb...\n";
- freeFcbData();
+ // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
+ freeEphemeris();
+ // std::cout << "freeing glonass ephemeris...\n";
+ freeGlonassEphemeris();
+ // std::cout << "freeing sbas ephemeris...\n";
+ freeSbasEphemeris();
+ // std::cout << "freeing precise ephemeris...\n";
+ freePreciseEphemeris();
+ // std::cout << "freeing precise clock...\n";
+ freePreciseClock();
+ // std::cout << "freeing almanac...\n";
+ freeAlmanac();
+ // std::cout << "freeing tec...\n";
+ freeTecData();
+ // std::cout << "freeing fcb...\n";
+ freeFcbData();
}
void Navigation::print()
@@ -271,23 +272,32 @@ void Navigation::print()
std::cout << "na: " << nav_.na << "\n";
std::cout << "nt: " << nav_.nt << "\n";
- std::cout << "nmax: " << nav_.nmax << "\n";
- std::cout << "ngmax: " << nav_.ngmax << "\n";
- std::cout << "nsmax: " << nav_.nsmax << "\n";
- std::cout << "nemax: " << nav_.nemax << "\n";
- std::cout << "ncmax: " << nav_.ncmax << "\n";
- std::cout << "namax: " << nav_.namax << "\n";
- std::cout << "ntmax: " << nav_.ntmax << "\n";
-
- std::cout << "utc_gps: " << nav_.utc_gps[0] << " " << nav_.utc_gps[1] << " " << nav_.utc_gps[2] << " " << nav_.utc_gps[3] << "\n";
- std::cout << "utc_glo: " << nav_.utc_glo[0] << " " << nav_.utc_glo[1] << " " << nav_.utc_glo[2] << " " << nav_.utc_glo[3] << "\n";
- std::cout << "utc_gal: " << nav_.utc_gal[0] << " " << nav_.utc_gal[1] << " " << nav_.utc_gal[2] << " " << nav_.utc_gal[3] << "\n";
- std::cout << "utc_qzs: " << nav_.utc_qzs[0] << " " << nav_.utc_qzs[1] << " " << nav_.utc_qzs[2] << " " << nav_.utc_qzs[3] << "\n";
- std::cout << "utc_cmp: " << nav_.utc_cmp[0] << " " << nav_.utc_cmp[1] << " " << nav_.utc_cmp[2] << " " << nav_.utc_cmp[3] << "\n";
- std::cout << "utc_sbs: " << nav_.utc_sbs[0] << " " << nav_.utc_sbs[1] << " " << nav_.utc_sbs[2] << " " << nav_.utc_sbs[3] << "\n";
- std::cout << "ion_gps: " << nav_.ion_gps[0] << " " << nav_.ion_gps[1] << " " << nav_.ion_gps[2] << " " << nav_.ion_gps[3] << "\n";
- std::cout << "ion_gal: " << nav_.ion_gal[0] << " " << nav_.ion_gal[1] << " " << nav_.ion_gal[2] << " " << nav_.ion_gal[3] << "\n";
- std::cout << "ion_qzs: " << nav_.ion_qzs[0] << " " << nav_.ion_qzs[1] << " " << nav_.ion_qzs[2] << " " << nav_.ion_qzs[3] << "\n";
- std::cout << "ion_cmp: " << nav_.ion_cmp[0] << " " << nav_.ion_cmp[1] << " " << nav_.ion_cmp[2] << " " << nav_.ion_cmp[3] << "\n";
+ std::cout << "nmax: " << nav_.nmax << "\n";
+ std::cout << "ngmax: " << nav_.ngmax << "\n";
+ std::cout << "nsmax: " << nav_.nsmax << "\n";
+ std::cout << "nemax: " << nav_.nemax << "\n";
+ std::cout << "ncmax: " << nav_.ncmax << "\n";
+ std::cout << "namax: " << nav_.namax << "\n";
+ std::cout << "ntmax: " << nav_.ntmax << "\n";
+ std::cout << "utc_gps: " << nav_.utc_gps[0] << " " << nav_.utc_gps[1] << " " << nav_.utc_gps[2] << " "
+ << nav_.utc_gps[3] << "\n";
+ std::cout << "utc_glo: " << nav_.utc_glo[0] << " " << nav_.utc_glo[1] << " " << nav_.utc_glo[2] << " "
+ << nav_.utc_glo[3] << "\n";
+ std::cout << "utc_gal: " << nav_.utc_gal[0] << " " << nav_.utc_gal[1] << " " << nav_.utc_gal[2] << " "
+ << nav_.utc_gal[3] << "\n";
+ std::cout << "utc_qzs: " << nav_.utc_qzs[0] << " " << nav_.utc_qzs[1] << " " << nav_.utc_qzs[2] << " "
+ << nav_.utc_qzs[3] << "\n";
+ std::cout << "utc_cmp: " << nav_.utc_cmp[0] << " " << nav_.utc_cmp[1] << " " << nav_.utc_cmp[2] << " "
+ << nav_.utc_cmp[3] << "\n";
+ std::cout << "utc_sbs: " << nav_.utc_sbs[0] << " " << nav_.utc_sbs[1] << " " << nav_.utc_sbs[2] << " "
+ << nav_.utc_sbs[3] << "\n";
+ std::cout << "ion_gps: " << nav_.ion_gps[0] << " " << nav_.ion_gps[1] << " " << nav_.ion_gps[2] << " "
+ << nav_.ion_gps[3] << "\n";
+ std::cout << "ion_gal: " << nav_.ion_gal[0] << " " << nav_.ion_gal[1] << " " << nav_.ion_gal[2] << " "
+ << nav_.ion_gal[3] << "\n";
+ std::cout << "ion_qzs: " << nav_.ion_qzs[0] << " " << nav_.ion_qzs[1] << " " << nav_.ion_qzs[2] << " "
+ << nav_.ion_qzs[3] << "\n";
+ std::cout << "ion_cmp: " << nav_.ion_cmp[0] << " " << nav_.ion_cmp[1] << " " << nav_.ion_cmp[2] << " "
+ << nav_.ion_cmp[3] << "\n";
}
diff --git a/src/observations.cpp b/src/observations.cpp
index 1b39857bbe0252fdf45491c6ebba405c42c8c051..ea2dc15d658ce4644e8446ae71182ce21c428d4a 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -1,38 +1,33 @@
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/observations.h"
-
using namespace GNSSUtils;
-
Observations::Observations()
{
-
}
Observations::Observations(const Observations& obs)
- : sat_2_idx_(obs.sat_2_idx_)
- , idx_2_sat_(obs.idx_2_sat_)
- , obs_(obs.obs_.size())
+ : sat_2_idx_(obs.sat_2_idx_), idx_2_sat_(obs.idx_2_sat_), obs_(obs.obs_.size())
{
- // copy all elements
- for (auto i = 0; i<obs.obs_.size(); i++)
- {
- obs_[i].time = obs.obs_[i].time;
- obs_[i].eventime = obs.obs_[i].eventime;
- obs_[i].timevalid = obs.obs_[i].timevalid;
- obs_[i].sat = obs.obs_[i].sat;
- obs_[i].rcv = obs.obs_[i].rcv;
- std::copy(obs.obs_[i].SNR, obs.obs_[i].SNR + NFREQ+NEXOBS, obs_[i].SNR);
- std::copy(obs.obs_[i].LLI, obs.obs_[i].LLI + NFREQ+NEXOBS, obs_[i].LLI);
- std::copy(obs.obs_[i].code, obs.obs_[i].code + NFREQ+NEXOBS, obs_[i].code);
- std::copy(obs.obs_[i].qualL, obs.obs_[i].qualL + NFREQ+NEXOBS, obs_[i].qualL);
- std::copy(obs.obs_[i].qualP, obs.obs_[i].qualP + NFREQ+NEXOBS, obs_[i].qualP);
- obs_[i].freq = obs.obs_[i].freq;
- std::copy(obs.obs_[i].L, obs.obs_[i].L + NFREQ+NEXOBS, obs_[i].L);
- std::copy(obs.obs_[i].P, obs.obs_[i].P + NFREQ+NEXOBS, obs_[i].P);
- std::copy(obs.obs_[i].D, obs.obs_[i].D + NFREQ+NEXOBS, obs_[i].D);
- }
+ // copy all elements
+ for (auto i = 0; i < obs.obs_.size(); i++)
+ {
+ obs_[i].time = obs.obs_[i].time;
+ obs_[i].eventime = obs.obs_[i].eventime;
+ obs_[i].timevalid = obs.obs_[i].timevalid;
+ obs_[i].sat = obs.obs_[i].sat;
+ obs_[i].rcv = obs.obs_[i].rcv;
+ std::copy(obs.obs_[i].SNR, obs.obs_[i].SNR + NFREQ + NEXOBS, obs_[i].SNR);
+ std::copy(obs.obs_[i].LLI, obs.obs_[i].LLI + NFREQ + NEXOBS, obs_[i].LLI);
+ std::copy(obs.obs_[i].code, obs.obs_[i].code + NFREQ + NEXOBS, obs_[i].code);
+ std::copy(obs.obs_[i].qualL, obs.obs_[i].qualL + NFREQ + NEXOBS, obs_[i].qualL);
+ std::copy(obs.obs_[i].qualP, obs.obs_[i].qualP + NFREQ + NEXOBS, obs_[i].qualP);
+ obs_[i].freq = obs.obs_[i].freq;
+ std::copy(obs.obs_[i].L, obs.obs_[i].L + NFREQ + NEXOBS, obs_[i].L);
+ std::copy(obs.obs_[i].P, obs.obs_[i].P + NFREQ + NEXOBS, obs_[i].P);
+ std::copy(obs.obs_[i].D, obs.obs_[i].D + NFREQ + NEXOBS, obs_[i].D);
+ }
}
Observations::~Observations()
@@ -46,116 +41,123 @@ void Observations::clearObservations()
this->sat_2_idx_.clear();
}
-void Observations::addObservation(const obsd_t & obs)
+void Observations::addObservation(const obsd_t& obs)
{
- assert(sat_2_idx_.size() == idx_2_sat_.size());
- assert(sat_2_idx_.size() == obs_.size());
-
- // copy obsd_t object
- obsd_t copy_obs;
- copy_obs.time = obs.time;
- copy_obs.eventime = obs.eventime;
- copy_obs.timevalid = obs.timevalid;
- copy_obs.sat = obs.sat;
- copy_obs.rcv = obs.rcv;
- std::copy(obs.SNR, obs.SNR + NFREQ+NEXOBS, copy_obs.SNR);
- std::copy(obs.LLI, obs.LLI + NFREQ+NEXOBS, copy_obs.LLI);
- std::copy(obs.code, obs.code + NFREQ+NEXOBS, copy_obs.code);
- std::copy(obs.qualL, obs.qualL + NFREQ+NEXOBS, copy_obs.qualL);
- std::copy(obs.qualP, obs.qualP + NFREQ+NEXOBS, copy_obs.qualP);
- copy_obs.freq = obs.freq;
- std::copy(obs.L, obs.L + NFREQ+NEXOBS, copy_obs.L);
- std::copy(obs.P, obs.P + NFREQ+NEXOBS, copy_obs.P);
- std::copy(obs.D, obs.D + NFREQ+NEXOBS, copy_obs.D);
-
- assert(!hasSatellite(copy_obs.sat) && "adding an observation of a satellite already stored!");
- this->obs_.push_back(copy_obs);
- idx_2_sat_.push_back(copy_obs.sat);
- sat_2_idx_[copy_obs.sat] = obs_.size()-1;
-
- //std::cout << "added observation of sat " << (int)obs.sat << " stored in idx " << sat_2_idx_[obs.sat] << std::endl;
-
- assert(sat_2_idx_.size() == idx_2_sat_.size());
- assert(sat_2_idx_.size() == obs_.size());
+ assert(sat_2_idx_.size() == idx_2_sat_.size());
+ assert(sat_2_idx_.size() == obs_.size());
+
+ // copy obsd_t object
+ obsd_t copy_obs;
+ copy_obs.time = obs.time;
+ copy_obs.eventime = obs.eventime;
+ copy_obs.timevalid = obs.timevalid;
+ copy_obs.sat = obs.sat;
+ copy_obs.rcv = obs.rcv;
+ std::copy(obs.SNR, obs.SNR + NFREQ + NEXOBS, copy_obs.SNR);
+ std::copy(obs.LLI, obs.LLI + NFREQ + NEXOBS, copy_obs.LLI);
+ std::copy(obs.code, obs.code + NFREQ + NEXOBS, copy_obs.code);
+ std::copy(obs.qualL, obs.qualL + NFREQ + NEXOBS, copy_obs.qualL);
+ std::copy(obs.qualP, obs.qualP + NFREQ + NEXOBS, copy_obs.qualP);
+ copy_obs.freq = obs.freq;
+ std::copy(obs.L, obs.L + NFREQ + NEXOBS, copy_obs.L);
+ std::copy(obs.P, obs.P + NFREQ + NEXOBS, copy_obs.P);
+ std::copy(obs.D, obs.D + NFREQ + NEXOBS, copy_obs.D);
+
+ assert(!hasSatellite(copy_obs.sat) && "adding an observation of a satellite already stored!");
+ this->obs_.push_back(copy_obs);
+ idx_2_sat_.push_back(copy_obs.sat);
+ sat_2_idx_[copy_obs.sat] = obs_.size() - 1;
+
+ // std::cout << "added observation of sat " << (int)obs.sat << " stored in idx " << sat_2_idx_[obs.sat] << std::endl;
+
+ assert(sat_2_idx_.size() == idx_2_sat_.size());
+ assert(sat_2_idx_.size() == obs_.size());
}
-void Observations::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt)
+void Observations::loadFromRinex(const std::string& rnx_file,
+ gtime_t t_start,
+ gtime_t t_end,
+ double dt,
+ const char* opt)
{
- obs_t obs;
- obs.data = (obsd_t *) malloc(sizeof(obsd_t)*MAXSAT);
- obs.n = 0;
- obs.nmax = MAXSAT;
-
- //const char* opt = "";
- auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, &obs, NULL, NULL);
- if (stat == 1)
- sortobs(&obs);
- else
- {
- std::cout << "Couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << std::endl;
- return;
- }
+ obs_t obs;
+ obs.data = (obsd_t*)malloc(sizeof(obsd_t) * MAXSAT);
+ obs.n = 0;
+ obs.nmax = MAXSAT;
+
+ // const char* opt = "";
+ auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, &obs, NULL, NULL);
+ if (stat == 1)
+ sortobs(&obs);
+ else
+ {
+ std::cout << "Observation: couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << stat << std::endl;
+ return;
+ }
- for (int i=0; i < obs.n; i++)
- {
- // std::cout << "time: " << time_str(obs.data[i].time, 3) << " | sat: " << int(obs.data[i].sat) << " | rcv: " << int(obs.data[i].rcv) <<
- // " | SNR: " << int(obs.data[i].SNR[0]) << " | LLI: " << int(obs.data[i].LLI[0]) << " | code: " << int(obs.data[i].code[0]) <<
- // " | L: " << obs.data[i].L[0] << " | P: " << obs.data[i].P[0] << " | D: " << obs.data[i].D[0] << std::endl;
- addObservation(obs.data[i]);
- }
+ for (int i = 0; i < obs.n; i++)
+ {
+ // std::cout << "time: " << time_str(obs.data[i].time, 3) << " | sat: " << int(obs.data[i].sat) << " | rcv: " <<
+ // int(obs.data[i].rcv) <<
+ // " | SNR: " << int(obs.data[i].SNR[0]) << " | LLI: " << int(obs.data[i].LLI[0]) << " | code: " <<
+ // int(obs.data[i].code[0]) <<
+ // " | L: " << obs.data[i].L[0] << " | P: " << obs.data[i].P[0] << " | D: " << obs.data[i].D[0] <<
+ // std::endl;
+ addObservation(obs.data[i]);
+ }
- free(obs.data);
+ free(obs.data);
}
void Observations::removeObservationByIdx(const int& _idx)
{
- //std::cout << "removing observation of idx " << _idx << std::endl;
- assert(sat_2_idx_.size() == idx_2_sat_.size());
- assert(sat_2_idx_.size() == obs_.size());
- assert(obs_.size() > _idx);
- assert(idx_2_sat_.size() > _idx);
- assert(sat_2_idx_.count(idx_2_sat_.at(_idx)) != 0);
-
- // remove obs from sat_2_idx map
- sat_2_idx_.erase(idx_2_sat_.at(_idx));
- // decrease the idx of the observations after the removed one
- for (auto&& sat_idx : sat_2_idx_)
- if (sat_idx.second > _idx)
- sat_idx.second--;
- // remove obs from obs and idx_2_sat vectors
- obs_.erase(obs_.begin() + _idx);
- idx_2_sat_.erase(idx_2_sat_.begin() + _idx);
-
- assert(sat_2_idx_.size() == idx_2_sat_.size());
- assert(sat_2_idx_.size() == obs_.size());
+ // std::cout << "removing observation of idx " << _idx << std::endl;
+ assert(sat_2_idx_.size() == idx_2_sat_.size());
+ assert(sat_2_idx_.size() == obs_.size());
+ assert(obs_.size() > _idx);
+ assert(idx_2_sat_.size() > _idx);
+ assert(sat_2_idx_.count(idx_2_sat_.at(_idx)) != 0);
+
+ // remove obs from sat_2_idx map
+ sat_2_idx_.erase(idx_2_sat_.at(_idx));
+ // decrease the idx of the observations after the removed one
+ for (auto&& sat_idx : sat_2_idx_)
+ if (sat_idx.second > _idx)
+ sat_idx.second--;
+ // remove obs from obs and idx_2_sat vectors
+ obs_.erase(obs_.begin() + _idx);
+ idx_2_sat_.erase(idx_2_sat_.begin() + _idx);
+
+ assert(sat_2_idx_.size() == idx_2_sat_.size());
+ assert(sat_2_idx_.size() == obs_.size());
}
void Observations::removeObservationBySat(const int& _sat)
{
- //std::cout << "removing observation of sat " << _sat << std::endl;
- assert(sat_2_idx_.size() == idx_2_sat_.size());
- assert(sat_2_idx_.size() == obs_.size());
- assert(sat_2_idx_.count(_sat) != 0);
- assert(hasSatellite(_sat));
- assert(obs_.size() > sat_2_idx_.at(_sat));
- assert(idx_2_sat_.size() > sat_2_idx_.at(_sat));
-
- int idx = sat_2_idx_.at(_sat);
- // remove obs from sat_2_idx map
- sat_2_idx_.erase(_sat);
- // decrease the idx of the observations after the removed one
- for (auto&& sat_idx : sat_2_idx_)
- if (sat_idx.second > idx)
- sat_idx.second--;
- // remove obs from obs and idx_2_sat vectors
- obs_.erase(obs_.begin() + idx);
- idx_2_sat_.erase(idx_2_sat_.begin() + idx);
-
- assert(sat_2_idx_.size() == idx_2_sat_.size());
- assert(sat_2_idx_.size() == obs_.size());
+ // std::cout << "removing observation of sat " << _sat << std::endl;
+ assert(sat_2_idx_.size() == idx_2_sat_.size());
+ assert(sat_2_idx_.size() == obs_.size());
+ assert(sat_2_idx_.count(_sat) != 0);
+ assert(hasSatellite(_sat));
+ assert(obs_.size() > sat_2_idx_.at(_sat));
+ assert(idx_2_sat_.size() > sat_2_idx_.at(_sat));
+
+ int idx = sat_2_idx_.at(_sat);
+ // remove obs from sat_2_idx map
+ sat_2_idx_.erase(_sat);
+ // decrease the idx of the observations after the removed one
+ for (auto&& sat_idx : sat_2_idx_)
+ if (sat_idx.second > idx)
+ sat_idx.second--;
+ // remove obs from obs and idx_2_sat vectors
+ obs_.erase(obs_.begin() + idx);
+ idx_2_sat_.erase(idx_2_sat_.begin() + idx);
+
+ assert(sat_2_idx_.size() == idx_2_sat_.size());
+ assert(sat_2_idx_.size() == obs_.size());
}
-void Observations::print(obsd_t & _obs)
+void Observations::print(obsd_t& _obs)
{
std::string msg = "Observation of satellite #" + std::to_string(_obs.sat);
GNSSUtils::print(msg);
@@ -186,33 +188,34 @@ void Observations::print()
}
}
-void Observations::findCommonObservations(const Observations& obs_1, const Observations& obs_2,
- Observations& common_obs_1, Observations& common_obs_2)
+void Observations::findCommonObservations(const Observations& obs_1,
+ const Observations& obs_2,
+ Observations& common_obs_1,
+ Observations& common_obs_2)
{
- // clear and reserve
- common_obs_1.clearObservations();
- common_obs_2.clearObservations();
-
- //std::cout << "obs 1 sats: ";
- //for (auto&& obs_1_ref : obs_1.getObservations())
- // std::cout << (int)obs_1_ref.sat << " ";
- //std::cout << std::endl;
- //std::cout << "obs 2 sats: ";
- //for (auto&& obs_2_ref : obs_2.getObservations())
- // std::cout << (int)obs_2_ref.sat << " ";
- //std::cout << std::endl;
-
- // iterate 1
- for (auto&& obs_1_ref : obs_1.getObservations())
- if (obs_2.hasSatellite(obs_1_ref.sat))
- {
- common_obs_1.addObservation(obs_1_ref);
- common_obs_2.addObservation(obs_2.getObservationBySat(obs_1_ref.sat));
- }
-
- //std::cout << "common sats: ";
- //for (auto&& obs_2_ref : common_obs_1.getObservations())
- // std::cout << (int)obs_2_ref.sat << " ";
- //std::cout << std::endl;
+ // clear and reserve
+ common_obs_1.clearObservations();
+ common_obs_2.clearObservations();
+
+ // std::cout << "obs 1 sats: ";
+ // for (auto&& obs_1_ref : obs_1.getObservations())
+ // std::cout << (int)obs_1_ref.sat << " ";
+ // std::cout << std::endl;
+ // std::cout << "obs 2 sats: ";
+ // for (auto&& obs_2_ref : obs_2.getObservations())
+ // std::cout << (int)obs_2_ref.sat << " ";
+ // std::cout << std::endl;
+
+ // iterate 1
+ for (auto&& obs_1_ref : obs_1.getObservations())
+ if (obs_2.hasSatellite(obs_1_ref.sat))
+ {
+ common_obs_1.addObservation(obs_1_ref);
+ common_obs_2.addObservation(obs_2.getObservationBySat(obs_1_ref.sat));
+ }
+ // std::cout << "common sats: ";
+ // for (auto&& obs_2_ref : common_obs_1.getObservations())
+ // std::cout << (int)obs_2_ref.sat << " ";
+ // std::cout << std::endl;
}
diff --git a/src/ublox_raw.cpp b/src/ublox_raw.cpp
index ab7b7399edbf49e24d87177d5cf404fed70a36ec..9cc2442d19ddaefe97e7741878a21a565fffbb50 100644
--- a/src/ublox_raw.cpp
+++ b/src/ublox_raw.cpp
@@ -2,98 +2,97 @@
using namespace GNSSUtils;
-UBloxRaw::UBloxRaw() :
- raw_data_type_(NO)
+UBloxRaw::UBloxRaw() : raw_data_type_(NO)
{
- if (init_raw(&raw_data_, STRFMT_UBX) == 0)
- {
- assert("Failed when allocating memory for raw_t");
- return;
- }
+ if (init_raw(&raw_data_, STRFMT_UBX) == 0)
+ {
+ assert("Failed when allocating memory for raw_t");
+ return;
+ }
};
UBloxRaw::~UBloxRaw()
{
- free_raw(&raw_data_);
+ free_raw(&raw_data_);
};
RawDataType UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream)
{
- // Update type based on RTKLIB
- for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte)
- {
- raw_data_type_ = input_ubx(&raw_data_, (unsigned char)*data_byte);
+ // Update type based on RTKLIB
+ for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte)
+ {
+ raw_data_type_ = input_ubx(&raw_data_, (unsigned char)*data_byte);
- switch (raw_data_type_)
- {
- case NO: //
- //std::cout << "0 received!\n";
- break;
+ switch (raw_data_type_)
+ {
+ case NO: //
+ // std::cout << "0 received!\n";
+ break;
- case OBS: // Observations
- std::cout << "Observations received!\n";
- updateObservations();
- //std::cout << "Observations updated!\n";
- break;
+ case OBS: // Observations
+ // std::cout << "Observations received!\n";
+ updateObservations();
+ // std::cout << "Observations updated!\n";
+ break;
- case NAV_EPH: // Ephemeris
- std::cout << "Ephemeris received!\n";
- nav_.copyEphemeris(raw_data_.nav);
- //std::cout << "Ephemeris copied!\n";
- break;
+ case NAV_EPH: // Ephemeris
+ std::cout << "Ephemeris received!\n";
+ nav_.copyEphemeris(raw_data_.nav);
+ // std::cout << "Ephemeris copied!\n";
+ break;
- case NAV_SBAS: // SBAS
- std::cout << "SBAS received!\n";
- nav_.addSbasMessage(raw_data_.sbsmsg);
- //std::cout << "SBAS added!\n";
- break;
+ case NAV_SBAS: // SBAS
+ std::cout << "SBAS received!\n";
+ nav_.addSbasMessage(raw_data_.sbsmsg);
+ // std::cout << "SBAS added!\n";
+ break;
- case NAV_ALM: // Almanac and ion/utc parameters
- std::cout << "Almanac and ion/utc parameters received!\n";
- nav_.freeAlmanac();
- //std::cout << "Almanac freed!\n";
- nav_.copyAlmanac(raw_data_.nav);
- //std::cout << "Almanac copied!\n";
- nav_.copyIonUtc(raw_data_.nav);
- //std::cout << "ION UTC copied!\n";
- break;
+ case NAV_ALM: // Almanac and ion/utc parameters
+ std::cout << "Almanac and ion/utc parameters received!\n";
+ nav_.freeAlmanac();
+ // std::cout << "Almanac freed!\n";
+ nav_.copyAlmanac(raw_data_.nav);
+ // std::cout << "Almanac copied!\n";
+ nav_.copyIonUtc(raw_data_.nav);
+ // std::cout << "ION UTC copied!\n";
+ break;
- // Not handled messages
- case NAV_ANT:
- std::cout << "UBloxRaw: Received antenna postion parameters. Not handled.\n";
- break;
- case NAV_DGPS:
- std::cout << "UBloxRaw: Received dgps correction. Not handled.\n";
- break;
- case NAV_SSR:
- std::cout << "UBloxRaw: Received ssr message. Not handled.\n";
- break;
- case NAV_LEX:
- std::cout << "UBloxRaw: Received lex message. Not handled.\n";
- break;
- case ERROR:
- std::cout << "UBloxRaw: Received error message. Not handled.\n";
- break;
- default:
- std::cout << "UBloxRaw: Received unknown message. Not handled.\n";
- break;
- }
+ // Not handled messages
+ case NAV_ANT:
+ std::cout << "UBloxRaw: Received antenna postion parameters. Not handled.\n";
+ break;
+ case NAV_DGPS:
+ std::cout << "UBloxRaw: Received dgps correction. Not handled.\n";
+ break;
+ case NAV_SSR:
+ std::cout << "UBloxRaw: Received ssr message. Not handled.\n";
+ break;
+ case NAV_LEX:
+ std::cout << "UBloxRaw: Received lex message. Not handled.\n";
+ break;
+ case ERROR:
+ std::cout << "UBloxRaw: Received error message. Not handled.\n";
+ break;
+ default:
+ std::cout << "UBloxRaw: Received unknown message. Not handled.\n";
+ break;
}
+ }
- return raw_data_type_;
+ return raw_data_type_;
}
void UBloxRaw::updateObservations()
{
- // sort and remove duplicated observations
- sortobs(&raw_data_.obs);
- // std::cout << "---------------------------JUST BEFORE!-------------------" << std::endl;
- // obs_.print();
+ // sort and remove duplicated observations
+ sortobs(&raw_data_.obs);
+ // std::cout << "---------------------------JUST BEFORE!-------------------" << std::endl;
+ // obs_.print();
- obs_.clearObservations();
- for (int ii = 0; ii < raw_data_.obs.n; ++ii)
- obs_.addObservation(raw_data_.obs.data[ii]);
+ obs_.clearObservations();
+ for (int ii = 0; ii < raw_data_.obs.n; ++ii)
+ obs_.addObservation(raw_data_.obs.data[ii]);
- // std::cout << "--------------------------JUST AFTER!---------------------" << std::endl;
- // obs_.print();
+ // std::cout << "--------------------------JUST AFTER!---------------------" << std::endl;
+ // obs_.print();
}
diff --git a/src/utils.cpp b/src/utils.cpp
index c967e3f7c26ea944279401f68777fb99b486a789..cd697b39951aa113fb7a160f9b75722cbdf740f7 100644
--- a/src/utils.cpp
+++ b/src/utils.cpp
@@ -1,66 +1,64 @@
#include "gnss_utils/utils.h"
-
namespace GNSSUtils
{
-void print(std::string & _msg)
- {
- std::string msg = GNSSUTILS_MSG + _msg;
+void print(std::string& _msg)
+{
+ std::string msg = GNSSUTILS_MSG + _msg;
- std::cout << msg << "\n";
-
- }
+ 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, 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)
+void printArray(std::string _name, unsigned char* _array, int size)
+{
+ std::cout << _name << ": [";
+ for (int ii = 0; ii < size; ++ii)
{
- 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 << ",";
- }
+ 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, 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 << ",";
- }
+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/test/CMakeLists.txt b/test/CMakeLists.txt
index dcf7deb1c88398bfe18e60e9499ebdfdc152fd58..93365929c3f20d1f751e1e5d6ff5476adf3a5249 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -15,3 +15,9 @@ include_directories(${GTEST_INCLUDE_DIRS})
# Transformations test
gnss_utils_add_gtest(gtest_transformations gtest_transformations.cpp)
target_link_libraries(gtest_transformations ${PROJECT_NAME})
+
+# Observations test
+add_executable(gtest_observations gtest_observations.cpp)
+add_dependencies(gtest_observations libgtest)
+target_link_libraries(gtest_observations libgtest ${PROJECT_NAME})
+add_test(NAME gtest_observations COMMAND gtest_observations "${CMAKE_CURRENT_LIST_DIR}/../src/examples/sample_data.obs")
diff --git a/test/gtest_navigation.cpp b/test/gtest_navigation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5170cf11310248899fd967b3b870f974a6368b2f
--- /dev/null
+++ b/test/gtest_navigation.cpp
@@ -0,0 +1,13 @@
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/gnss_utils.h"
+
+TEST(NavigationTests, Whatever)
+{
+
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/test/gtest_observations.cpp b/test/gtest_observations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fc24115a0c7198f24b9b19718d219a9928ef78db
--- /dev/null
+++ b/test/gtest_observations.cpp
@@ -0,0 +1,75 @@
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/observations.h"
+
+using namespace GNSSUtils;
+
+std::string rnx_file;
+obs_t obs;
+const gtime_t t_start{ 0, 0 }; // no limit
+const gtime_t t_end{ 0, 0 }; // no limit
+const double dt = 0.0; // no limit
+const char* opt = "-SYS=G"; // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
+
+void loadRinex()
+{
+ // RTKLIB utilities
+ obs.data = (obsd_t*)malloc(sizeof(obsd_t) * MAXSAT);
+ obs.n = 0;
+ obs.nmax = MAXSAT;
+
+ int stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, &obs, NULL, NULL);
+
+ ASSERT_EQ(stat, 1);
+ sortobs(&obs);
+}
+
+TEST(ObservationsTest, AddClearObservation)
+{
+ loadRinex();
+
+ Observations observations;
+
+ // testing addition
+ for (int ii = 0; ii < obs.n; ++ii)
+ {
+ observations.addObservation(obs.data[ii]);
+ ASSERT_TRUE(equalObservations(obs.data[ii], observations.getObservations()[ii]));
+ }
+
+ ASSERT_TRUE(obs.n == observations.getObservations().size());
+
+ //Testing clear
+ observations.clearObservations();
+ ASSERT_TRUE(0 == observations.getObservations().size());
+
+}
+
+TEST(ObservationsTest, LoadFromRinex)
+{
+ // GNSSUtils utilities
+ Observations observations;
+ observations.loadFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+ observations.print();
+
+ // RTKLIB utilities
+ loadRinex();
+ ASSERT_EQ(obs.n, 6);
+
+ // Comparison
+ ASSERT_TRUE(obs.n == observations.getObservations().size());
+
+ for (int ii = 0; ii < obs.n; ++ii)
+ {
+ ASSERT_TRUE(equalObservations(obs.data[ii], observations.getObservations()[ii]));
+ }
+
+ free(obs.data);
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ rnx_file = argv[1];
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file