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/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 cc9f65be15cadcb21aebf8f9505dbf4931e54008..929ccd8767017382bd520780e62e4c32140be6fb 100644 --- a/include/gnss_utils/ublox_raw.h +++ b/include/gnss_utils/ublox_raw.h @@ -5,42 +5,52 @@ 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(); RawDataType addDataStream(const std::vector<u_int8_t>& data_stream); - + Observations& getObservations(); - Navigation& getNavigation(); + Navigation& getNavigation(); RawDataType getRawDataType() const; - private: +private: raw_t raw_data_; Observations obs_; - Navigation nav_; + Navigation nav_; RawDataType raw_data_type_; - - void updateObservations(); + void updateObservations(); }; inline GNSSUtils::Observations& UBloxRaw::getObservations() { - return obs_; + return obs_; } inline 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..6b66334719c894be87acb930224220337162f1c5 100644 --- a/src/examples/gnss_utils_test.cpp +++ b/src/examples/gnss_utils_test.cpp @@ -8,32 +8,29 @@ #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 = "-SYS=G"; // 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: "" // load observations from RINEX file observations.loadFromRinex("../src/examples/raw_201805171357.obs", t_start, t_end, dt, opt); @@ -51,26 +48,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 +76,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 +86,7 @@ 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; } diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp index 856755d1c10f46f8aa15b852c48cefd10d8cc2f4..6417b51cda0fbbc3aef3535496bed5020f23c4a0 100644 --- a/src/gnss_utils.cpp +++ b/src/gnss_utils.cpp @@ -12,44 +12,41 @@ ComputePosOutput computePos(const GNSSUtils::Observations& _observations, // Define error msg char msg[128] = ""; - // Remove duplicated satellites - _navigation.uniqueNavigation(); - - GNSSUtils::ComputePosOutput output; - sol_t sol; - sol = {{0}}; - - output.pos_stat = pntpos(_observations.data(), _observations.size(), - &(_navigation.getNavigation()), - &_prcopt, &sol, NULL, NULL, msg); - - if (output.pos_stat == 0) - { - std::cout << "computePos: error in computing positioning, message: " << msg << "\n"; - } - - output.time = sol.time.time; - output.sec = sol.time.sec; - output.pos = Eigen::Vector3d(sol.rr); - 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; + // Remove duplicated satellites + _navigation.uniqueNavigation(); + + GNSSUtils::ComputePosOutput output; + sol_t sol; + sol = { { 0 } }; + + output.pos_stat = pntpos( + _observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg); + + if (output.pos_stat == 0) + { + std::cout << "computePos: error in computing positioning, message: " << msg << "\n"; + } + + output.time = sol.time.time; + output.sec = sol.time.sec; + output.pos = Eigen::Vector3d(sol.rr); + 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; } // ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations, diff --git a/src/navigation.cpp b/src/navigation.cpp index a3e3dac38d96b00f539d712b76e5f9f8465dba14..a1ccfd7a04671fb7f69f9c4174b374b60a6b116e 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 == 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; } 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..98b72a6ad523e15833219def37c160163802e14f 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 << "Couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << 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 8fdc8cbfa8f09ff7b71f65a5c245c8edeb1cf297..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