diff --git a/CMakeLists.txt b/CMakeLists.txt index b0da1a80eb594eb158b4901a1ceb7f0c026fabc9..33c532478665f879b3658b0f173f2745d25bc8cc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,7 +39,106 @@ endif() set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fpermissive") -ADD_SUBDIRECTORY(src) +IF(NOT BUILD_TESTS) + OPTION(BUILD_TESTS "Build Unit tests" ON) +ENDIF(NOT BUILD_TESTS) + +# rtklib path +SET(RTKLIB_DIR deps/RTKLIB) +SET(RTKLIB_SRC_DIR ${RTKLIB_DIR}/src) + +# driver source files +SET(SOURCES + src/gnss_utils.cpp + src/observations.cpp + src/navigation.cpp + src/TDCP.cpp + src/utils.cpp + src/ublox_raw.cpp) + +SET(RTKLIB_SRC + ${RTKLIB_SRC_DIR}/pntpos.c + ${RTKLIB_SRC_DIR}/rtkcmn.c + ${RTKLIB_SRC_DIR}/sbas.c + ${RTKLIB_SRC_DIR}/ephemeris.c + ${RTKLIB_SRC_DIR}/preceph.c + ${RTKLIB_SRC_DIR}/qzslex.c + ${RTKLIB_SRC_DIR}/rtcm.c + ${RTKLIB_SRC_DIR}/rtcm2.c + ${RTKLIB_SRC_DIR}/rtcm3.c + ${RTKLIB_SRC_DIR}/rtcm3e.c + ${RTKLIB_SRC_DIR}/ionex.c + ${RTKLIB_SRC_DIR}/rinex.c + ${RTKLIB_SRC_DIR}/rcvraw.c + ${RTKLIB_SRC_DIR}/rcv/binex.c + ${RTKLIB_SRC_DIR}/rcv/cmr.c + ${RTKLIB_SRC_DIR}/rcv/comnav.c + ${RTKLIB_SRC_DIR}/rcv/crescent.c + ${RTKLIB_SRC_DIR}/rcv/gw10.c + ${RTKLIB_SRC_DIR}/rcv/javad.c + ${RTKLIB_SRC_DIR}/rcv/novatel.c + ${RTKLIB_SRC_DIR}/rcv/nvs.c + ${RTKLIB_SRC_DIR}/rcv/rt17.c + ${RTKLIB_SRC_DIR}/rcv/septentrio.c + ${RTKLIB_SRC_DIR}/rcv/skytraq.c + ${RTKLIB_SRC_DIR}/rcv/swiftnav.c + ${RTKLIB_SRC_DIR}/rcv/tersus.c + ${RTKLIB_SRC_DIR}/rcv/ublox.c) + + +# application header files +SET(HEADERS + include/gnss_utils/gnss_utils.h + include/gnss_utils/utils.h + include/gnss_utils/observations.h + include/gnss_utils/navigation.h + include/gnss_utils/TDCP.h + include/gnss_utils/ublox_raw.h + ${RTKLIB_SRC_DIR}/rtklib.h) + +# Eigen ####### +FIND_PACKAGE(Eigen3 REQUIRED) + +# Boost ####### +set(Boost_USE_STATIC_LIBS OFF) +set(Boost_USE_MULTITHREADED OFF) +set(Boost_USE_STATIC_RUNTIME OFF) +FIND_PACKAGE(Boost REQUIRED) + +# Adding libraries' directories +link_directories(/usr/lib/x86_64-linux-gnu/) + +# Adding include directories +INCLUDE_DIRECTORIES(include/ ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${RTKLIB_SRC_DIR}) + +# create the shared library +ADD_LIBRARY(gnss_utils SHARED ${SOURCES} ${RTKLIB_SRC}) +TARGET_LINK_LIBRARIES(gnss_utils ${Boost_LIBRARIES}) + +# Installing +INSTALL(TARGETS ${PROJECT_NAME} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib/iri-algorithms + ARCHIVE DESTINATION lib/iri-algorithms) +INSTALL(FILES ${HEADERS} DESTINATION include/iri-algorithms/gnss_utils) +INSTALL(FILES Findgnss_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/) + + +# Examples +ADD_SUBDIRECTORY(src/examples) +if(BUILD_TESTS) + # Enables testing for this directory and below. + # Note that ctest expects to find a test file in the build directory root. + # Therefore, this command should be in the source directory root. + #include(CTest) # according to http://public.kitware.com/pipermail/cmake/2012-June/050853.html + MESSAGE("Building tests.") + enable_testing() +endif() + +# Testing +if(BUILD_TESTS) + add_subdirectory(test) +endif() FIND_PACKAGE(Doxygen) diff --git a/README.md b/README.md new file mode 100644 index 0000000000000000000000000000000000000000..8db7c470a0e95d47e2f01e033e8636ba77959a91 --- /dev/null +++ b/README.md @@ -0,0 +1,26 @@ +# Installation + +Clone the repository in your chosen directory. In a new terminal: + +``` +cd <your/path> +git clone <repo-link> +``` + +Clone the RTKLIB lib as a sub-module: +``` +git submodule update --init +``` + +Create a directory to build all the source files: +``` +$ mkdir build +$ cd build +``` + +Compile the source files: + +``` +$ cmake .. +$ make +``` \ No newline at end of file diff --git a/include/gnss_utils/TDCP.h b/include/gnss_utils/TDCP.h new file mode 100644 index 0000000000000000000000000000000000000000..439a981ad097ea568d1cda9321b31c9e1b86fa55 --- /dev/null +++ b/include/gnss_utils/TDCP.h @@ -0,0 +1,85 @@ +/* + * TDCP.h + * + * Created on: Dec 4, 2019 + * Author: jvallve + */ + +#ifndef INCLUDE_GNSS_UTILS_TDCP_H_ +#define INCLUDE_GNSS_UTILS_TDCP_H_ + +#define GNSS_UTILS_TDCP_DEBUG 0 + +#include <set> +#include "gnss_utils/gnss_utils.h" +#include "gnss_utils/observations.h" +#include "gnss_utils/navigation.h" + +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); + +} // namespace GNSSUtils + +#endif /* INCLUDE_GNSS_UTILS_TDCP_H_ */ diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h index 626030968e0fa13278085403435d955227a8a2f3..a16976acdec9a321d0b47fa1f4c647fab896940b 100644 --- a/include/gnss_utils/gnss_utils.h +++ b/include/gnss_utils/gnss_utils.h @@ -10,39 +10,95 @@ #include <eigen3/Eigen/Geometry> #include <eigen3/Eigen/Sparse> -#include "observations.h" -#include "navigation.h" +#include "gnss_utils/observations.h" +#include "gnss_utils/navigation.h" -extern "C" -{ - #include "rtklib.h" +extern "C" { +#include "rtklib.h" } namespace GNSSUtils { - struct ComputePosOutput{ - time_t time; - double sec; - Eigen::Vector3d pos; // position (m) - Eigen::Vector3d vel; // velocity (m/s) - Eigen::Matrix3d pos_covar; // position covariance (m^2) - // {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} - Eigen::VectorXd rcv_bias; // receiver clock bias to time systems (s) - int type; // type (0:xyz-ecef,1:enu-baseline) - int stat; // solution status (SOLQ_???) - int ns; // number of valid satellites - double age; // age of differential (s) - double ratio; // AR ratio factor for valiation - - int pos_stat; // return from pntpos - Eigen::Vector3d lat_lon; // latitude_longitude_altitude - }; - - ComputePosOutput computePos(const Observations & _observations, - Navigation & _navigation, - const prcopt_t & _prcopt); - - Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef); +struct ComputePosOutput +{ + time_t time; + double sec; + Eigen::Vector3d pos; // position (m) + Eigen::Vector3d vel; // velocity (m/s) + Eigen::Matrix3d pos_covar; // position covariance (m^2) + // {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} + Eigen::VectorXd rcv_bias; // receiver clock bias to time systems (s) + int type; // coordinates used (0:xyz-ecef,1:enu-baseline) + int stat; // solution status (SOLQ_???) + int ns; // number of valid satellites + double age; // age of differential (s) + double ratio; // AR ratio factor for valiation + + int pos_stat; // return from pntpos + Eigen::Vector3d lat_lon; // latitude_longitude_altitude +}; + +ComputePosOutput computePos(const Observations& _observations, Navigation& _navigation, const prcopt_t& _prcopt); + +// ComputePosOutput computePosOwn(const Observations & _observations, +// Navigation & _navigation, +// const prcopt_t & _prcopt); + +// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav, +// const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, +// char *msg); + +int estposOwn(const obsd_t* obs, + int n, + const double* rs, + const double* dts, + const double* vare, + const int* svh, + const nav_t* nav, + const prcopt_t* opt, + sol_t* sol, + double* azel, + int* vsat, + double* resp, + char* msg); + +Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef); +Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon); +Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef); +Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu); + +void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, + Eigen::Matrix3d& R_ENU_ECEF, + Eigen::Vector3d& t_ENU_ECEF); +void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, + Eigen::Matrix3d& R_ENU_ECEF, + Eigen::Vector3d& t_ENU_ECEF); + +double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef); + +void computeSatellitesPositions(const Observations& obs, + const Navigation& nav, + const prcopt_t& opt, + std::map<int, Eigen::Vector3d>& sats_pos); + +template <typename T> +bool equalArray(const T* array1, const T* array2, const int& size1, const int& size2) +{ + if (size1 != size2) + return false; + + for (int ii = 0; ii < size1; ++ii) + { + if (array1[ii] != array2[ii]) + return false; + } + + return true; } +bool equalTime(const gtime_t& time1, const gtime_t& time2); +bool equalObservations(const obsd_t& obs1, const obsd_t& obs2); +bool equalObservations(const obs_t& obs1, const obs_t& obs2); + +} // namespace GNSSUtils #endif diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h index d93ed0fe4c1715c3ee11bada887674e452dade2f..20f65a0ffb1e111a5ba68f5a1a4f22693e73b426 100644 --- a/include/gnss_utils/navigation.h +++ b/include/gnss_utils/navigation.h @@ -5,58 +5,361 @@ #include <iostream> #include <memory> +extern "C" { +#include "rtklib.h" +} + +namespace GNSSUtils +{ +class Navigation; +typedef std::shared_ptr<Navigation> NavigationPtr; +typedef std::shared_ptr<const Navigation> NavigationConstPtr; -extern "C" +class Navigation { - #include "rtklib.h" +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 +{ + uniqnav(&nav_); } -namespace GNSSUtils +inline const nav_t& Navigation::getNavigation() const { - class Navigation - { - public: - // Constructor & Destructor - Navigation(); - ~Navigation(); + return nav_; +} + +inline nav_t& Navigation::getNavigation() +{ + return nav_; +} + +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; +} + +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; +} + +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; +} + +inline bool Navigation::addPreciseEphemeris(const peph_t& peph) +{ + 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); +} + +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; +} + +inline bool Navigation::addTec(const tec_t& tec) +{ + 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); +} + +inline void Navigation::addSbasMessage(const sbsmsg_t& sbas_msg) +{ + sbsupdatecorr(&sbas_msg, &nav_); +} - // Public objects +inline void Navigation::freeEphemeris() +{ + freeEph(nav_); +} - // Public methods +inline void Navigation::freeGlonassEphemeris() +{ + freeGeph(nav_); +} + +inline void Navigation::freePreciseEphemeris() +{ + freePeph(nav_); +} + +inline void Navigation::freeSbasEphemeris() +{ + freeSeph(nav_); +} + +inline void Navigation::freePreciseClock() +{ + freePclk(nav_); +} + +inline void Navigation::freeTecData() +{ + freeTec(nav_); +} + +inline void Navigation::freeFcbData() +{ + freeFcb(nav_); +} + +inline void Navigation::freeAlmanac() +{ + 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) + { + // 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; + } + // std::cout << "addToArray: adding element " << n << "\n"; + array[n++] = new_element; + // std::cout << "addToArray: added!\n"; + return true; +} - void clearNavigation(); +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; - void setNavigation(nav_t nav); + // std::cout << "copyArray: array in not null\n"; - const nav_t & getNavigation() const; - nav_t & getNavigation(); + 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: failed to add..\n"; + return false; + } + // std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n"; + } + // std::cout << "copyArray: all copied\n"; - /****************** Array memory management ******************/ + return true; +} +template <typename T> +void Navigation::freeArray(T*& array, int& n, int& nmax) +{ + if (array != NULL) + free(array); + array = NULL; + n = nmax = 0; +} - // Ephemeris - void allocateEphemeris(int n_sat = MAXSAT); - void deleteEphemeris(); +inline void Navigation::freeEph(nav_t& nav) +{ + freeArray<eph_t>(nav.eph, nav.n, nav.nmax); +} - void allocateGLONASSEphemeris(int n_sat = NSATGLO); - void deleteGLONASSEphemeris(); +inline void Navigation::freeGeph(nav_t& nav) +{ + freeArray<geph_t>(nav.geph, nav.ng, nav.ngmax); +} - void allocateSBASEphemeris(int n_sat = NSATSBS*2); //SBAS - void deleteSBASEphemeris(); +inline void Navigation::freeSeph(nav_t& nav) +{ + freeArray<seph_t>(nav.seph, nav.ns, nav.nsmax); +} - void allocateAlmanac(int n_sat = MAXSAT); - void deleteAlmanac(); - void print(); +inline void Navigation::freePeph(nav_t& nav) +{ + freeArray<peph_t>(nav.peph, nav.ne, nav.nemax); +} - private: +inline void Navigation::freeAlm(nav_t& nav) +{ + freeArray<alm_t>(nav.alm, nav.na, nav.namax); +} - // rtklib-like attribute to represent the different navigation msgs for a given epoch - nav_t nav_; +inline void Navigation::freePclk(nav_t& nav) +{ + freeArray<pclk_t>(nav.pclk, nav.nc, nav.ncmax); +} - // Private methods +inline void Navigation::freeTec(nav_t& nav) +{ + freeArray<tec_t>(nav.tec, nav.nt, nav.ntmax); +} +inline void Navigation::freeFcb(nav_t& nav) +{ + freeArray<fcbd_t>(nav.fcb, nav.nf, nav.nfmax); +} - }; +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); } + +} // namespace GNSSUtils #endif diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h index e84820c2ab4dc86ef80ee79d06acc59da5047812..5610a7ec08698c19a6cf84152da0773b83f55cb2 100644 --- a/include/gnss_utils/observations.h +++ b/include/gnss_utils/observations.h @@ -2,55 +2,134 @@ #define OBSERVATIONS_H #include <vector> +#include <map> #include <iostream> #include <memory> +#include <cassert> #include "gnss_utils/utils.h" -extern "C" -{ - #include "rtklib.h" +extern "C" { +#include "rtklib.h" } namespace GNSSUtils { - class Observations - { - public: - // Constructor & Destructor - Observations(); - ~Observations(); +class Observations; +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 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 pushObservation(obsd_t & obs); + void removeObservationByIdx(const int& _idx); + void removeObservationBySat(const int& _sat); - void reserveObservations(unsigned int n); + std::vector<obsd_t>& getObservations(); + const std::vector<obsd_t>& getObservations() const; - 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; - void print(obsd_t & _obs); - void print(int _obs_idx); - void print(); + obsd_t& getObservationByIdx(const int& idx); + const obsd_t& getObservationByIdx(const int& idx) const; + obsd_t* data(); + const obsd_t* data() const; + size_t size() const; - private: - // Private objects - // rtklib-like attribute to represent the different observation msgs for a given epoch - std::vector<obsd_t> obs_vector_; + bool hasSatellite(const unsigned char& i) const; + void print(obsd_t& _obs); + void printBySat(const int& _sat); + void printByIdx(const int& _idx); + void print(); - // Private methods + 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 methods +}; + +inline const std::vector<obsd_t>& Observations::getObservations() const +{ + return this->obs_; +} + +inline std::vector<obsd_t>& Observations::getObservations() +{ + 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)); +} + +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)); +} + +inline obsd_t& Observations::getObservationByIdx(const int& 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); +} + +inline obsd_t* Observations::data() +{ + return obs_.data(); +} + +inline const obsd_t* Observations::data() const +{ + return obs_.data(); +} + +inline size_t Observations::size() const +{ + return obs_.size(); +} + +inline bool Observations::hasSatellite(const unsigned char& i) const +{ + 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 new file mode 100644 index 0000000000000000000000000000000000000000..c3e4cc15180559331f3b0732f4243eb684076b63 --- /dev/null +++ b/include/gnss_utils/ublox_raw.h @@ -0,0 +1,63 @@ +#ifndef UBLOX_RAW_H +#define UBLOX_RAW_H + +#include "gnss_utils/gnss_utils.h" + +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 +}; + +class UBloxRaw +{ +public: + UBloxRaw(); + ~UBloxRaw(); + + RawDataType addDataStream(const std::vector<u_int8_t>& data_stream); + + const Observations& getObservations(); + const Navigation& getNavigation(); + + RawDataType getRawDataType() const; + +private: + raw_t raw_data_; + + Observations obs_; + Navigation nav_; + + RawDataType raw_data_type_; + + void updateObservations(); +}; + +inline const GNSSUtils::Observations& UBloxRaw::getObservations() +{ + return obs_; +} + +inline const GNSSUtils::Navigation& UBloxRaw::getNavigation() +{ + return nav_; +} + +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/CMakeLists.txt b/src/CMakeLists.txt deleted file mode 100644 index bf6f31f6e439b57c78c3d7a9ed52da3c7462a125..0000000000000000000000000000000000000000 --- a/src/CMakeLists.txt +++ /dev/null @@ -1,62 +0,0 @@ -# rtklib path -SET(RTKLIB_DIR ../deps/RTKLIB) -SET(RTKLIB_SRC_DIR ${RTKLIB_DIR}/src) - -# driver source files -SET(SOURCES - gnss_utils.cpp - observations.cpp - navigation.cpp - utils.cpp) - -SET(RTKLIB_SRC - ${RTKLIB_SRC_DIR}/pntpos.c - ${RTKLIB_SRC_DIR}/rtkcmn.c - ${RTKLIB_SRC_DIR}/sbas.c - ${RTKLIB_SRC_DIR}/ephemeris.c - ${RTKLIB_SRC_DIR}/preceph.c - ${RTKLIB_SRC_DIR}/qzslex.c - ${RTKLIB_SRC_DIR}/rtcm.c - ${RTKLIB_SRC_DIR}/rtcm2.c - ${RTKLIB_SRC_DIR}/rtcm3.c - ${RTKLIB_SRC_DIR}/rtcm3e.c - ${RTKLIB_SRC_DIR}/ionex.c - ${RTKLIB_SRC_DIR}/rinex.c) - - -# application header files -SET(HEADERS - ../include/gnss_utils/gnss_utils.h - ../include/gnss_utils/utils.h - ../include/gnss_utils/observations.h - ../include/gnss_utils/navigation.h - ${RTKLIB_SRC_DIR}/rtklib.h) - -# Eigen ####### -FIND_PACKAGE(Eigen3 REQUIRED) - -# Boost ####### -set(Boost_USE_STATIC_LIBS OFF) -set(Boost_USE_MULTITHREADED OFF) -set(Boost_USE_STATIC_RUNTIME OFF) -FIND_PACKAGE(Boost REQUIRED) - -# Adding libraries' directories -link_directories(/usr/lib/x86_64-linux-gnu/) - -# Adding include directories -INCLUDE_DIRECTORIES(../include/ ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${RTKLIB_SRC_DIR}) - -# create the shared library -ADD_LIBRARY(gnss_utils SHARED ${SOURCES} ${RTKLIB_SRC}) -TARGET_LINK_LIBRARIES(gnss_utils ${Boost_LIBRARIES}) - -# Installing -INSTALL(TARGETS ${PROJECT_NAME} - RUNTIME DESTINATION bin - LIBRARY DESTINATION lib/iri-algorithms - ARCHIVE DESTINATION lib/iri-algorithms) -INSTALL(FILES ${HEADERS} DESTINATION include/iri-algorithms/gnss_utils) -INSTALL(FILES ../Findgnss_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/) - -ADD_SUBDIRECTORY(examples) diff --git a/src/TDCP.cpp b/src/TDCP.cpp new file mode 100644 index 0000000000000000000000000000000000000000..66e5a10997152c4544c7b6d1e5eaefa2cfa6028f --- /dev/null +++ b/src/TDCP.cpp @@ -0,0 +1,579 @@ +/* + * TDCP.cpp + * + * Created on: Dec 4, 2019 + * Author: jvallve + */ + +#include "../include/gnss_utils/TDCP.h" + +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) +{ + // 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) +{ + // 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) +{ + 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); + + 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; + } + + // 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) + { + // 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++; + } + } + int n_differences = row_2_sat_freq.size(); + + // Initial guess + Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero()); + +#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; + } + + // 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; + } + } + + // 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; + } + } + + // Solve + cov_d = (A.transpose() * A).inverse(); + delta_d = -cov_d * A.transpose() * r; + d += delta_d; + + // wrong solution + if (d.array().isNaN().any()) + { + std::cout << "TDCP: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl; + return false; + } + + // 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) + { + 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)); +#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; +} + +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; + + std::set<int> remove_sats; + + 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; + + // already discarded sats + if (discarded_sats.count(sat_number) != 0) + { + 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; + } + + // 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; + } + + // 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 fd5589b48b65ae24c960a19b75900cd8c419cc00..814ee443c905e3a1fd8d327bcad4d096227a02f6 100644 --- a/src/examples/gnss_utils_test.cpp +++ b/src/examples/gnss_utils_test.cpp @@ -1,5 +1,6 @@ #include "gnss_utils/observations.h" #include "gnss_utils/navigation.h" +#include "gnss_utils/gnss_utils.h" #include <typeinfo> @@ -7,146 +8,44 @@ #include <iostream> -extern "C" -{ - #include "../deps/RTKLIB/src/rinex.c" +extern "C" { +#include "../deps/RTKLIB/src/rinex.c" } using namespace GNSSUtils; - -int createObsAndNav(Observations* observations, char* obs_path, Navigation* navigation, char* nav_path) -{ - // Time - double ts_d[] = {2018, 5, 17, 14, 1, 2}; - gtime_t ts = epoch2time(ts_d); - - double te_d[] = {2018, 5, 17, 14, 42, 8}; - gtime_t te = epoch2time(te_d); - - double tint = 0.1; - - // Create options container - char* opt = "-SYS=G"; - - // Create receiver identifier - int rcv = 1; - - // Create object types - rnxopt_t rnx_opt; - - // Create obsd_t object to hold the observations - obs_t obs; - obs.data = (obsd_t *) malloc(sizeof(obsd_t)*MAXSAT); - - /* header */ - std::cout << "------------" << std::endl; - std::cout << "Observations" << std::endl; - std::cout << "------------" << std::endl; - - if (readrnx(obs_path, rcv, opt, &obs, NULL, NULL)) - { - sortobs(&obs); - obs.data[0].P[0] = 21597659.275; - obs.data[1].P[0] = 22348322.550; - obs.data[2].P[0] = 20873167.574; - obs.data[3].P[0] = 20920707.830; - - std::cout << "Obs number: " << obs.n << std::endl; - - - } - - - else - { - std::cout << "Couldn't load provided observation file" << std::endl; - return 0; - } - - - - 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: " << obs.data[i].rcv << - " | SNR: " << obs.data[i].SNR[0] << " | LLI: " << obs.data[i].LLI[0] << " | code: " << 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; - observations->pushObservation(obs.data[i]); - } - - free(obs.data); - - /* header */ - std::cout << "----------" << std::endl; - std::cout << "Navigation" << std::endl; - std::cout << "----------" << std::endl; - - /* Load _nav attribute into nav variable */ - nav_t nav = navigation->getNavigation(); - - - if (readrnx(nav_path, rcv, opt, NULL, &nav, NULL)) - { - 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; - uniqnav(&nav); - } - - - else - { - std::cout << "Couldn't load provided navigation file" << std::endl; - return 0; - } - - - /* Print Constellation Satellites ids */ - for (int i=0;i<nav.n;i++) - { - std::cout << i << " GPS Sat Id: " << nav.eph[i].sat << std::endl; - } - - for (int i=0;i<nav.ng;i++) - { - std::cout << i << " GLONASS Sat Id: " << nav.geph[i].sat << std::endl; - } - - for (int i=0;i<nav.ns;i++) - { - std::cout << i << " SBAS Sat Id: " << nav.seph[i].sat << std::endl; - } - - - /* Load nav into Navigation object*/ - navigation->setNavigation(nav); - - std::cout << "Navigation message loaded to Navigation class." << std::endl; - - return 1; -} - -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; - createObsAndNav(&observations, "../src/examples/raw_201805171357.obs", &navigation, "../src/examples/raw_201805171357.nav"); + gtime_t t_start{ 0, 0 }; // no limit + gtime_t t_end{ 0, 0 }; // no limit + double dt = 0.0; // no limit + const char* opt = ""; // only GPS | GPS+GAL: "-SYS=G,L" | ALL: "" - nav_t nav = navigation.getNavigation(); + // RTKLIB trace + // char str_file[80]; + // snprintf(str_file, sizeof str_file, "../src/examples/trace"); - std::cout << "Navigation eph number " << nav.n << std::endl; + // load observations from RINEX file + observations.loadFromRinex("../src/examples/sample_data.obs", t_start, t_end, dt, opt); + observations.print(); + + // Trace close + // traceclose(); + + // load navigation from RINEX file + navigation.loadFromRinex("../src/examples/sample_data.nav", t_start, t_end, dt, opt); + navigation.print(); /* Set processing options */ @@ -156,25 +55,26 @@ int main(int argc, char *argv[]) std::cout << "------------------" << std::endl; prcopt_t prcopt = prcopt_default; - prcopt.mode = PMODE_SINGLE; - prcopt.soltype = 0; - prcopt.nf = 1; - prcopt.navsys = SYS_GPS; - //prcopt.elmin = 1.05; // 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] = 4789374.0336; - prcopt.ru[1] = 177048.3292; - prcopt.ru[2] = 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); - //Compute spp + // Compute spp /* header */ std::cout << "-----------" << std::endl; @@ -183,27 +83,19 @@ int main(int argc, char *argv[]) int stat; - sol_t solb={{0}}; - - char msg[128]=""; + sol_t solb = { { 0 } }; - std::vector<obsd_t> obs = observations.getObservations(); + char msg[128] = ""; - for (int i=0; i < obs.size(); i++) - { - std::cout << "time: " << time_str(obs[i].time, 3) << " | sat: " << int(obs[i].sat) << " | rcv: " << obs[i].rcv << - " | SNR: " << obs[i].SNR[0] << " | LLI: " << obs[i].LLI[0] << " | code: " << obs[i].code[0] << - " | L: " << obs[i].L[0] << " | P: " << obs[i].P[0] << " | D: " << obs[i].D[0] << std::endl; - } - - std::cout << "obs.size(): " << obs.size() << std::endl; - - stat = pntpos(&obs[0], obs.size(), &nav, &prcopt, &solb, NULL, NULL, msg); + stat = pntpos(observations.data(), observations.size(), &navigation.getNavigation(), &prcopt, &solb, NULL, NULL, msg); std::cout << "msg: " << msg << std::endl; - - std::cout << "sol.stat: " << solb.stat << std::endl; - - std::cout << "Position: " << solb.rr[0] << ", " << solb.rr[1] << ", " << solb.rr[2] << std::endl; - + 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 (GT): " << lla_gt.transpose() << std::endl; + + traceclose(); } diff --git a/src/examples/sample_data.nav b/src/examples/sample_data.nav new file mode 100644 index 0000000000000000000000000000000000000000..5c4c418dc876fd45e7fddca388b4cc98b0202441 --- /dev/null +++ b/src/examples/sample_data.nav @@ -0,0 +1,406 @@ + 3.04 N: GNSS NAV DATA M: MIXED RINEX VERSION / TYPE +sbf2rin-13.4.3 20200205 113410 UTC PGM / RUN BY / DATE +GPSA 9.3132E-09 -1.4901E-08 -5.9605E-08 1.1921E-07 IONOSPHERIC CORR +GPSB 9.6256E+04 -1.4746E+05 -1.3107E+05 9.1750E+05 IONOSPHERIC CORR +GAL 2.9250E+01 3.8281E-01 3.2959E-03 0.0000E+00 IONOSPHERIC CORR +GPUT -9.3132257462E-10 8.881784197E-16 503808 2083 TIME SYSTEM CORR +GAUT 9.3132257462E-10 0.000000000E+00 345600 2083 TIME SYSTEM CORR +GAGP 1.2514647096E-09-5.329070518E-15 345600 2083 TIME SYSTEM CORR + 18 LEAP SECONDS + END OF HEADER +G02 2019 12 12 12 00 00-3.648423589766E-04-7.503331289627E-12 0.000000000000E+00 + 4.700000000000E+01-1.208437500000E+02 4.378039505776E-09-1.891689129337E+00 + -6.053596735001E-06 1.953727600630E-02 9.275972843170E-06 5.153594808578E+03 + 3.888000000000E+05 3.371387720108E-07-2.768163608668E-01 2.346932888031E-07 + 9.575379914494E-01 1.915312500000E+02-1.691294764827E+00-7.654961717025E-09 + 2.000083311498E-11 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00 + 2.000000000000E+00 0.000000000000E+00-1.769512891769E-08 4.700000000000E+01 + 3.868920000000E+05 4.000000000000E+00 +G04 2019 12 12 12 00 00-1.993030309677E-05-5.002220859751E-12 0.000000000000E+00 + 1.570000000000E+02-1.387500000000E+01 4.959135139313E-09-2.339868507292E+00 + -8.419156074524E-07 1.249722088687E-03 5.329027771950E-06 5.150576126099E+03 + 3.888000000000E+05 5.587935447693E-09 1.915372002909E+00 1.303851604462E-08 + 9.594040575671E-01 2.769062500000E+02-1.281079249179E+00-8.132124449911E-09 + -3.685867816904E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00 + 4.096000000000E+03 6.300000000000E+01-4.190951585770E-09 6.690000000000E+02 + 3.873960000000E+05 4.000000000000E+00 +G06 2019 12 12 12 00 00-1.512947492301E-04-1.114131009672E-11 0.000000000000E+00 + 1.000000000000E+02-1.239062500000E+02 3.927663602954E-09-1.824134113525E+00 + -6.347894668579E-06 1.658447668888E-03 9.786337614059E-06 5.153719285965E+03 + 3.888000000000E+05-1.490116119385E-08-2.122724576342E-01 5.401670932770E-08 + 9.781077241470E-01 2.010000000000E+02-1.200768673201E+00-7.640318249923E-09 + 1.214336296267E-11 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00 + 2.000000000000E+00 0.000000000000E+00 4.656612873077E-09 1.000000000000E+02 + 3.858180000000E+05 4.000000000000E+00 +G07 2019 12 12 12 00 00-1.683332957327E-04-8.185452315956E-12 0.000000000000E+00 + 5.700000000000E+01 3.909375000000E+01 4.250534194668E-09-2.882882750792E+00 + 1.888722181320E-06 1.320131728426E-02 1.172721385956E-05 5.153718780518E+03 + 3.888000000000E+05-7.450580596924E-09 2.943673467473E+00-1.341104507446E-07 + 9.545190186113E-01 1.482812500000E+02-2.422380694519E+00-7.540671242082E-09 + -4.639478967207E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00 + 2.000000000000E+00 0.000000000000E+00-1.117587089539E-08 5.700000000000E+01 + 3.858180000000E+05 4.000000000000E+00 +G09 2019 12 12 12 00 00-1.062308438122E-04-9.436007530894E-12 0.000000000000E+00 + 7.800000000000E+01-1.206250000000E+01 5.168786729286E-09 4.992301981873E-01 + -7.078051567078E-07 1.692383317277E-03 5.021691322327E-06 5.153534730911E+03 + 3.888000000000E+05-2.793967723846E-08 1.868752621939E+00 1.490116119385E-08 + 9.520568016730E-01 2.776875000000E+02 1.684705661839E+00-8.283559329210E-09 + -2.914407111040E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00 + 2.000000000000E+00 0.000000000000E+00 1.396983861923E-09 7.800000000000E+01 + 3.857520000000E+05 4.000000000000E+00 +G13 2019 12 12 12 00 00-2.714386209846E-05 2.387423592154E-12 0.000000000000E+00 + 8.700000000000E+01-1.987500000000E+01 4.779841956746E-09-8.793798521920E-01 + -1.072883605957E-06 4.130274290219E-03 5.709007382393E-06 5.153656044006E+03 + 3.888000000000E+05-6.332993507385E-08 2.009100851183E+00 9.313225746155E-08 + 9.674158381471E-01 2.706562500000E+02 1.183260297004E+00-8.083193840326E-09 + -5.078782980268E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00 + 2.000000000000E+00 0.000000000000E+00-1.117587089539E-08 8.700000000000E+01 + 3.870420000000E+05 4.000000000000E+00 +G23 2019 12 12 12 00 00-1.500290818512E-04 2.387423592154E-12 0.000000000000E+00 + 3.000000000000E+01-1.009375000000E+01 5.335222233421E-09-1.452196357053E+00 + -4.973262548447E-07 1.326873130165E-02 4.552304744720E-06 5.153694892883E+03 + 3.888000000000E+05 2.980232238770E-07 1.861752114203E+00 4.470348358154E-08 + 9.428683609718E-01 2.778750000000E+02-2.215284453592E+00-8.261415549690E-09 + -2.478674675321E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00 + 2.000000000000E+00 0.000000000000E+00-2.048909664154E-08 3.000000000000E+01 + 3.858420000000E+05 4.000000000000E+00 +G30 2019 12 12 12 00 00-1.042019575834E-04-8.640199666843E-12 0.000000000000E+00 + 2.500000000000E+01 4.368750000000E+01 4.663408535398E-09-2.906933186921E+00 + 2.210959792137E-06 4.164319136180E-03 1.142919063568E-05 5.153722436905E+03 + 3.888000000000E+05-2.793967723846E-08 2.968865819418E+00-3.725290298462E-09 + 9.399056183160E-01 1.480312500000E+02-2.921787544453E+00-7.888185717455E-09 + -4.778770483544E-10 1.000000000000E+00 2.083000000000E+03 0.000000000000E+00 + 2.000000000000E+00 0.000000000000E+00 3.725290298462E-09 2.500000000000E+01 + 3.858180000000E+05 4.000000000000E+00 +R03 2019 12 12 11 15 00 6.369315087795E-06 0.000000000000E+00 3.858000000000E+05 + 9.652274414063E+03 3.457994461060E-01 9.313225746155E-10 0.000000000000E+00 + 1.708528710938E+04 2.202743530273E+00-1.862645149231E-09 5.000000000000E+00 + 1.629986035156E+04-2.522974967957E+00-2.793967723846E-09 0.000000000000E+00 +R03 2019 12 12 11 45 00 6.371177732944E-06 0.000000000000E+00 3.870000000000E+05 + 1.043946044922E+04 4.914197921753E-01 9.313225746155E-10 0.000000000000E+00 + 2.039817236328E+04 1.453030586243E+00-1.862645149231E-09 5.000000000000E+00 + 1.118693066406E+04-3.121288299561E+00-1.862645149231E-09 0.000000000000E+00 +R05 2019 12 12 11 15 00 4.108343273401E-05 9.094947017729E-13 3.862800000000E+05 + 5.985222167969E+03 1.195173263550E-01-1.862645149231E-09 0.000000000000E+00 + -1.828012353516E+04 2.295486450195E+00 1.862645149231E-09 1.000000000000E+00 + 1.676849218750E+04 2.457766532898E+00-1.862645149231E-09 0.000000000000E+00 +R05 2019 12 12 11 45 00 4.108529537916E-05 9.094947017729E-13 3.871800000000E+05 + 6.598400390625E+03 5.781021118164E-01-9.313225746155E-10 0.000000000000E+00 + -1.370180371094E+04 2.745733261108E+00 9.313225746155E-10 1.000000000000E+00 + 2.048785058594E+04 1.648019790649E+00-1.862645149231E-09 0.000000000000E+00 +R14 2019 12 12 11 15 00 4.597380757332E-05 0.000000000000E+00 3.858000000000E+05 + 2.135816357422E+04-6.865911483765E-01 3.725290298462E-09 0.000000000000E+00 + 1.261080761719E+04-4.694700241089E-01-0.000000000000E+00-7.000000000000E+00 + 5.991423828125E+03 3.442845344543E+00-2.793967723846E-09 0.000000000000E+00 +R14 2019 12 12 11 45 00 4.597473889589E-05 0.000000000000E+00 3.870000000000E+05 + 1.936734765625E+04-1.519350051880E+00 1.862645149231E-09 0.000000000000E+00 + 1.162212011719E+04-5.869359970093E-01-9.313225746155E-10-7.000000000000E+00 + 1.187716162109E+04 3.054467201233E+00-2.793967723846E-09 0.000000000000E+00 +R15 2019 12 12 11 15 00 1.051910221577E-04 0.000000000000E+00 3.860100000000E+05 + 2.249286962891E+04 1.470921516418E+00 5.587935447693E-09 0.000000000000E+00 + 4.179755859375E+03 4.950771331787E-01 9.313225746155E-10 0.000000000000E+00 + -1.125261865234E+04 3.118181228638E+00-1.862645149231E-09 0.000000000000E+00 +R15 2019 12 12 11 45 00 1.051910221577E-04 0.000000000000E+00 3.870000000000E+05 + 2.451276953125E+04 7.425718307495E-01 4.656612873077E-09 0.000000000000E+00 + 4.643450683594E+03 4.951381683350E-02 0.000000000000E+00 0.000000000000E+00 + -5.276882812500E+03 3.478320121765E+00-2.793967723846E-09 0.000000000000E+00 +R19 2019 12 12 11 15 00-6.853323429823E-05-1.818989403546E-12 3.859500000000E+05 + 2.008951708984E+04 2.060537338257E-01 2.793967723846E-09 0.000000000000E+00 + -1.541425244141E+04-4.377956390381E-01 2.793967723846E-09 3.000000000000E+00 + 3.049126953125E+03-3.578741073608E+00-1.862645149231E-09 0.000000000000E+00 +R19 2019 12 12 11 45 00-6.853695958853E-05-1.818989403546E-12 3.870000000000E+05 + 1.978871044922E+04-5.135507583618E-01 3.725290298462E-09 0.000000000000E+00 + -1.571828076172E+04 1.333026885986E-01 1.862645149231E-09 3.000000000000E+00 + -3.427280273438E+03-3.570507049561E+00-1.862645149231E-09 0.000000000000E+00 +R13 2019 12 12 11 45 00-3.215298056602E-05-0.000000000000E+00 3.876000000000E+05 + 3.100402832031E+03-2.939124107361E+00-1.862645149231E-09 0.000000000000E+00 + 1.186301953125E+04-8.695030212402E-01-9.313225746155E-10-2.000000000000E+00 + 2.235348193359E+04 8.685455322266E-01-1.862645149231E-09 0.000000000000E+00 +R20 2019 12 12 11 45 00-3.986386582255E-04-0.000000000000E+00 3.877800000000E+05 + 9.837636718750E+03 1.428291320801E+00-0.000000000000E+00 0.000000000000E+00 + -1.795626220703E+04-1.534766197205E+00 1.862645149231E-09 2.000000000000E+00 + 1.525312060547E+04-2.726587295532E+00-1.862645149231E-09 0.000000000000E+00 +E01 2019 12 12 11 10 00-7.507699192502E-04-7.986500349944E-12 0.000000000000E+00 + 3.000000000000E+00 2.013125000000E+02 2.449744898851E-09 7.981291655576E-01 + 9.473413228989E-06 1.731918891892E-04 9.533017873764E-06 5.440622966766E+03 + 3.858000000000E+05 3.166496753693E-08-2.605880888987E+00-1.117587089539E-08 + 9.855193908613E-01 1.463750000000E+02-1.069210219000E+00-5.272719629937E-09 + 1.914365455291E-10 5.170000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09 + 3.864650000000E+05 +E01 2019 12 12 11 20 00-7.507746340707E-04-7.972289495228E-12 0.000000000000E+00 + 4.000000000000E+00 2.002500000000E+02 2.449744898851E-09 8.736282654153E-01 + 9.419396519661E-06 1.731686061248E-04 9.588897228241E-06 5.440622581482E+03 + 3.864000000000E+05 3.725290298462E-08-2.605884063520E+00-1.862645149231E-09 + 9.855195488564E-01 1.452500000000E+02-1.070324887968E+00-5.265933632987E-09 + 1.896507568581E-10 5.170000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09 + 3.870650000000E+05 +E09 2019 12 12 11 10 00 6.223833654076E-03-1.216449163621E-11 0.000000000000E+00 + 3.000000000000E+00 7.218750000000E+00 3.823373544569E-09-3.442271110123E-02 + 3.892928361893E-07 2.964780433103E-04 3.403052687645E-06 5.440603132248E+03 + 3.858000000000E+05-9.313225746155E-09 1.575307486314E+00-6.146728992462E-08 + 9.586413959478E-01 2.625000000000E+02 2.794206052069E-01-5.815242228181E-09 + 8.571785620706E-12 5.170000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-4.656612873077E-10-6.984919309616E-10 + 3.869030000000E+05 +E12 2019 12 12 10 40 00 6.055364210624E-03-1.870148480521E-11 0.000000000000E+00 + 0.000000000000E+00-2.075312500000E+02 2.603322724555E-09-2.414195903395E+00 + -9.659677743912E-06 2.996901748702E-04 6.495043635368E-06 5.440614477158E+03 + 3.840000000000E+05-2.421438694000E-08-5.219517721978E-01 5.774199962616E-08 + 9.864495053986E-01 2.151250000000E+02-9.348391204646E-01-5.376652530588E-09 + -2.257236880119E-10 5.170000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-1.327134668827E-08-1.327134668827E-08 + 3.858050000000E+05 +E19 2019 12 12 11 00 00-2.469634637237E-06 2.273736754432E-13 0.000000000000E+00 + 2.000000000000E+00 1.103125000000E+01 3.765871149364E-09-1.685770007887E-01 + 4.824250936508E-07 1.212444622070E-04 3.688037395477E-06 5.440600940704E+03 + 3.852000000000E+05-5.401670932770E-08 1.580615112564E+00 5.215406417847E-08 + 9.583897608719E-01 2.595937500000E+02 1.996512379669E+00-5.770954669140E-09 + 1.160762636137E-10 5.170000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09-5.820766091347E-09 + 3.858660000000E+05 +E19 2019 12 12 11 10 00-2.469576429576E-06 2.273736754432E-13 0.000000000000E+00 + 3.000000000000E+00 1.168750000000E+01 3.765156833895E-09-9.388288915917E-02 + 5.066394805908E-07 1.211694907397E-04 3.693625330925E-06 5.440600774765E+03 + 3.858000000000E+05-5.960464477539E-08 1.580611582543E+00 4.097819328308E-08 + 9.583898398694E-01 2.594687500000E+02 1.996204499781E+00-5.764168672191E-09 + 1.207193141583E-10 5.170000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09-5.820766091347E-09 + 3.864650000000E+05 +E21 2019 12 12 11 10 00-5.709171527997E-04-2.131628207280E-12 0.000000000000E+00 + 3.000000000000E+00 2.180312500000E+02 2.333311477503E-09 1.537835498027E+00 + 1.022964715958E-05 9.841390419751E-05 9.685754776001E-06 5.440636165619E+03 + 3.858000000000E+05-2.980232238770E-08-2.608584029515E+00 4.842877388000E-08 + 9.850865499746E-01 1.424062500000E+02-1.025413854454E+00-5.217717338871E-09 + 1.871506527187E-10 5.170000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10 + 3.867350000000E+05 +E21 2019 12 12 11 20 00-5.709183751605E-04-2.131628207280E-12 0.000000000000E+00 + 4.000000000000E+00 2.213125000000E+02 2.317953694933E-09 1.607657241034E+00 + 1.038052141666E-05 9.810912888497E-05 9.816139936447E-06 5.440637243271E+03 + 3.864000000000E+05-4.097819328308E-08-2.608587130902E+00 4.470348358154E-08 + 9.850867050439E-01 1.395312500000E+02-1.020851268976E+00-5.213788603794E-09 + 1.800074980348E-10 5.170000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10 + 3.870650000000E+05 +E01 2019 12 12 11 10 00-7.507689879276E-04-7.986500349944E-12 0.000000000000E+00 + 3.000000000000E+00 2.013125000000E+02 2.449744898851E-09 7.981291655576E-01 + 9.473413228989E-06 1.731918891892E-04 9.533017873764E-06 5.440622966766E+03 + 3.858000000000E+05 3.166496753693E-08-2.605880888987E+00-1.117587089539E-08 + 9.855193908613E-01 1.463750000000E+02-1.069210219000E+00-5.272719629937E-09 + 1.914365455291E-10 2.580000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00 + 3.865400000000E+05 +E01 2019 12 12 11 20 00-7.507736445405E-04-7.972289495228E-12 0.000000000000E+00 + 4.000000000000E+00 2.002500000000E+02 2.449744898851E-09 8.736282654153E-01 + 9.419396519661E-06 1.731686061248E-04 9.588897228241E-06 5.440622581482E+03 + 3.864000000000E+05 3.725290298462E-08-2.605884063520E+00-1.862645149231E-09 + 9.855195488564E-01 1.452500000000E+02-1.070324887968E+00-5.265933632987E-09 + 1.896507568581E-10 2.580000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00 + 3.871400000000E+05 +E12 2019 12 12 10 40 00 6.055364909116E-03-1.871569565992E-11 0.000000000000E+00 + 0.000000000000E+00-2.075312500000E+02 2.603322724555E-09-2.414195903395E+00 + -9.659677743912E-06 2.996901748702E-04 6.495043635368E-06 5.440614477158E+03 + 3.840000000000E+05-2.421438694000E-08-5.219517721978E-01 5.774199962616E-08 + 9.864495053986E-01 2.151250000000E+02-9.348391204646E-01-5.376652530588E-09 + -2.257236880119E-10 2.580000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-1.327134668827E-08 0.000000000000E+00 + 3.858400000000E+05 +E19 2019 12 12 11 00 00-2.468528691679E-06 2.273736754432E-13 0.000000000000E+00 + 2.000000000000E+00 1.103125000000E+01 3.765871149364E-09-1.685770007887E-01 + 4.824250936508E-07 1.212444622070E-04 3.688037395477E-06 5.440600940704E+03 + 3.852000000000E+05-5.401670932770E-08 1.580615112564E+00 5.215406417847E-08 + 9.583897608719E-01 2.595937500000E+02 1.996512379669E+00-5.770954669140E-09 + 1.160762636137E-10 2.580000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09 0.000000000000E+00 + 3.859400000000E+05 +E19 2019 12 12 11 10 00-2.468470484018E-06 2.273736754432E-13 0.000000000000E+00 + 3.000000000000E+00 1.168750000000E+01 3.765156833895E-09-9.388288915917E-02 + 5.066394805908E-07 1.211694907397E-04 3.693625330925E-06 5.440600774765E+03 + 3.858000000000E+05-5.960464477539E-08 1.580611582543E+00 4.097819328308E-08 + 9.583898398694E-01 2.594687500000E+02 1.996204499781E+00-5.764168672191E-09 + 1.207193141583E-10 2.580000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-5.820766091347E-09 0.000000000000E+00 + 3.865400000000E+05 +E21 2019 12 12 11 10 00-5.709161632694E-04-2.117417352565E-12 0.000000000000E+00 + 3.000000000000E+00 2.180312500000E+02 2.333311477503E-09 1.537835498027E+00 + 1.022964715958E-05 9.841390419751E-05 9.685754776001E-06 5.440636165619E+03 + 3.858000000000E+05-2.980232238770E-08-2.608584029515E+00 4.842877388000E-08 + 9.850865499746E-01 1.424062500000E+02-1.025413854454E+00-5.217717338871E-09 + 1.871506527187E-10 2.580000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00 + 3.867900000000E+05 +E21 2019 12 12 11 20 00-5.709173856303E-04-2.131628207280E-12 0.000000000000E+00 + 4.000000000000E+00 2.213125000000E+02 2.317953694933E-09 1.607657241034E+00 + 1.038052141666E-05 9.810912888497E-05 9.816139936447E-06 5.440637243271E+03 + 3.864000000000E+05-4.097819328308E-08-2.608587130902E+00 4.470348358154E-08 + 9.850867050439E-01 1.395312500000E+02-1.020851268976E+00-5.213788603794E-09 + 1.800074980348E-10 2.580000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00 + 3.871400000000E+05 +E04 2019 12 12 11 10 00-4.236891982146E-04-7.531752999057E-12 0.000000000000E+00 + 3.000000000000E+00 9.781250000000E+00 3.935521073107E-09-4.009111796513E-01 + 5.532056093216E-07 8.811207953840E-05 3.596767783165E-06 5.440604427338E+03 + 3.858000000000E+05 9.499490261078E-08 1.582511472789E+00 3.166496753693E-08 + 9.526713574660E-01 2.577187500000E+02 1.440283619102E+00-5.908817554540E-09 + 7.071723137082E-11 5.170000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-5.355104804039E-09-5.820766091347E-09 + 3.875670000000E+05 +E04 2019 12 12 11 10 00-4.236878594384E-04-7.531752999057E-12 0.000000000000E+00 + 3.000000000000E+00 9.781250000000E+00 3.935521073107E-09-4.009111796513E-01 + 5.532056093216E-07 8.811207953840E-05 3.596767783165E-06 5.440604427338E+03 + 3.858000000000E+05 9.499490261078E-08 1.582511472789E+00 3.166496753693E-08 + 9.526713574660E-01 2.577187500000E+02 1.440283619102E+00-5.908817554540E-09 + 7.071723137082E-11 2.580000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-5.355104804039E-09 0.000000000000E+00 + 3.875900000000E+05 +E11 2019 12 12 11 00 00 3.295102505945E-04 1.526245796413E-10 5.204170427930E-18 + 2.000000000000E+00-2.123750000000E+02 2.665468170305E-09 2.480654590844E+00 + -9.858980774879E-06 1.651302445680E-04 7.150694727898E-06 5.440609029770E+03 + 3.852000000000E+05-3.352761268616E-08-5.219456059981E-01 2.048909664154E-08 + 9.864349859366E-01 1.975937500000E+02-2.012781103955E-01-5.400939256513E-09 + -2.103659054415E-10 5.170000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-1.583248376846E-08-1.699663698673E-08 + 3.876330000000E+05 +E21 2019 12 12 11 30 00-5.709196557291E-04-2.131628207280E-12 0.000000000000E+00 + 5.000000000000E+00 2.240625000000E+02 2.303667385565E-09 1.677767938158E+00 + 1.050904393196E-05 9.777839295566E-05 9.970739483833E-06 5.440638303757E+03 + 3.870000000000E+05-5.401670932770E-08-2.608590226436E+00 3.725290298462E-08 + 9.850868571874E-01 1.362812500000E+02-1.016577701982E+00-5.210574184187E-09 + 1.721500278825E-10 5.170000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 2.328306436539E-10 + 3.876650000000E+05 +E11 2019 12 12 11 00 00 3.295124042779E-04 1.526387904960E-10 5.204170427930E-18 + 2.000000000000E+00-2.123750000000E+02 2.665468170305E-09 2.480654590844E+00 + -9.858980774879E-06 1.651302445680E-04 7.150694727898E-06 5.440609029770E+03 + 3.852000000000E+05-3.352761268616E-08-5.219456059981E-01 2.048909664154E-08 + 9.864349859366E-01 1.975937500000E+02-2.012781103955E-01-5.400939256513E-09 + -2.103659054415E-10 2.580000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-1.583248376846E-08 0.000000000000E+00 + 3.876800000000E+05 +E01 2019 12 12 11 30 00-7.507792906836E-04-7.972289495228E-12 0.000000000000E+00 + 5.000000000000E+00 1.994687500000E+02 2.447601952446E-09 9.486982138657E-01 + 9.378418326378E-06 1.731649972498E-04 9.616836905479E-06 5.440622346878E+03 + 3.870000000000E+05 3.911554813385E-08-2.605887229274E+00 7.450580596924E-09 + 9.855197039257E-01 1.446875000000E+02-1.071010335308E+00-5.259504793772E-09 + 1.871506527187E-10 5.170000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09-1.862645149231E-09 + 3.877250000000E+05 +E21 2019 12 12 11 30 00-5.709186661988E-04-2.117417352565E-12 0.000000000000E+00 + 5.000000000000E+00 2.240625000000E+02 2.303667385565E-09 1.677767938158E+00 + 1.050904393196E-05 9.777839295566E-05 9.970739483833E-06 5.440638303757E+03 + 3.870000000000E+05-5.401670932770E-08-2.608590226436E+00 3.725290298462E-08 + 9.850868571874E-01 1.362812500000E+02-1.016577701982E+00-5.210574184187E-09 + 1.721500278825E-10 2.580000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00 4.656612873077E-10 0.000000000000E+00 + 3.877400000000E+05 +E01 2019 12 12 11 30 00-7.507783011533E-04-7.972289495228E-12 0.000000000000E+00 + 5.000000000000E+00 1.994687500000E+02 2.447601952446E-09 9.486982138657E-01 + 9.378418326378E-06 1.731649972498E-04 9.616836905479E-06 5.440622346878E+03 + 3.870000000000E+05 3.911554813385E-08-2.605887229274E+00 7.450580596924E-09 + 9.855197039257E-01 1.446875000000E+02-1.071010335308E+00-5.259504793772E-09 + 1.871506527187E-10 2.580000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00-1.629814505577E-09 0.000000000000E+00 + 3.878200000000E+05 +E36 2019 12 12 10 30 00 6.253067986108E-04-4.803268893738E-12 0.000000000000E+00 + 1.270000000000E+02-2.396562500000E+02 2.521890761159E-09 3.060908164528E+00 + -1.121312379837E-05 3.076605498791E-04 7.288530468941E-06 5.440609262466E+03 + 3.834000000000E+05 2.980232238770E-08-5.264936383676E-01 5.587935447693E-08 + 9.896539850266E-01 1.944062500000E+02-1.782833699345E+00-5.428440402046E-09 + -2.457245211269E-10 5.170000000000E+02 2.083000000000E+03 + 3.120000000000E+00 0.000000000000E+00 3.725290298462E-09 4.423782229424E-09 + 3.878450000000E+05 +S36 2019 12 12 11 36 48 0.000000000000E+00 0.000000000000E+00 3.874820000000E+05 + 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01 + 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04 + 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.530000000000E+02 +S25 2019 12 11 23 59 44 0.000000000000E+00 0.000000000000E+00 3.874840000000E+05 + 4.053076976000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01 + -1.162201120000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04 + 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 +S23 2019 12 12 11 37 04 0.000000000000E+00 0.000000000000E+00 3.874940000000E+05 + 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01 + 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04 + 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.460000000000E+02 +S36 2019 12 12 11 38 56 0.000000000000E+00 0.000000000000E+00 3.875460000000E+05 + 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01 + 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04 + 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.540000000000E+02 +S23 2019 12 12 11 39 12 0.000000000000E+00 0.000000000000E+00 3.875580000000E+05 + 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01 + 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04 + 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.470000000000E+02 +S23 2019 12 12 11 41 20 0.000000000000E+00 0.000000000000E+00 3.876860000000E+05 + 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01 + 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04 + 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.480000000000E+02 +S36 2019 12 12 11 41 04 0.000000000000E+00 0.000000000000E+00 3.877380000000E+05 + 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01 + 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04 + 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 2.550000000000E+02 +S36 2019 12 12 11 43 12 0.000000000000E+00 0.000000000000E+00 3.878020000000E+05 + 4.200368800000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01 + 3.674846960000E+03 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04 + 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 +S23 2019 12 12 11 43 28 0.000000000000E+00 0.000000000000E+00 3.878780000000E+05 + 3.594460000000E+04 0.000000000000E+00 0.000000000000E+00 6.300000000000E+01 + 2.204414000000E+04 0.000000000000E+00 0.000000000000E+00 3.276700000000E+04 + 0.000000000000E+00 0.000000000000E+00 0.000000000000E+00 1.490000000000E+02 +C05 2019 12 12 11 00 00-2.244751667604E-04-5.808153957787E-11 0.000000000000E+00 + 1.000000000000E+00-4.986718750000E+02 1.785788670980E-12-5.169766818568E-01 + -1.630326732993E-05 6.773804780096E-04 2.061109989882E-05 6.493346771240E+03 + 3.852000000000E+05 1.271255314350E-07-1.766797704845E-01 3.259629011154E-09 + 1.180676987021E-01-6.241562500000E+02-1.608095995036E+00 1.192549674481E-09 + 7.575315542299E-10 0.000000000000E+00 7.270000000000E+02 + 2.000000000000E+00 0.000000000000E+00-8.000000000000E-10-9.200000000000E-09 + 3.858276000000E+05 0.000000000000E+00 +C10 2019 12 12 11 00 00-4.537283675745E-04-3.685762806072E-11 0.000000000000E+00 + 1.000000000000E+00 9.611718750000E+02 9.743262988869E-10-3.040647920928E+00 + 3.135204315186E-05 6.156359100714E-03 7.578637450933E-06 6.493785018921E+03 + 3.852000000000E+05-1.676380634308E-08-2.379959881122E+00-4.004687070847E-08 + 9.058375010021E-01-1.493750000000E+01-2.570309406835E+00-2.115088101909E-09 + 4.782342060886E-10 0.000000000000E+00 7.270000000000E+02 + 2.000000000000E+00 0.000000000000E+00 5.000000000000E-09 2.400000000000E-09 + 3.858180000000E+05 0.000000000000E+00 +C23 2019 12 12 11 00 00-8.696540025994E-04 2.136069099379E-12 0.000000000000E+00 + 1.000000000000E+00 1.034062500000E+02 3.498002848716E-09 1.631067082891E+00 + 5.179084837437E-06 2.014992060140E-04 1.188227906823E-05 5.282628129959E+03 + 3.852000000000E+05 3.725290298462E-08-3.016985033742E+00 6.565824151039E-08 + 9.546959600159E-01 1.175156250000E+02-1.126531243634E+00-6.488127399406E-09 + -1.864363372504E-10 0.000000000000E+00 7.270000000000E+02 + 2.000000000000E+00 0.000000000000E+00 2.430000000000E-08 2.430000000000E-08 + 3.858180000000E+05 1.000000000000E+00 +C30 2019 12 12 11 00 00 2.388220746070E-04 6.104450278599E-12 0.000000000000E+00 + 1.000000000000E+00-1.393437500000E+02 3.927663602954E-09-2.732742750025E+00 + -6.837770342827E-06 3.649030113593E-04 1.750886440277E-06 5.282614929199E+03 + 3.852000000000E+05-3.864988684654E-08-9.521190897989E-01-5.587935447693E-09 + 9.644793109832E-01 3.275781250000E+02-4.157902561103E-01-7.082080711374E-09 + -2.339383158984E-10 0.000000000000E+00 7.270000000000E+02 + 2.000000000000E+00 0.000000000000E+00-1.040000000000E-08-1.040000000000E-08 + 3.858180000000E+05 1.000000000000E+00 +C32 2019 12 12 11 00 00-8.566775359213E-04 2.992273095970E-12 0.000000000000E+00 + 1.000000000000E+00 2.875000000000E+01 4.228033257413E-09 2.867304095205E+00 + 1.456588506699E-06 1.842749770731E-04 6.427057087421E-06 5.282614164352E+03 + 3.852000000000E+05 1.862645149231E-09 1.148173467908E+00 6.705522537231E-08 + 9.608588315814E-01 2.297500000000E+02-1.050918258529E+00-6.857785654299E-09 + 5.228789228631E-10 0.000000000000E+00 7.270000000000E+02 + 2.000000000000E+00 0.000000000000E+00-9.900000000000E-09-9.900000000000E-09 + 3.857580000000E+05 1.000000000000E+00 +C37 2019 12 12 11 00 00-8.880557725206E-04-5.954881032721E-11 0.000000000000E+00 + 1.000000000000E+00 1.086562500000E+02 3.521932416908E-09 8.066563687499E-01 + 5.388632416725E-06 7.037379546091E-04 1.127412542701E-05 5.282630052567E+03 + 3.852000000000E+05-2.142041921616E-08-3.020306267399E+00 1.536682248116E-08 + 9.560827215058E-01 1.306718750000E+02-1.080448983277E+00-6.580988410297E-09 + -2.328668426958E-10 0.000000000000E+00 7.270000000000E+02 + 2.000000000000E+00 0.000000000000E+00-1.390000000000E-08-1.390000000000E-08 + 3.858180000000E+05 1.000000000000E+00 +C20 2019 12 12 11 00 00-4.067538538948E-04-5.008171655163E-11 0.000000000000E+00 + 1.000000000000E+00 2.850000000000E+01 4.229461888350E-09 1.463145840983E+00 + 1.523178070784E-06 5.460308166221E-04 6.177462637424E-06 5.282614208221E+03 + 3.852000000000E+05 3.445893526077E-08 1.153432718382E+00-1.210719347000E-08 + 9.622213291121E-01 2.341406250000E+02-4.480160490039E-01-6.852785446020E-09 + 4.753769442150E-10 0.000000000000E+00 7.270000000000E+02 + 2.000000000000E+00 0.000000000000E+00 2.090000000000E-08 2.090000000000E-08 + 3.875880000000E+05 1.000000000000E+00 diff --git a/src/examples/sample_data.obs b/src/examples/sample_data.obs new file mode 100644 index 0000000000000000000000000000000000000000..d773d3b4d57556afcd38b64ef4167981a00de992 --- /dev/null +++ b/src/examples/sample_data.obs @@ -0,0 +1,58 @@ + 3.04 OBSERVATION DATA M RINEX VERSION / TYPE +sbf2rin-13.4.3 20200205 113408 UTC PGM / RUN BY / DATE +SEPT MARKER NAME +Unknown MARKER NUMBER +Unknown Unknown OBSERVER / AGENCY +3021420 SEPT ASTERX-M2 4.4.0 REC # / TYPE / VERS +Unknown Unknown ANT # / TYPE + 4789398.3686 176958.8129 4194502.0999 APPROX POSITION XYZ + 0.0000 0.0000 0.0000 ANTENNA: DELTA H/E/N +G 7 X1 C1C L1C C2W L2W C2L L2L SYS / # / OBS TYPES +E 7 X1 C1C L1C C5Q L5Q C7Q L7Q SYS / # / OBS TYPES +S 3 X1 C1C L1C SYS / # / OBS TYPES +R 5 X1 C1C L1C C2C L2C SYS / # / OBS TYPES +C 5 X1 C2I L2I C7I L7I SYS / # / OBS TYPES +SEPTENTRIO RECEIVERS OUTPUT ALIGNED CARRIER PHASES. COMMENT +NO FURTHER PHASE SHIFT APPLIED IN THE RINEX ENCODER. COMMENT +G L1C SYS / PHASE SHIFT +G L2W SYS / PHASE SHIFT +G L2L 0.00000 SYS / PHASE SHIFT +E L1C 0.00000 SYS / PHASE SHIFT +E L5Q 0.00000 SYS / PHASE SHIFT +E L7Q 0.00000 SYS / PHASE SHIFT +S L1C SYS / PHASE SHIFT +R L1C SYS / PHASE SHIFT +R L2C SYS / PHASE SHIFT +C L2I SYS / PHASE SHIFT +C L7I SYS / PHASE SHIFT + 0.050 INTERVAL + 2019 12 12 11 37 42.0000000 GPS TIME OF FIRST OBS + 2019 12 12 11 45 12.9000000 GPS TIME OF LAST OBS + 36 # OF SATELLITES + C1C 0.000 C2C 0.000 GLONASS COD/PHS/BIS + 8 R03 5 R05 1 R13 -2 R14 -7 R15 0 R19 3 R20 2 R21 4 GLONASS SLOT / FRQ # + END OF HEADER +> 2019 12 12 11 37 42.0000000 0 23 +C05 16.000 40138840.659 6 209013478.40606 40138832.256 6 161622441.87406 +C10 17.000 39633104.355 5 206379985.90205 39633100.852 6 159586077.69206 +C23 18.000 22865676.301 8 119067585.90908 +C32 19.000 22153454.599 8 115358921.19508 +C37 20.000 23690566.611 7 123363010.77907 +E01 11.000 26311074.676 7 138265636.53007 26311074.470 5 103250320.19005 26311070.977 7 105943790.57907 +E09 12.000 25723493.482 5 +E12 13.000 24803095.627 5 130341141.55605 24803092.768 4 97332661.71504 24803092.407 6 99871767.93506 +E19 14.000 24540665.652 6 128962068.93606 24540664.837 5 96302837.53105 24540661.694 6 98815075.63606 +E21 15.000 25609802.251 7 134580416.73507 25609802.381 7 100498367.38007 25609799.519 7 103120055.01507 +G02 1.000 22032430.127 5 +G06 2.000 22147435.410 6 116385570.45206 22147437.095 4 90690045.24704 22147437.422 7 90690046.16907 +G07 3.000 20902411.481 8 109842911.60108 20902406.002 6 85591857.15006 20902406.930 7 85591857.16507 +G09 4.000 21908403.525 6 115129430.37406 21908399.544 1 89711233.32401 21908399.320 5 +G23 5.000 24104714.314 6 126671129.49306 24104706.816 2 98704739.67602 +G30 6.000 21404145.095 8 112479542.93308 21404142.953 6 87646388.26706 21404143.916 7 87646383.27307 +R03 7.000 21945110.617 7 117474090.63807 21945109.720 6 91368580.68606 +R14 8.000 20214975.526 8 107757316.39008 20214976.579 7 83811176.41707 +R15 9.000 22699008.378 5 121296675.96005 22699009.712 5 94341922.34705 +R19 10.000 22853592.745 4 22853590.888 4 +S23 21.000 38309228.895 7 201316353.13407 +S25 22.000 37834172.957 6 198818604.39206 +S36 23.000 37630702.258 7 197750698.01307 diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp index dcc3f24cc5aa51cb925e9211b4b610f9fe4b1502..70bad327efe878de41efd6fd2c21d5f7f4e51528 100644 --- a/src/gnss_utils.cpp +++ b/src/gnss_utils.cpp @@ -1,64 +1,510 @@ #include "gnss_utils/gnss_utils.h" - namespace GNSSUtils { - - ComputePosOutput computePos(const GNSSUtils::Observations & _observations, - GNSSUtils::Navigation & _navigation, - const prcopt_t & _prcopt) - { +ComputePosOutput computePos(const GNSSUtils::Observations& _observations, + GNSSUtils::Navigation& _navigation, + const prcopt_t& _prcopt) +{ + // Remove duplicated satellites + _navigation.uniqueNavigation(); - // Remove duplicated satellites - uniqnav(&(_navigation.getNavigation())); + // Define error msg + char msg[128] = ""; - // Define error msg - char msg[128] = ""; + // Remove duplicated satellites + _navigation.uniqueNavigation(); - GNSSUtils::ComputePosOutput output; - sol_t sol; - sol = {{0}}; + GNSSUtils::ComputePosOutput output; + sol_t sol; + sol = { { 0 } }; - output.pos_stat = pntpos(_observations.getObservations().data(), _observations.getObservations().size(), + output.pos_stat = pntpos(_observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg); if (output.pos_stat == 0) { - std::cout << "Bad computing: " << msg << "\n"; + std::cout << "computePos: error in computing positioning, message: " << msg << "\n"; } output.time = sol.time.time; - output.time = sol.time.sec; + 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]; + sol.qr[5], sol.qr[4], sol.qr[2]; + //std::cout << "Compute pos: " << output.pos.transpose() << "\n"; + //std::cout << "Covariance:\n" << output.pos_covar << "\n"; // XXX: segmentation fault here. // if (sol.dtr != NULL) // { // output.rcv_bias << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5]; // } - output.type = sol.type; output.stat = sol.stat; output.ns = sol.ns; output.age = sol.age; output.ratio = sol.ratio; - output.lat_lon = ecefToLatLon(output.pos); + output.lat_lon = ecefToLatLonAlt(output.pos); return output; - } +} - Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef) - { - double pos[3]; - ecef2pos(&_ecef(0), pos); +// ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations, +// GNSSUtils::Navigation & _navigation, +// const prcopt_t & _prcopt) +// { + +// // Remove duplicated satellites +// uniqnav(&(_navigation.getNavigation())); + +// // Define error msg +// char msg[128] = ""; + +// GNSSUtils::ComputePosOutput output; +// sol_t sol; +// sol = {{0}}; + +// output.pos_stat = pntposOwn(_observations.data(), _observations.size(), +// &(_navigation.getNavigation()), +// &_prcopt, &sol, NULL, NULL, msg); + +// if (output.pos_stat == 0) +// { +// std::cout << "Bad computing: " << msg << "\n"; +// } + +// output.time = sol.time.time; +// output.sec = sol.time.sec; +// output.pos = Eigen::Vector3d(sol.rr); +// // std::cout << "Compute pos: " << output.pos.transpose() << "\n"; +// output.vel = Eigen::Vector3d(&sol.rr[3]); +// output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5], +// sol.qr[3], sol.qr[1], sol.qr[4], +// sol.qr[5], sol.qr[3], sol.qr[2]; + +// // XXX: segmentation fault here. +// // if (sol.dtr != NULL) +// // { +// // output.rcv_bias << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5]; +// // } + +// output.type = sol.type; +// output.stat = sol.stat; +// output.ns = sol.ns; +// output.age = sol.age; +// output.ratio = sol.ratio; +// output.lat_lon = ecefToLatLonAlt(output.pos); + +// return output; +// } + +/* single-point positioning COPIED FROM RTKLIB ------------------------------ + * compute receiver position, velocity, clock bias by single-point positioning + * with pseudorange and doppler observables + * args : obsd_t *obs I observation data + * int n I number of observation data + * nav_t *nav I navigation data + * prcopt_t *opt I processing options + * sol_t *sol IO solution + * double *azel IO azimuth/elevation angle (rad) (NULL: no output) + * ssat_t *ssat IO satellite status (NULL: no output) + * char *msg O error message for error exit + * return : status(1:ok,0:error) + * notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and + * receiver bias are negligible (only involving glonass-gps time offset + * and receiver bias) + *-----------------------------------------------------------------------------*/ +// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav, +// const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, +// char *msg) +// { +// prcopt_t opt_=*opt; +// double *rs,*dts,*var,*azel_,*resp; +// int i,stat,vsat[MAXOBS]={0},svh[MAXOBS]; + +// sol->stat=SOLQ_NONE; + +// if (n<=0) {strcpy(msg,"no observation data"); return 0;} + +// trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n); + +// sol->time=obs[0].time; msg[0]='\0'; + +// rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n); + +// if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */ +// #if 0 +// opt_.sateph =EPHOPT_BRDC; +// #endif +// opt_.ionoopt=IONOOPT_BRDC; +// opt_.tropopt=TROPOPT_SAAS; +// } +// /* satellite positons, velocities and clocks */ +// satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh); + +// /* estimate receiver position with pseudorange */ +// stat=estposOwn(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); + +// /* raim fde */ +// if (!stat&&n>=6&&opt->posopt[4]) { +// stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); +// } +// /* estimate receiver velocity with doppler */ +// //if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat); + +// if (azel) { +// for (i=0;i<n*2;i++) azel[i]=azel_[i]; +// } +// if (ssat) { +// for (i=0;i<MAXSAT;i++) { +// ssat[i].vs=0; +// ssat[i].azel[0]=ssat[i].azel[1]=0.0; +// ssat[i].resp[0]=ssat[i].resc[0]=0.0; +// ssat[i].snr[0]=0; +// } +// for (i=0;i<n;i++) { +// ssat[obs[i].sat-1].azel[0]=azel_[ i*2]; +// ssat[obs[i].sat-1].azel[1]=azel_[1+i*2]; +// ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0]; +// if (!vsat[i]) continue; +// ssat[obs[i].sat-1].vs=1; +// ssat[obs[i].sat-1].resp[0]=resp[i]; +// } +// } +// free(rs); free(dts); free(var); free(azel_); free(resp); +// return stat; +// } + +int estposOwn(const obsd_t* obs, + int n, + const double* rs, + const double* dts, + const double* vare, + const int* svh, + const nav_t* nav, + const prcopt_t* opt, + sol_t* sol, + double* azel, + int* vsat, + double* resp, + char* msg) +{ + // double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig; + // int i,j,k,info,stat,nv,ns; + // + // trace(3,"estpos : n=%d\n",n); + // + // v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1); + // + // for (i=0;i<3;i++) x[i]=sol->rr[i]; + // + // for (i=0;i<MAXITR;i++) { + // + // /* pseudorange residuals */ + // nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp, + // &ns); + // + // if (nv<NX) { + // sprintf(msg,"lack of valid sats ns=%d",nv); + // break; + // } + // /* weight by variance */ + // for (j=0;j<nv;j++) { + // sig=sqrt(var[j]); + // v[j]/=sig; + // for (k=0;k<NX;k++) H[k+j*NX]/=sig; + // } + // /* least square estimation */ + // if ((info=lsq(H,v,NX,nv,dx,Q))) { + // sprintf(msg,"lsq error info=%d",info); + // break; + // } + // for (j=0;j<NX;j++) x[j]+=dx[j]; + // + // if (norm(dx,NX)<1E-4) { + // sol->type=0; + // sol->time=timeadd(obs[0].time,-x[3]/CLIGHT); + // sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */ + // sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */ + // sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */ + // sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */ + // for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0; + // for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX]; + // sol->qr[3]=(float)Q[1]; /* cov xy */ + // sol->qr[4]=(float)Q[2+NX]; /* cov yz */ + // sol->qr[5]=(float)Q[2]; /* cov zx */ + // sol->ns=(unsigned char)ns; + // sol->age=sol->ratio=0.0; + // + // /* validate solution */ + // if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) { + // sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE; + // } + // free(v); free(H); free(var); + // + // return stat; + // } + // } + // if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i); + // + // free(v); free(H); free(var); + + return 0; +} + +Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef) +{ + Eigen::Vector3d pos; + ecef2pos(_ecef.data(), pos.data()); + + return pos; +} + +Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _pos) +{ + Eigen::Vector3d ecef; + pos2ecef(_pos.data(), ecef.data()); + + return ecef; +} + +Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef) +{ + Eigen::Matrix3d cov_enu; + + /* RTKLIB transform covariance to local tangental coordinate -------------------------- + * transform ecef covariance to local tangental coordinate + * args : double *pos I geodetic position {lat,lon} (rad) + * double *P I covariance in ecef coordinate + * double *Q O covariance in local tangental coordinate + * return : none + *-----------------------------------------------------------------------------*/ + // extern void covenu(const double *pos, const double *P, double *Q); + covenu(_latlon.data(), _cov_ecef.data(), cov_enu.data()); + + return cov_enu; +} + +Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu) +{ + Eigen::Matrix3d cov_ecef; + + /* RTKLIB transform local enu coordinate covariance to xyz-ecef ----------------------- + * transform local enu covariance to xyz-ecef coordinate + * args : double *pos I geodetic position {lat,lon} (rad) + * double *Q I covariance in local enu coordinate + * double *P O covariance in xyz-ecef coordinate + * return : none + *-----------------------------------------------------------------------------*/ + // extern void covecef(const double *pos, const double *Q, double *P) + covecef(_latlon.data(), _cov_enu.data(), cov_ecef.data()); + + return cov_ecef; +} + +void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, + Eigen::Matrix3d& R_ENU_ECEF, + Eigen::Vector3d& t_ENU_ECEF) +{ + // Convert ECEF coordinates to geodetic coordinates. + // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates + // to geodetic coordinates," IEEE Transactions on Aerospace and + // Electronic Systems, vol. 30, pp. 957-961, 1994. + + // double r = std::sqrt(_t_ECEF_ENU(0) * _t_ECEF_ENU(0) + _t_ECEF_ENU(1) * _t_ECEF_ENU(1)); + // double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis; + // double F = 54 * kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) * _t_ECEF_ENU(2); + // double G = r * r + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) - kFirstEccentricitySquared + // * Esq; double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3); double S = + // cbrt(1 + C + sqrt(C * C + 2 * C)); double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G); double Q = sqrt(1 + 2 * + // kFirstEccentricitySquared * kFirstEccentricitySquared * P); double r_0 = -(P * kFirstEccentricitySquared * r) / + // (1 + Q) + // + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q) + // - P * (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P * + // r * r); + // double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) + // * _t_ECEF_ENU(2)); double Z_0 = kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) / (kSemimajorAxis * V); + // + // double latitude = atan((_t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r); + // double longitude = atan2(_t_ECEF_ENU(1), _t_ECEF_ENU(0)); + // + // + // double sLat = sin(latitude); + // double cLat = cos(latitude); + // double sLon = sin(longitude); + // double cLon = cos(longitude); + // + // R_ENU_ECEF(0,0) = -sLon; + // R_ENU_ECEF(0,1) = cLon; + // R_ENU_ECEF(0,2) = 0.0; + // + // R_ENU_ECEF(1,0) = -sLat*cLon; + // R_ENU_ECEF(1,1) = -sLat * sLon; + // R_ENU_ECEF(1,2) = cLat; + // + // R_ENU_ECEF(2,0) = cLat * cLon; + // R_ENU_ECEF(2,1) = cLat * sLon; + // R_ENU_ECEF(2,2) = sLat; + // + // t_ENU_ECEF = -R_ENU_ECEF*_t_ECEF_ENU; + + Eigen::Vector3d ENU_lat_lon_alt = GNSSUtils::ecefToLatLonAlt(_t_ECEF_ENU); + + double sLat = sin(ENU_lat_lon_alt(0)); + double cLat = cos(ENU_lat_lon_alt(0)); + double sLon = sin(ENU_lat_lon_alt(1)); + double cLon = cos(ENU_lat_lon_alt(1)); + + R_ENU_ECEF(0, 0) = -sLon; + R_ENU_ECEF(0, 1) = cLon; + R_ENU_ECEF(0, 2) = 0.0; + + R_ENU_ECEF(1, 0) = -sLat * cLon; + R_ENU_ECEF(1, 1) = -sLat * sLon; + R_ENU_ECEF(1, 2) = cLat; + + R_ENU_ECEF(2, 0) = cLat * cLon; + R_ENU_ECEF(2, 1) = cLat * sLon; + R_ENU_ECEF(2, 2) = sLat; + + t_ENU_ECEF = -R_ENU_ECEF * _t_ECEF_ENU; +} + +void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, + Eigen::Matrix3d& R_ENU_ECEF, + Eigen::Vector3d& t_ENU_ECEF) +{ + double sLat = sin(_ENU_latlonalt(0)); + double cLat = cos(_ENU_latlonalt(0)); + double sLon = sin(_ENU_latlonalt(1)); + double cLon = cos(_ENU_latlonalt(1)); + + R_ENU_ECEF(0, 0) = -sLon; + R_ENU_ECEF(0, 1) = cLon; + R_ENU_ECEF(0, 2) = 0.0; + + R_ENU_ECEF(1, 0) = -sLat * cLon; + R_ENU_ECEF(1, 1) = -sLat * sLon; + R_ENU_ECEF(1, 2) = cLat; + + R_ENU_ECEF(2, 0) = cLat * cLon; + R_ENU_ECEF(2, 1) = cLat * sLon; + R_ENU_ECEF(2, 2) = sLat; + + Eigen::Vector3d t_ECEF_ENU = latLonAltToEcef(_ENU_latlonalt); + + t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU; +} - return Eigen::Vector3d(pos); +double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef) +{ + // ecef 2 geodetic + Eigen::Vector3d receiver_geo; + ecef2pos(receiver_ecef.data(), receiver_geo.data()); + + // receiver-sat vector ecef + Eigen::Vector3d receiver_sat_ecef = sat_ecef - receiver_ecef; + + // receiver-sat vector enu (receiver ecef as origin) + Eigen::Vector3d receiver_sat_enu; + ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad) + receiver_sat_ecef.data(), // vector in ecef coordinate {x,y,z} + receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u} + + // elevation + return atan2(receiver_sat_enu(2), + sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1))); +} + +void computeSatellitesPositions(const Observations& obs, + const Navigation& nav, + const prcopt_t& opt, + std::map<int, Eigen::Vector3d>& sats_pos) +{ + double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()]; + int svh[obs.size()]; + + // std::cout << "computing sats position from sats: "; + // for (auto&& obs_ref : obs.getObservations()) + // std::cout << (int)obs_ref.sat << " "; + // std::cout << std::endl; + + // compute positions + satposs( + obs.getObservations().front().time, obs.data(), obs.size(), &nav.getNavigation(), opt.sateph, rs, dts, var, svh); + + // store positions + // std::cout << "filling sats positions: \n"; + for (int i = 0; i < obs.size(); i++) + { + if (svh[i] < 0) // ephemeris unavailable + sats_pos[obs.getObservationByIdx(i).sat] = Eigen::Vector3d::Zero(); + else + sats_pos[obs.getObservationByIdx(i).sat] << rs[6 * i], rs[6 * i + 1], rs[6 * i + 2]; + // std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << ": " << + // sats_pos[obs.getObservationByIdx(i).sat].transpose() << std::endl; } +} + +bool equalTime(const gtime_t& time1, const gtime_t& time2) +{ + return (difftime(time1.time, time2.time) == 0.0 && time1.sec == time2.sec); +} + +bool equalObservations(const obsd_t& obs1, const obsd_t& obs2) +{ + if (!equalTime(obs1.time, obs2.time)) + return false; + if (!equalTime(obs1.eventime, obs2.eventime)) + return false; + if (obs1.timevalid != obs2.timevalid) + return false; + if (obs1.sat != obs2.sat) + return false; + if (obs1.rcv != obs2.rcv) + return false; + if (memcmp(obs1.SNR, obs2.SNR, sizeof(obs1.SNR)) != 0) + return false; + if (memcmp(obs1.LLI, obs2.LLI, sizeof(obs1.LLI)) != 0) + return false; + if (memcmp(obs1.code, obs2.code, sizeof(obs1.code)) != 0) + return false; + if (memcmp(obs1.qualL, obs2.qualL, sizeof(obs1.qualL)) != 0) + return false; + if (memcmp(obs1.qualP, obs2.qualP, sizeof(obs1.qualP)) != 0) + return false; + if (obs1.freq != obs2.freq) + return false; + if (!equalArray<double>(obs1.L, obs2.L, sizeof(obs1.L) / sizeof(obs1.L[0]), sizeof(obs2.L) / sizeof(obs2.L[0]))) + return false; + if (!equalArray<double>(obs1.P, obs2.P, sizeof(obs1.P) / sizeof(obs1.P[0]), sizeof(obs2.P) / sizeof(obs2.P[0]))) + return false; + if (!equalArray<float>(obs1.D, obs2.D, sizeof(obs1.D) / sizeof(obs1.D[0]), sizeof(obs2.D) / sizeof(obs2.D[0]))) + return false; + return true; } + +bool equalObservations(const obs_t& obs1, const obs_t& obs2) +{ + if (obs1.n != obs2.n) + return false; + if (obs1.nmax != obs2.nmax) + return false; + if (obs1.flag != obs2.flag) + return false; + if (obs1.rcvcount != obs2.rcvcount) + return false; + if (obs1.tmcount != obs2.tmcount) + return false; + if (!equalObservations(*(obs1.data), *(obs2.data))) + return false; + + return true; +} + +} // namespace GNSSUtils diff --git a/src/navigation.cpp b/src/navigation.cpp index d53cf51287ae59572b4885aa0fba47cf0420dbe4..398d0a86f275b64f4d469a2bce0ddbdf3dba6172 100644 --- a/src/navigation.cpp +++ b/src/navigation.cpp @@ -1,96 +1,265 @@ #include "gnss_utils/navigation.h" - using namespace GNSSUtils; - Navigation::Navigation() { - allocateEphemeris(0); - allocateGLONASSEphemeris(0); - allocateSBASEphemeris(0); - allocateAlmanac(0); + // 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() +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(); - //free(_nav); + setNavigation(nav.getNavigation()); } -void Navigation::clearNavigation() +Navigation::~Navigation() { - deleteEphemeris(); - deleteGLONASSEphemeris(); - deleteSBASEphemeris(); - deleteAlmanac(); + freeNavigationArrays(); } void Navigation::setNavigation(nav_t _nav) { - clearNavigation(); - nav_ = _nav; -} + freeNavigationArrays(); -const nav_t & Navigation::getNavigation() const -{ - return nav_; -} + 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) */ -nav_t & Navigation::getNavigation() -{ - return nav_; -} + 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 */ -/****************** Array memory management ******************/ -void Navigation::allocateEphemeris(int n_sat) -{ - eph_t eph0 ={0,-1,-1}; - int i; - nav_.eph = (eph_t *) malloc(sizeof(eph_t)*n_sat); - for (i=0;i<n_sat;i++) nav_.eph[i] = eph0; + // ********** 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::deleteEphemeris() + +void Navigation::clearNavigation() { - free(nav_.eph); + 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::allocateGLONASSEphemeris(int n_sat) +void Navigation::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt) { - geph_t geph0={0,-1}; - int i; - nav_.geph = (geph_t *)malloc(sizeof(geph_t)*n_sat); + auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, NULL, &nav_, NULL); + if (stat == 1) + { + std::cout << "Navigation file loaded." << std::endl; + std::cout << "GPS satellites in navigation file: " << nav_.n << std::endl; + std::cout << "GLONASS satellites in navigation file: " << nav_.ng << std::endl; + std::cout << "SBAS satellites in navigation file: " << nav_.ns << std::endl; + std::cout << "Almanac satellites in navigation file: " << nav_.na << std::endl; + uniqueNavigation(); + } + else + std::cout << "Navigation: couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << std::endl; } -void Navigation::deleteGLONASSEphemeris() + +void Navigation::copyAllArrays(const nav_t& nav) { - free(nav_.geph); + // 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(); } -void Navigation::allocateSBASEphemeris(int n_sat) +void Navigation::copyEphemeris(const nav_t& nav) { - seph_t seph0={0}; - int i; + 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]); + // } + // } - nav_.seph = (seph_t *)malloc(sizeof(seph_t)*n_sat); - for (i=0; i<n_sat; i++) nav_.seph[i] = seph0; + 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]); + // } + // } + + uniqueNavigation(); } -void Navigation::deleteSBASEphemeris() + +void Navigation::copyAlmanac(const nav_t& nav) { - free(nav_.seph); + 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::allocateAlmanac(int n_sat) +void Navigation::copyIonUtc(const nav_t& nav) { - alm_t alm0 ={0,-1}; - int i; + 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); - nav_.alm = (alm_t *)malloc(sizeof(alm_t)*n_sat); - for (i=0; i<n_sat; i++) nav_.alm[i] = alm0; + 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; +} + +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); + } } -void Navigation::deleteAlmanac() + +void Navigation::freeNavigationArrays() { - free(nav_.alm); + // 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() @@ -102,26 +271,33 @@ void Navigation::print() std::cout << "nc: " << nav_.nc << "\n"; std::cout << "na: " << nav_.na << "\n"; std::cout << "nt: " << nav_.nt << "\n"; - std::cout << "nn: " << nav_.nn << "\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 << "nnmax: " << nav_.nnmax << "\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 4ef5448237b6e98ba5fc17f92e9f3da2cc3a6a6a..ea2dc15d658ce4644e8446ae71182ce21c428d4a 100644 --- a/src/observations.cpp +++ b/src/observations.cpp @@ -1,45 +1,167 @@ #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()) +{ + // 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() { - this->obs_vector_.erase(obs_vector_.begin(), obs_vector_.end()); } void Observations::clearObservations() { - this->obs_vector_.clear(); + this->obs_.clear(); + this->idx_2_sat_.clear(); + this->sat_2_idx_.clear(); } -void Observations::pushObservation(obsd_t & obs) +void Observations::addObservation(const obsd_t& obs) { - this->obs_vector_.push_back(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()); +} + +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 << "Observation: couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << stat << std::endl; + return; + } + + for (int i = 0; i < obs.n; i++) + { + // std::cout << "time: " << time_str(obs.data[i].time, 3) << " | sat: " << int(obs.data[i].sat) << " | rcv: " << + // int(obs.data[i].rcv) << + // " | SNR: " << int(obs.data[i].SNR[0]) << " | LLI: " << int(obs.data[i].LLI[0]) << " | code: " << + // int(obs.data[i].code[0]) << + // " | L: " << obs.data[i].L[0] << " | P: " << obs.data[i].P[0] << " | D: " << obs.data[i].D[0] << + // std::endl; + addObservation(obs.data[i]); + } + + free(obs.data); } -void Observations::reserveObservations(unsigned int n) +void Observations::removeObservationByIdx(const int& _idx) { - this->obs_vector_.reserve(n); + // 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()); } -const std::vector<obsd_t> & Observations::getObservations() const +void Observations::removeObservationBySat(const int& _sat) { - return this->obs_vector_; + // 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); - std::cout << "Time [s]: " << _obs.time.time << "\n"; + std::cout << "Time [s]: " << _obs.time.time << " + " << _obs.time.sec << "\n"; printArray("SNR: ", _obs.SNR, ARRAY_SIZE(_obs.SNR)); printArray("LLI: ", _obs.LLI, ARRAY_SIZE(_obs.LLI)); printArray("code: ", _obs.code, ARRAY_SIZE(_obs.code)); @@ -48,15 +170,52 @@ void Observations::print(obsd_t & _obs) printArray("D: ", _obs.D, ARRAY_SIZE(_obs.D)); } -void Observations::print(int _obs_idx) +void Observations::printBySat(const int& _sat_number) { - print(obs_vector_[_obs_idx]); + print(getObservationBySat(_sat_number)); +} + +void Observations::printByIdx(const int& _idx) +{ + print(getObservationByIdx(_idx)); } void Observations::print() { - for (auto obs: obs_vector_) + for (auto obs : obs_) { print(obs); } -} \ No newline at end of file +} + +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; +} diff --git a/src/ublox_raw.cpp b/src/ublox_raw.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9cc2442d19ddaefe97e7741878a21a565fffbb50 --- /dev/null +++ b/src/ublox_raw.cpp @@ -0,0 +1,98 @@ +#include "gnss_utils/ublox_raw.h" + +using namespace GNSSUtils; + +UBloxRaw::UBloxRaw() : raw_data_type_(NO) +{ + if (init_raw(&raw_data_, STRFMT_UBX) == 0) + { + assert("Failed when allocating memory for raw_t"); + return; + } +}; + +UBloxRaw::~UBloxRaw() +{ + 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); + + 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 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_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; + } + } + + return raw_data_type_; +} + +void UBloxRaw::updateObservations() +{ + // 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]); + + // std::cout << "--------------------------JUST AFTER!---------------------" << std::endl; + // obs_.print(); +} diff --git a/src/utils.cpp b/src/utils.cpp index 3e41a0e2461e417c8e3ce267963f681d9ca6614e..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 << (unsigned)_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 << ","; } +} -} \ No newline at end of file +} // namespace GNSSUtils diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..93365929c3f20d1f751e1e5d6ff5476adf3a5249 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,23 @@ +# Retrieve googletest from github & compile +add_subdirectory(gtest) + +# Include gtest directory. +include_directories(${GTEST_INCLUDE_DIRS}) + +############# USE THIS TEST AS AN EXAMPLE #################### +# # +# Create a specific test executable for gtest_example # +# gnss_utils_add_gtest(gtest_example gtest_example.cpp) # +# target_link_libraries(gtest_example ${PROJECT_NAME}) # +# # +############################################################## + +# Transformations test +gnss_utils_add_gtest(gtest_transformations gtest_transformations.cpp) +target_link_libraries(gtest_transformations ${PROJECT_NAME}) + +# Observations test +add_executable(gtest_observations gtest_observations.cpp) +add_dependencies(gtest_observations libgtest) +target_link_libraries(gtest_observations libgtest ${PROJECT_NAME}) +add_test(NAME gtest_observations COMMAND gtest_observations "${CMAKE_CURRENT_LIST_DIR}/../src/examples/sample_data.obs") diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..3c3c48c74516488a8c4eb24c2492bd81a5edc7f9 --- /dev/null +++ b/test/gtest/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 2.8.8) +project(gtest_builder C CXX) + +# We need thread support +#find_package(Threads REQUIRED) + +# Enable ExternalProject CMake module +include(ExternalProject) + +set(GTEST_FORCE_SHARED_CRT ON) +set(GTEST_DISABLE_PTHREADS OFF) + +# For some reason I need to disable PTHREADS +# with g++ (Ubuntu 4.9.3-8ubuntu2~14.04) 4.9.3 +# This is a known issue for MinGW : +# https://github.com/google/shaderc/pull/174 +#if(MINGW) + set(GTEST_DISABLE_PTHREADS ON) +#endif() + +# Download GoogleTest +ExternalProject_Add(googletest + GIT_REPOSITORY https://github.com/google/googletest.git + GIT_TAG v1.8.x + # TIMEOUT 1 # We'll try this + CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs + -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs + -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS} + -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT} + -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS} + -DBUILD_GTEST=ON + PREFIX "${CMAKE_CURRENT_BINARY_DIR}" + # Disable install step + INSTALL_COMMAND "" + UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github +) + +# Get GTest source and binary directories from CMake project + +# Specify include dir +ExternalProject_Get_Property(googletest source_dir) +set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE) + +# Specify MainTest's link libraries +ExternalProject_Get_Property(googletest binary_dir) +set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE) + +# Create a libgtest target to be used as a dependency by test programs +add_library(libgtest IMPORTED STATIC GLOBAL) +add_dependencies(libgtest googletest) + +# Set libgtest properties +set_target_properties(libgtest PROPERTIES + "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a" + "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}" +) + +function(gnss_utils_add_gtest target) + add_executable(${target} ${ARGN}) + add_dependencies(${target} libgtest) + target_link_libraries(${target} libgtest) + + #WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/bin + add_test(NAME ${target} COMMAND ${target}) +endfunction() diff --git a/test/gtest/utils_gtest.h b/test/gtest/utils_gtest.h new file mode 100644 index 0000000000000000000000000000000000000000..09c290758a8b953abc270a386ab4964075ebfdf8 --- /dev/null +++ b/test/gtest/utils_gtest.h @@ -0,0 +1,146 @@ +/** + * \file utils_gtest.h + * \brief Some utils for gtest + * \author Jeremie Deray + * Created on: 26/09/2016 + * Eigen macros extension by: Joan Sola on 26/04/2017 + */ + +#ifndef GNSSUTILS_UTILS_GTEST_H +#define GNSSUTILS_UTILS_GTEST_H + +#include <gtest/gtest.h> + +// Macros for testing equalities and inequalities. +// +// * {ASSERT|EXPECT}_EQ(expected, actual): Tests that expected == actual +// * {ASSERT|EXPECT}_NE(v1, v2): Tests that v1 != v2 +// * {ASSERT|EXPECT}_LT(v1, v2): Tests that v1 < v2 +// * {ASSERT|EXPECT}_LE(v1, v2): Tests that v1 <= v2 +// * {ASSERT|EXPECT}_GT(v1, v2): Tests that v1 > v2 +// * {ASSERT|EXPECT}_GE(v1, v2): Tests that v1 >= v2 +// +// C String Comparisons. All tests treat NULL and any non-NULL string +// as different. Two NULLs are equal. +// +// * {ASSERT|EXPECT}_STREQ(s1, s2): Tests that s1 == s2 +// * {ASSERT|EXPECT}_STRNE(s1, s2): Tests that s1 != s2 +// * {ASSERT|EXPECT}_STRCASEEQ(s1, s2): Tests that s1 == s2, ignoring case +// * {ASSERT|EXPECT}_STRCASENE(s1, s2): Tests that s1 != s2, ignoring case +// +// Macros for comparing floating-point numbers. +// +// * {ASSERT|EXPECT}_FLOAT_EQ(expected, actual): +// Tests that two float values are almost equal. +// * {ASSERT|EXPECT}_DOUBLE_EQ(expected, actual): +// Tests that two double values are almost equal. +// * {ASSERT|EXPECT}_NEAR(v1, v2, abs_error): +// Tests that v1 and v2 are within the given distance to each other. +// +// These predicate format functions work on floating-point values, and +// can be used in {ASSERT|EXPECT}_PRED_FORMAT2*(), e.g. +// +// EXPECT_PRED_FORMAT2(testing::DoubleLE, Foo(), 5.0); +// +// Macros that execute statement and check that it doesn't generate new fatal +// failures in the current thread. +// +// * {ASSERT|EXPECT}_NO_FATAL_FAILURE(statement); + +// http://stackoverflow.com/a/29155677 + +namespace testing +{ +namespace internal +{ +enum GTestColor +{ + COLOR_DEFAULT, + COLOR_RED, + COLOR_GREEN, + COLOR_YELLOW +}; + +extern void ColoredPrintf(GTestColor color, const char* fmt, ...); + +#define PRINTF(...) \ + do { testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN,\ + "[ ] "); \ + testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); } \ + while(0) + +#define PRINT_TEST_FINISHED \ +{ \ + const ::testing::TestInfo* const test_info = \ + ::testing::UnitTest::GetInstance()->current_test_info(); \ + PRINTF(std::string("Finished test case ").append(test_info->test_case_name()).\ + append(" of test ").append(test_info->name()).append(".\n").c_str()); \ +} + +// C++ stream interface +class TestCout : public std::stringstream +{ +public: + ~TestCout() + { + PRINTF("%s\n", str().c_str()); + } +}; + +/* Usage : + +TEST(Test, Foo) +{ + // the following works but prints default stream + EXPECT_TRUE(false) << "Testing Stream."; + + // or you can play with AINSI color code + EXPECT_TRUE(false) << "\033[1;31m" << "Testing Stream."; + + // or use the above defined macros + + PRINTF("Hello world"); + + // or + + TEST_COUT << "Hello world"; +} + +*/ +#define TEST_COUT testing::internal::TestCout() + +} // namespace internal + +/* Macros related to testing Eigen classes: + */ +#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \ + return (lhs - rhs).isMuchSmallerThan(1, precision); \ + }, \ + C_expect, C_actual); + +#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \ + return (lhs - rhs).isMuchSmallerThan(1, precision); \ + }, \ + C_expect, C_actual); + +#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision) + +#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision) + +#define EXPECT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \ + MatrixXd er = lhs - rhs; \ + er(2) = pi2pi((double)er(2)); \ + return er.isMuchSmallerThan(1, precision); \ + }, \ + C_expect, C_actual); + +#define ASSERT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \ + MatrixXd er = lhs - rhs; \ + er(2) = pi2pi((double)er(2)); \ + return er.isMuchSmallerThan(1, precision); \ + }, \ + C_expect, C_actual); + +} // namespace testing + +#endif /* GNSSUTILS_UTILS_GTEST_H */ diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7dd3b9ceea4ff0b3c7ea1c6f8db3286b273f7b66 --- /dev/null +++ b/test/gtest_example.cpp @@ -0,0 +1,20 @@ +#include "gtest/utils_gtest.h" + +TEST(TestTest, DummyTestExample) +{ + EXPECT_FALSE(false); + + ASSERT_TRUE(true); + + int my_int = 5; + + ASSERT_EQ(my_int, 5); + +// PRINTF("All good at TestTest::DummyTestExample !\n"); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/gtest_navigation.cpp b/test/gtest_navigation.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5170cf11310248899fd967b3b870f974a6368b2f --- /dev/null +++ b/test/gtest_navigation.cpp @@ -0,0 +1,13 @@ +#include "gtest/utils_gtest.h" +#include "gnss_utils/gnss_utils.h" + +TEST(NavigationTests, Whatever) +{ + +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/test/gtest_observations.cpp b/test/gtest_observations.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fc24115a0c7198f24b9b19718d219a9928ef78db --- /dev/null +++ b/test/gtest_observations.cpp @@ -0,0 +1,75 @@ +#include "gtest/utils_gtest.h" +#include "gnss_utils/gnss_utils.h" +#include "gnss_utils/observations.h" + +using namespace GNSSUtils; + +std::string rnx_file; +obs_t obs; +const gtime_t t_start{ 0, 0 }; // no limit +const gtime_t t_end{ 0, 0 }; // no limit +const double dt = 0.0; // no limit +const char* opt = "-SYS=G"; // only GPS | GPS+GAL: "-SYS=G,L" | ALL: "" + +void loadRinex() +{ + // RTKLIB utilities + obs.data = (obsd_t*)malloc(sizeof(obsd_t) * MAXSAT); + obs.n = 0; + obs.nmax = MAXSAT; + + int stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, &obs, NULL, NULL); + + ASSERT_EQ(stat, 1); + sortobs(&obs); +} + +TEST(ObservationsTest, AddClearObservation) +{ + loadRinex(); + + Observations observations; + + // testing addition + for (int ii = 0; ii < obs.n; ++ii) + { + observations.addObservation(obs.data[ii]); + ASSERT_TRUE(equalObservations(obs.data[ii], observations.getObservations()[ii])); + } + + ASSERT_TRUE(obs.n == observations.getObservations().size()); + + //Testing clear + observations.clearObservations(); + ASSERT_TRUE(0 == observations.getObservations().size()); + +} + +TEST(ObservationsTest, LoadFromRinex) +{ + // GNSSUtils utilities + Observations observations; + observations.loadFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt); + observations.print(); + + // RTKLIB utilities + loadRinex(); + ASSERT_EQ(obs.n, 6); + + // Comparison + ASSERT_TRUE(obs.n == observations.getObservations().size()); + + for (int ii = 0; ii < obs.n; ++ii) + { + ASSERT_TRUE(equalObservations(obs.data[ii], observations.getObservations()[ii])); + } + + free(obs.data); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + rnx_file = argv[1]; + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/test/gtest_transformations.cpp b/test/gtest_transformations.cpp new file mode 100644 index 0000000000000000000000000000000000000000..cae4e24bb4bd45e99fceaa07a201fe36926b5f78 --- /dev/null +++ b/test/gtest_transformations.cpp @@ -0,0 +1,200 @@ +#include "gtest/utils_gtest.h" +#include "gnss_utils/gnss_utils.h" + +// Geodetic system parameters +static double kSemimajorAxis = 6378137; +static double kSemiminorAxis = 6356752.3142; +static double kFirstEccentricitySquared = 6.69437999014 * 0.001; +static double kSecondEccentricitySquared = 6.73949674228 * 0.001; + +using namespace GNSSUtils; + +TEST(TransformationsTest, ecefToLatLonAlt) +{ + Eigen::Vector3d ecef, latlonalt; + + // aligned with ECEF X axis + ecef << 1e3, 0, 0; + latlonalt = ecefToLatLonAlt(ecef); + + ASSERT_NEAR(latlonalt(0),0.0,1e-6); + ASSERT_NEAR(latlonalt(1),0.0,1e-6); + + // aligned with ECEF Y axis + ecef << 0, 1e3, 0; + latlonalt = ecefToLatLonAlt(ecef); + + ASSERT_NEAR(latlonalt(0),0.0,1e-6); + ASSERT_NEAR(latlonalt(1),M_PI / 2,1e-6); + + // aligned with ECEF Z axis + ecef << 0, 0, 1e3; + latlonalt = ecefToLatLonAlt(ecef); + + ASSERT_NEAR(latlonalt(0),M_PI / 2,1e-6); + ASSERT_NEAR(latlonalt(1),0.0,1e-6); +} + +TEST(TransformationsTest, latLonAltToEcef) +{ + Eigen::Vector3d ecef, latlonalt; + + // aligned with ECEF X axis + latlonalt << 0, 0, 0; + ecef = latLonAltToEcef(latlonalt); + + ASSERT_NEAR(ecef(1),0.0,1e-6); + ASSERT_NEAR(ecef(2),0.0,1e-6); + + // aligned with ECEF Y axis + latlonalt << 0, M_PI / 2, 0; + ecef = latLonAltToEcef(latlonalt); + + ASSERT_NEAR(ecef(0),0.0,1e-6); + ASSERT_NEAR(ecef(2),0.0,1e-6); + + // aligned with ECEF Z axis + latlonalt << M_PI / 2, 0, 0; + ecef = latLonAltToEcef(latlonalt); + + ASSERT_NEAR(ecef(0),0.0,1e-6); + ASSERT_NEAR(ecef(1),0.0,1e-6); +} + +TEST(TransformationsTest, latLonAltToEcef_ecefToLatLonAlt) +{ + Eigen::Vector3d ecef0, ecef1, latlonalt0, latlonalt1; + + // ecef -> latlon -> ecef + for (auto i = 0; i<100; i++) + { + ecef0 = 1e3 * Eigen::Vector3d::Random(); + + latlonalt0 = ecefToLatLonAlt(ecef0); + ecef1 = latLonAltToEcef(latlonalt0); + ASSERT_MATRIX_APPROX(ecef0,ecef1,1e-6); + } + // latlon -> ecef -> latlon + for (auto i = 0; i<100; i++) + { + latlonalt0 = Eigen::Vector3d::Random(); + latlonalt0(0) *= M_PI / 2; + latlonalt0(1) *= M_PI; + latlonalt0(2) *= 1e3; + + ecef0 = latLonAltToEcef(latlonalt0); + latlonalt1 = ecefToLatLonAlt(ecef0); + ASSERT_MATRIX_APPROX(latlonalt0,latlonalt1,1e-6); + } +} + +TEST(TransformationsTest, computeEnuEcef) +{ + Eigen::Vector3d t_ECEF_ENU, t_ENU_latlonalt, latlonalt1; + Eigen::Vector3d t_ENU_ECEF1,t_ENU_ECEF2,t_ENU_ECEF3; + Eigen::Matrix3d R_ENU_ECEF1,R_ENU_ECEF2,R_ENU_ECEF3; + + // random + for (auto i = 0; i<100; i++) + { + t_ENU_latlonalt = Eigen::Vector3d::Random(); + t_ENU_latlonalt(0) *= M_PI / 2; + t_ENU_latlonalt(1) *= M_PI; + t_ENU_latlonalt(2) *= 1e3; + + t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt); + + // 1 from ECEF + computeEnuEcefFromEcef(t_ECEF_ENU, R_ENU_ECEF1, t_ENU_ECEF1); + // 2 from latlonalt + computeEnuEcefFromLatLonAlt(t_ENU_latlonalt, R_ENU_ECEF2, t_ENU_ECEF2); + // 3 Hardcoded solution + /* Convert ECEF coordinates to geodetic coordinates. + * J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates + * to geodetic coordinates," IEEE Transactions on Aerospace and + * Electronic Systems, vol. 30, pp. 957-961, 1994. + */ + double r = std::sqrt(t_ECEF_ENU(0) * t_ECEF_ENU(0) + t_ECEF_ENU(1) * t_ECEF_ENU(1)); + double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis; + double F = 54 * kSemiminorAxis * kSemiminorAxis * t_ECEF_ENU(2) * t_ECEF_ENU(2); + double G = r * r + (1 - kFirstEccentricitySquared) * t_ECEF_ENU(2) * t_ECEF_ENU(2) - kFirstEccentricitySquared * Esq; + double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3); + double S = cbrt(1 + C + sqrt(C * C + 2 * C)); + double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G); + double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P); + double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q) + + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q) + - P * (1 - kFirstEccentricitySquared) * t_ECEF_ENU(2) * t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P * r * r); + double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * t_ECEF_ENU(2) * t_ECEF_ENU(2)); + double Z_0 = kSemiminorAxis * kSemiminorAxis * t_ECEF_ENU(2) / (kSemimajorAxis * V); + + double latitude = atan((t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r); + double longitude = atan2(t_ECEF_ENU(1), t_ECEF_ENU(0)); + + double sLat = sin(latitude); + double cLat = cos(latitude); + double sLon = sin(longitude); + double cLon = cos(longitude); + + R_ENU_ECEF3(0,0) = -sLon; + R_ENU_ECEF3(0,1) = cLon; + R_ENU_ECEF3(0,2) = 0.0; + + R_ENU_ECEF3(1,0) = -sLat*cLon; + R_ENU_ECEF3(1,1) = -sLat * sLon; + R_ENU_ECEF3(1,2) = cLat; + + R_ENU_ECEF3(2,0) = cLat * cLon; + R_ENU_ECEF3(2,1) = cLat * sLon; + R_ENU_ECEF3(2,2) = sLat; + + t_ENU_ECEF3 = -R_ENU_ECEF3*t_ECEF_ENU; + + // CHECK + ASSERT_MATRIX_APPROX(t_ENU_ECEF1,t_ENU_ECEF2,1e-6); + ASSERT_MATRIX_APPROX(R_ENU_ECEF1,R_ENU_ECEF2,1e-6); + ASSERT_MATRIX_APPROX(t_ENU_ECEF1,t_ENU_ECEF3,1e-6); + ASSERT_MATRIX_APPROX(R_ENU_ECEF1,R_ENU_ECEF3,1e-6); + } +} + +TEST(TransformationsTest, computeSatElevation) +{ + Eigen::Vector3d t_ENU_ECEF,t_ECEF_ENU,t_ENU_latlonalt,rec_sat_ENU, sat_ECEF; + Eigen::Matrix3d R_ENU_ECEF; + + // random receiver position + for (auto i = 0; i<100; i++) + { + t_ENU_latlonalt = Eigen::Vector3d::Random(); + t_ENU_latlonalt(0) *= M_PI / 2; + t_ENU_latlonalt(1) *= M_PI; + t_ENU_latlonalt(2) *= 1e3; + + t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt); + + computeEnuEcefFromLatLonAlt(t_ENU_latlonalt, R_ENU_ECEF, t_ENU_ECEF); + + // random elevation and heading + for (auto j = 0; j<100; j++) + { + Eigen::Vector2d rand2 = Eigen::Vector2d::Random(); + double heading = rand2(0) * M_PI; + double elevation = rand2(0) * M_PI / 2; + + rec_sat_ENU << cos(heading) * 1000, sin(heading) * 1000, tan(elevation) * 1000; + + sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * rec_sat_ENU; + + double elevation2 = computeSatElevation(t_ECEF_ENU, sat_ECEF); + + ASSERT_NEAR(elevation, elevation2,1e-6); + } + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}