diff --git a/CMakeLists.txt b/CMakeLists.txt index 9be84be8d..4d8d7cc94 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -523,7 +523,6 @@ if(NOT GNURADIO_RUNTIME_FOUND) message("You can install it easily via Macports:") message(" sudo port install gnuradio ") message("Alternatively, you can use homebrew:") - message(" brew tap odrisci/gnuradio") message(" brew install gnuradio" ) message(FATAL_ERROR "GNU Radio ${GNSSSDR_GNURADIO_MIN_VERSION} or later is required to build gnss-sdr") endif(OS_IS_MACOSX) diff --git a/MANIFEST.md b/MANIFEST.md index d98411e9f..ea64ec678 100644 --- a/MANIFEST.md +++ b/MANIFEST.md @@ -12,11 +12,17 @@ author: - et altri (see AUTHORS file for a list of contributors) copyright_owner: - The Authors -dependencies: gnuradio (>= 3.7.3), armadillo, gflags, glog, gnutls, matio +dependencies: + - gnuradio (>= 3.7.3) + - armadillo + - gflags + - glog + - gnutls + - matio license: GPLv3+ repo: https://github.com/gnss-sdr/gnss-sdr website: https://gnss-sdr.org -icon: https://raw.githubusercontent.com/gnss-sdr/gnss-sdr/master/docs/doxygen/images/gnss-sdr_logo.png +icon: https://gnss-sdr.org/assets/images/logo400x400.jpg --- Global Navigation Satellite Systems receiver defined by software. It performs all the signal processing from raw signal samples up to the computation of the Position-Velocity-Time solution, diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 6bbf91c2d..9a5335c8a 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -364,8 +364,7 @@ rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels, d_rinexobs_rate_ms = rinexobs_rate_ms; d_rinexnav_rate_ms = rinexnav_rate_ms; - d_dump_filename.append("_raw.dat"); - dump_ls_pvt_filename.append("_ls_pvt.dat"); + dump_ls_pvt_filename.append("_pvt.dat"); d_ls_pvt = std::make_shared(static_cast(nchannels), dump_ls_pvt_filename, d_dump, rtk); d_ls_pvt->set_averaging_depth(1); @@ -374,23 +373,6 @@ rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels, d_last_status_print_seg = 0; - // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) - { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "PVT dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception opening PVT dump file " << e.what(); - } - } - } // Create Sys V message queue first_fix = true; @@ -500,18 +482,6 @@ rtklib_pvt_cc::~rtklib_pvt_cc() { LOG(WARNING) << "Failed to save GLONASS GNAV Ephemeris, map is empty"; } - - if (d_dump_file.is_open() == true) - { - try - { - d_dump_file.close(); - } - catch (const std::exception& ex) - { - LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); - } - } } @@ -2102,7 +2072,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item << std::fixed << std::setprecision(3) << " [deg], Height = " << d_ls_pvt->get_height() << " [m]" << TEXT_RESET << std::endl; std::cout << std::setprecision(ss); - LOG(INFO) << "RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]"; + DLOG(INFO) << "RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]"; // boost::posix_time::ptime p_time; // gtime_t rtklib_utc_time = gpst2time(adjgpsweek(d_ls_pvt->gps_ephemeris_map.cbegin()->second.i_GPS_week), d_rx_time); @@ -2110,36 +2080,15 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item // p_time += boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6)); // std::cout << TEXT_MAGENTA << "Observable RX time (GPST) " << boost::posix_time::to_simple_string(p_time) << TEXT_RESET << std::endl; - LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) - << " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude() - << " [deg], Height = " << d_ls_pvt->get_height() << " [m]"; + DLOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) + << " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude() + << " [deg], Height = " << d_ls_pvt->get_height() << " [m]"; /* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) << " UTC using "<< d_ls_pvt->get_num_valid_observations() <<" observations is HDOP = " << d_ls_pvt->get_hdop() << " VDOP = " << d_ls_pvt->get_vdop() << " GDOP = " << d_ls_pvt->get_gdop() << std::endl; */ } - - // MULTIPLEXED FILE RECORDING - Record results to file - if (d_dump == true) - { - try - { - double tmp_double; - for (uint32_t i = 0; i < d_nchannels; i++) - { - tmp_double = in[i][epoch].Pseudorange_m; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = 0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_rx_time), sizeof(double)); - } - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); - } - } } } diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index 97e581f86..36d09fab4 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -122,7 +122,6 @@ private: uint32_t d_nchannels; std::string d_dump_filename; - std::ofstream d_dump_file; int32_t d_output_rate_ms; int32_t d_display_rate_ms; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 472ffc9d4..1f87b8797 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -86,7 +86,7 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag } catch (const std::ifstream::failure& e) { - LOG(WARNING) << "Exception opening PVT lib dump file " << e.what(); + LOG(WARNING) << "Exception opening RTKLIB dump file " << e.what(); } } } @@ -103,7 +103,7 @@ rtklib_solver::~rtklib_solver() } catch (const std::exception& ex) { - LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); + LOG(WARNING) << "Exception in destructor closing the RTKLIB dump file " << ex.what(); } } } @@ -556,34 +556,55 @@ bool rtklib_solver::get_PVT(const std::map& gnss_observables_ try { double tmp_double; + uint32_t tmp_uint32; + // TOW + tmp_uint32 = gnss_observables_map.begin()->second.TOW_at_current_symbol_ms; + d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + // WEEK + tmp_uint32 = adjgpsweek(nav_data.eph[0].week); + d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); // PVT GPS time tmp_double = gnss_observables_map.begin()->second.RX_time; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // ECEF User Position East [m] - tmp_double = rx_position_and_time(0); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // ECEF User Position North [m] - tmp_double = rx_position_and_time(1); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // ECEF User Position Up [m] - tmp_double = rx_position_and_time(2); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // User clock offset [s] tmp_double = rx_position_and_time(3); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double) + d_dump_file.write(reinterpret_cast(&pvt_sol.rr[0]), sizeof(pvt_sol.rr)); + + // position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double) + d_dump_file.write(reinterpret_cast(&pvt_sol.qr[0]), sizeof(pvt_sol.qr)); + // GEO user position Latitude [deg] - tmp_double = this->get_latitude(); + tmp_double = get_latitude(); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // GEO user position Longitude [deg] - tmp_double = this->get_longitude(); + tmp_double = get_longitude(); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // GEO user position Height [m] - tmp_double = this->get_height(); + tmp_double = get_height(); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // NUMBER OF VALID SATS + d_dump_file.write(reinterpret_cast(&pvt_sol.ns), sizeof(uint8_t)); + // RTKLIB solution status + d_dump_file.write(reinterpret_cast(&pvt_sol.stat), sizeof(uint8_t)); + // RTKLIB solution type (0:xyz-ecef,1:enu-baseline) + d_dump_file.write(reinterpret_cast(&pvt_sol.type), sizeof(uint8_t)); + //AR ratio factor for validation + tmp_double = pvt_sol.ratio; + d_dump_file.write(reinterpret_cast(&pvt_sol.ratio), sizeof(float)); + //AR ratio threshold for validation + tmp_double = pvt_sol.thres; + d_dump_file.write(reinterpret_cast(&pvt_sol.thres), sizeof(float)); + + //GDOP//PDOP//HDOP//VDOP + d_dump_file.write(reinterpret_cast(&dop_[0]), sizeof(dop_)); } catch (const std::ifstream::failure& e) { - LOG(WARNING) << "Exception writing PVT LS dump file " << e.what(); + LOG(WARNING) << "Exception writing RTKLIB dump file " << e.what(); } } } diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index 88cc2ca38..f7e4e76c5 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -89,38 +89,6 @@ double leaps[MAXLEAPS + 1][7] = {/* leap seconds (y,m,d,h,m,s,utc-gpst) */ {}}; -const prcopt_t prcopt_default = { /* defaults processing options */ - PMODE_SINGLE, 0, 2, SYS_GPS, /* mode, soltype, nf, navsys */ - 15.0 * D2R, {{}, {{}, {}}}, /* elmin, snrmask */ - 0, 1, 1, 1, /* sateph, modear, glomodear, bdsmodear */ - 5, 0, 10, 1, /* maxout, minlock, minfix, armaxiter */ - 0, 0, 0, 0, /* estion, esttrop, dynamics, tidecorr */ - 1, 0, 0, 0, 0, /* niter, codesmooth, intpref, sbascorr, sbassatsel */ - 0, 0, /* rovpos, refpos */ - {100.0, 100.0, 100.0}, /* eratio[] */ - {100.0, 0.003, 0.003, 0.0, 1.0}, /* err[] */ - {30.0, 0.03, 0.3}, /* std[] */ - {1e-4, 1e-3, 1e-4, 1e-1, 1e-2, 0.0}, /* prn[] */ - 5E-12, /* sclkstab */ - {3.0, 0.9999, 0.25, 0.1, 0.05, 0, 0, 0}, /* thresar */ - 0.0, 0.0, 0.05, /* elmaskar, almaskhold, thresslip */ - 30.0, 30.0, 30.0, /* maxtdif, maxinno, maxgdop */ - {}, {}, {}, /* baseline, ru, rb */ - {"", ""}, /* anttype */ - {}, {}, {}, /* antdel, pcv, exsats */ - 0, 0, 0, {"", ""}, {}, 0, {{}, {}}, {{}, {{}, {}}, {{}, {}}, {}, {}}, 0, {}}; - - -const solopt_t solopt_default = { - /* defaults solution output options */ - SOLF_LLH, TIMES_GPST, 1, 3, /* posf, times, timef, timeu */ - 0, 1, 0, 0, 0, 0, /* degf, outhead, outopt, datum, height, geoid */ - 0, 0, 0, /* solstatic, sstat, trace */ - {0.0, 0.0}, /* nmeaintv */ - " ", "", 0 /* separator/program name */ -}; - - const char *formatstrs[32] = {/* stream format strings */ "RTCM 2", /* 0 */ "RTCM 3", /* 1 */ diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 6c4448995..8ed59e572 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -18,6 +18,7 @@ add_subdirectory(unit-tests/signal-processing-blocks/libs) +add_subdirectory(system-tests/libs) ################################################################################ # Google Test - https://github.com/google/googletest @@ -342,6 +343,7 @@ set(LIST_INCLUDE_DIRS ${CMAKE_SOURCE_DIR}/src/algorithms/acquisition/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs ${CMAKE_SOURCE_DIR}/src/tests/unit-tests/signal-processing-blocks/libs + ${CMAKE_SOURCE_DIR}/src/tests/system-tests/libs ${CMAKE_SOURCE_DIR}/src/tests/common-files ${GLOG_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} @@ -489,29 +491,24 @@ if(ENABLE_SYSTEM_TESTING) ${GTEST_LIBRARIES} ${GNURADIO_RUNTIME_LIBRARIES} ${GNURADIO_BLOCKS_LIBRARIES} ${GNURADIO_FILTER_LIBRARIES} ${GNURADIO_ANALOG_LIBRARIES} ${VOLK_GNSSSDR_LIBRARIES} - gnss_sp_libs gnss_rx gnss_system_parameters ) + gnss_sp_libs gnss_rx gnss_system_parameters + system_testing_lib) add_system_test(position_test) - if(GPSTK_FOUND OR OWN_GPSTK) + #if(GPSTK_FOUND OR OWN_GPSTK) ## OBS_SYSTEM_TEST and OBS_GPS_L1_SYSTEM_TEST - set(OPT_LIBS_ ${GFlags_LIBS} ${GLOG_LIBRARIES} ${GTEST_LIBRARIES} - gnss_sp_libs gnss_rx ${gpstk_libs} ) - set(OPT_INCLUDES_ ${GPSTK_INCLUDE_DIRS} ${GPSTK_INCLUDE_DIRS}/gpstk) - add_system_test(obs_gps_l1_system_test) - add_system_test(obs_system_test) - endif(GPSTK_FOUND OR OWN_GPSTK) + # set(OPT_LIBS_ ${GFlags_LIBS} ${GLOG_LIBRARIES} ${GTEST_LIBRARIES} + # gnss_sp_libs gnss_rx ${gpstk_libs} ) + # set(OPT_INCLUDES_ ${GPSTK_INCLUDE_DIRS} ${GPSTK_INCLUDE_DIRS}/gpstk) + # add_system_test(obs_gps_l1_system_test) + # add_system_test(obs_system_test) + #endif(GPSTK_FOUND OR OWN_GPSTK) else(ENABLE_SYSTEM_TESTING_EXTRA) # Avoid working with old executables if they were switched ON and then OFF if(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test) file(REMOVE ${CMAKE_SOURCE_DIR}/install/position_test) endif(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test) - if(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test) - file(REMOVE ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test) - endif(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test) - if(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_system_test) - file(REMOVE ${CMAKE_SOURCE_DIR}/install/obs_system_test) - endif(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_system_test) endif(ENABLE_SYSTEM_TESTING_EXTRA) else(ENABLE_SYSTEM_TESTING) # Avoid working with old executables if they were switched ON and then OFF @@ -521,12 +518,6 @@ else(ENABLE_SYSTEM_TESTING) if(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test) file(REMOVE ${CMAKE_SOURCE_DIR}/install/position_test) endif(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test) - if(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test) - file(REMOVE ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test) - endif(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test) - if(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_system_test) - file(REMOVE ${CMAKE_SOURCE_DIR}/install/obs_system_test) - endif(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_system_test) endif(ENABLE_SYSTEM_TESTING) diff --git a/src/tests/common-files/signal_generator_flags.h b/src/tests/common-files/signal_generator_flags.h index 399c1ce26..b5e737adc 100644 --- a/src/tests/common-files/signal_generator_flags.h +++ b/src/tests/common-files/signal_generator_flags.h @@ -38,7 +38,7 @@ DEFINE_bool(disable_generator, false, "Disable the signal generator (a external DEFINE_string(generator_binary, std::string(SW_GENERATOR_BIN), "Path of software-defined signal generator binary"); DEFINE_string(rinex_nav_file, std::string(DEFAULT_RINEX_NAV), "Input RINEX navigation file"); DEFINE_int32(duration, 100, "Duration of the experiment [in seconds, max = 300]"); -DEFINE_string(static_position, "30.286502,120.032669,100", "Static receiver position [log,lat,height]"); +DEFINE_string(static_position, "30.286502,120.032669,100", "Static receiver position [latitude,longitude,height]"); DEFINE_string(dynamic_position, "", "Observer positions file, in .csv or .nmea format"); DEFINE_string(filename_rinex_obs, "sim.16o", "Filename of output RINEX navigation file"); DEFINE_string(filename_raw_data, "signal_out.bin", "Filename of output raw data file"); diff --git a/src/tests/system-tests/libs/CMakeLists.txt b/src/tests/system-tests/libs/CMakeLists.txt new file mode 100644 index 000000000..a6459b058 --- /dev/null +++ b/src/tests/system-tests/libs/CMakeLists.txt @@ -0,0 +1,41 @@ +# Copyright (C) 2012-2018 (see AUTHORS file for a list of contributors) +# +# This file is part of GNSS-SDR. +# +# GNSS-SDR is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# GNSS-SDR is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with GNSS-SDR. If not, see . +# + + +set(SYSTEM_TESTING_LIB_SOURCES + geofunctions.cc + spirent_motion_csv_dump_reader.cc + rtklib_solver_dump_reader.cc +) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR} + ${GLOG_INCLUDE_DIRS} + ${GFlags_INCLUDE_DIRS} + ${MATIO_INCLUDE_DIRS} +) + + +file(GLOB SYSTEM_TESTING_LIB_HEADERS "*.h") +list(SORT SYSTEM_TESTING_LIB_HEADERS) +add_library(system_testing_lib ${SYSTEM_TESTING_LIB_SOURCES} ${SYSTEM_TESTING_LIB_HEADERS}) +source_group(Headers FILES ${SYSTEM_TESTING_LIB_HEADERS}) + +if(NOT MATIO_FOUND) + add_dependencies(system_testing_lib matio-${GNSSSDR_MATIO_LOCAL_VERSION}) +endif(NOT MATIO_FOUND) \ No newline at end of file diff --git a/src/tests/system-tests/libs/geofunctions.cc b/src/tests/system-tests/libs/geofunctions.cc new file mode 100644 index 000000000..fbbe97266 --- /dev/null +++ b/src/tests/system-tests/libs/geofunctions.cc @@ -0,0 +1,473 @@ +/*! + * \file geofunctions.h + * \brief A set of coordinate transformations functions and helpers, + * some of them migrated from MATLAB, for geographic information systems. + * \author Javier Arribas, 2018. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ +#include "geofunctions.h" + +#define STRP_G_SI 9.80665 +#define STRP_PI 3.1415926535898 //!< Pi as defined in IS-GPS-200E + +arma::mat Skew_symmetric(arma::vec a) +{ + arma::mat A = arma::zeros(3, 3); + + A << 0.0 << -a(2) << a(1) << arma::endr + << a(2) << 0.0 << -a(0) << arma::endr + << -a(1) << a(0) << 0 << arma::endr; + + // {{0, -a(2), a(1)}, + // {a(2), 0, -a(0)}, + // {-a(1), a(0), 0}}; + return A; +} + + +double WGS84_g0(double Lat_rad) +{ + double k = 0.001931853; //normal gravity constant + double e2 = 0.00669438002290; //the square of the first numerical eccentricity + double nge = 9.7803253359; //normal gravity value on the equator (m/sec^2) + double b = sin(Lat_rad); //Lat in degrees + b = b * b; + double g0 = nge * (1 + k * b) / (sqrt(1 - e2 * b)); + return g0; +} + +double WGS84_geocentric_radius(double Lat_geodetic_rad) +{ + //WGS84 earth model Geocentric radius (Eq. 2.88) + + double WGS84_A = 6378137.0; //Semi-major axis of the Earth, a [m] + double WGS84_IF = 298.257223563; //Inverse flattening of the Earth + double WGS84_F = (1 / WGS84_IF); //The flattening of the Earth + //double WGS84_B=(WGS84_A*(1-WGS84_F)); // Semi-minor axis of the Earth [m] + double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F)); //Eccentricity of the Earth + + //transverse radius of curvature + double R_E = WGS84_A / sqrt(1 - + WGS84_E * WGS84_E * + sin(Lat_geodetic_rad) * + sin(Lat_geodetic_rad)); // (Eq. 2.66) + + //gocentric radius at the Earth surface + double r_eS = R_E * sqrt(cos(Lat_geodetic_rad) * cos(Lat_geodetic_rad) + + (1 - WGS84_E * WGS84_E) * (1 - WGS84_E * WGS84_E) * sin(Lat_geodetic_rad) * sin(Lat_geodetic_rad)); // (Eq. 2.88) + return r_eS; +} + +int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx) +{ + double lambda; + double phi; + double h; + double dtr = STRP_PI / 180.0; + double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84 + double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84 + + // Transform x into geodetic coordinates + togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2)); + + double cl = cos(lambda * dtr); + double sl = sin(lambda * dtr); + double cb = cos(phi * dtr); + double sb = sin(phi * dtr); + + arma::mat F = arma::zeros(3, 3); + + F(0, 0) = -sl; + F(0, 1) = -sb * cl; + F(0, 2) = cb * cl; + + F(1, 0) = cl; + F(1, 1) = -sb * sl; + F(1, 2) = cb * sl; + + F(2, 0) = 0; + F(2, 1) = cb; + F(2, 2) = sb; + + arma::vec local_vector; + + local_vector = arma::htrans(F) * dx; + + double E = local_vector(0); + double N = local_vector(1); + double U = local_vector(2); + + double hor_dis; + hor_dis = sqrt(E * E + N * N); + + if (hor_dis < 1.0E-20) + { + *Az = 0; + *El = 90; + } + else + { + *Az = atan2(E, N) / dtr; + *El = atan2(U, hor_dis) / dtr; + } + + if (*Az < 0) + { + *Az = *Az + 360.0; + } + + *D = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); + return 0; +} + + +int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z) +{ + *h = 0; + double tolsq = 1.e-10; // tolerance to accept convergence + int maxit = 10; // max number of iterations + double rtd = 180.0 / STRP_PI; + + // compute square of eccentricity + double esq; + if (finv < 1.0E-20) + { + esq = 0.0; + } + else + { + esq = (2.0 - 1.0 / finv) / finv; + } + + // first guess + double P = sqrt(X * X + Y * Y); // P is distance from spin axis + + //direct calculation of longitude + if (P > 1.0E-20) + { + *dlambda = atan2(Y, X) * rtd; + } + else + { + *dlambda = 0.0; + } + + // correct longitude bound + if (*dlambda < 0) + { + *dlambda = *dlambda + 360.0; + } + + double r = sqrt(P * P + Z * Z); // r is distance from origin (0,0,0) + + double sinphi; + if (r > 1.0E-20) + { + sinphi = Z / r; + } + else + { + sinphi = 0.0; + } + *dphi = asin(sinphi); + + // initial value of height = distance from origin minus + // approximate distance from origin to surface of ellipsoid + if (r < 1.0E-20) + { + *h = 0; + return 1; + } + + *h = r - a * (1 - sinphi * sinphi / finv); + + // iterate + double cosphi; + double N_phi; + double dP; + double dZ; + double oneesq = 1.0 - esq; + + for (int i = 0; i < maxit; i++) + { + sinphi = sin(*dphi); + cosphi = cos(*dphi); + + // compute radius of curvature in prime vertical direction + N_phi = a / sqrt(1 - esq * sinphi * sinphi); + + // compute residuals in P and Z + dP = P - (N_phi + (*h)) * cosphi; + dZ = Z - (N_phi * oneesq + (*h)) * sinphi; + + // update height and latitude + *h = *h + (sinphi * dZ + cosphi * dP); + *dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h)); + + // test for convergence + if ((dP * dP + dZ * dZ) < tolsq) + { + break; + } + if (i == (maxit - 1)) + { + // LOG(WARNING) << "The computation of geodetic coordinates did not converge"; + } + } + *dphi = (*dphi) * rtd; + return 0; +} + +arma::mat Gravity_ECEF(arma::vec r_eb_e) +{ + //Parameters + double R_0 = 6378137; //WGS84 Equatorial radius in meters + double mu = 3.986004418E14; //WGS84 Earth gravitational constant (m^3 s^-2) + double J_2 = 1.082627E-3; //WGS84 Earth's second gravitational constant + double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s) + // Calculate distance from center of the Earth + double mag_r = sqrt(arma::as_scalar(r_eb_e.t() * r_eb_e)); + // If the input position is 0,0,0, produce a dummy output + arma::vec g = arma::zeros(3, 1); + if (mag_r != 0) + { + //Calculate gravitational acceleration using (2.142) + double z_scale = 5 * pow((r_eb_e(2) / mag_r), 2); + arma::vec tmp_vec = {(1 - z_scale) * r_eb_e(0), + (1 - z_scale) * r_eb_e(1), + (3 - z_scale) * r_eb_e(2)}; + arma::vec gamma_ = (-mu / pow(mag_r, 3)) * (r_eb_e + 1.5 * J_2 * pow(R_0 / mag_r, 2) * tmp_vec); + + //Add centripetal acceleration using (2.133) + g(0) = gamma_(0) + pow(omega_ie, 2) * r_eb_e(0); + g(1) = gamma_(1) + pow(omega_ie, 2) * r_eb_e(1); + g(2) = gamma_(2); + } + return g; +} + +arma::vec LLH_to_deg(arma::vec LLH) +{ + double rtd = 180.0 / STRP_PI; + LLH(0) = LLH(0) * rtd; + LLH(1) = LLH(1) * rtd; + return LLH; +} + +double degtorad(double angleInDegrees) +{ + double angleInRadians = (STRP_PI / 180.0) * angleInDegrees; + return angleInRadians; +} + +double radtodeg(double angleInRadians) +{ + double angleInDegrees = (180.0 / STRP_PI) * angleInRadians; + return angleInDegrees; +} + + +double mstoknotsh(double MetersPerSeconds) +{ + double knots = mstokph(MetersPerSeconds) * 0.539957; + return knots; +} + +double mstokph(double MetersPerSeconds) +{ + double kph = 3600.0 * MetersPerSeconds / 1e3; + return kph; +} + +arma::vec CTM_to_Euler(arma::mat C) +{ + //Calculate Euler angles using (2.23) + arma::vec eul = arma::zeros(3, 1); + eul(0) = atan2(C(1, 2), C(2, 2)); // roll + if (C(0, 2) < -1.0) C(0, 2) = -1.0; + if (C(0, 2) > 1.0) C(0, 2) = 1.0; + eul(1) = -asin(C(0, 2)); // pitch + eul(2) = atan2(C(0, 1), C(0, 0)); // yaw + return eul; +} + +arma::mat Euler_to_CTM(arma::vec eul) +{ + //Eq.2.15 + //Euler angles to Attitude matrix is equivalent to rotate the body + //in the three axes: + // arma::mat Ax= {{1,0,0}, {0,cos(Att_phi),sin(Att_phi)} ,{0,-sin(Att_phi),cos(Att_phi)}}; + // arma::mat Ay= {{cos(Att_theta), 0, -sin(Att_theta)}, {0,1,0} , {sin(Att_theta), 0, cos(Att_theta)}}; + // arma::mat Az= {{cos(Att_psi), sin(Att_psi), 0}, {-sin(Att_psi), cos(Att_psi), 0},{0,0,1}}; + // arma::mat C_b_n=Ax*Ay*Az; // Attitude expressed in the LOCAL FRAME (NED) + // C_b_n=C_b_n.t(); + + //Precalculate sines and cosines of the Euler angles + double sin_phi = sin(eul(0)); + double cos_phi = cos(eul(0)); + double sin_theta = sin(eul(1)); + double cos_theta = cos(eul(1)); + double sin_psi = sin(eul(2)); + double cos_psi = cos(eul(2)); + + arma::mat C = arma::zeros(3, 3); + //Calculate coordinate transformation matrix using (2.22) + C(0, 0) = cos_theta * cos_psi; + C(0, 1) = cos_theta * sin_psi; + C(0, 2) = -sin_theta; + C(1, 0) = -cos_phi * sin_psi + sin_phi * sin_theta * cos_psi; + C(1, 1) = cos_phi * cos_psi + sin_phi * sin_theta * sin_psi; + C(1, 2) = sin_phi * cos_theta; + C(2, 0) = sin_phi * sin_psi + cos_phi * sin_theta * cos_psi; + C(2, 1) = -sin_phi * cos_psi + cos_phi * sin_theta * sin_psi; + C(2, 2) = cos_phi * cos_theta; + return C; +} + +arma::vec cart2geo(arma::vec XYZ, int elipsoid_selection) +{ + const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0}; + const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563}; + + double lambda = atan2(XYZ[1], XYZ[0]); + double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection])); + double c = a[elipsoid_selection] * sqrt(1.0 + ex2); + double phi = atan(XYZ[2] / ((sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection]))); + + double h = 0.1; + double oldh = 0.0; + double N; + int iterations = 0; + do + { + oldh = h; + N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi))); + phi = atan(XYZ[2] / ((sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h))))); + h = sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) / cos(phi) - N; + iterations = iterations + 1; + if (iterations > 100) + { + //std::cout << "Failed to approximate h with desired precision. h-oldh= " << h - oldh; + break; + } + } + while (std::abs(h - oldh) > 1.0e-12); + + arma::vec LLH = {{phi, lambda, h}}; //radians + return LLH; +} + +void ECEF_to_Geo(arma::vec r_eb_e, arma::vec v_eb_e, arma::mat C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n) +{ + //Compute the Latitude of the ECEF position + LLH = cart2geo(r_eb_e, 4); //ECEF -> WGS84 geographical + + // Calculate ECEF to Geographical coordinate transformation matrix using (2.150) + double cos_lat = cos(LLH(0)); + double sin_lat = sin(LLH(0)); + double cos_long = cos(LLH(1)); + double sin_long = sin(LLH(1)); + //C++11 and arma >= 5.2 + // arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat}, + // {-sin_long, cos_long, 0}, + // {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}}; //ECEF to Geo + + //C++98 arma <5.2 + arma::mat C_e_n = arma::zeros(3, 3); + C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr + << -sin_long << cos_long << 0 << arma::endr + << -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr; //ECEF to Geo + // Transform velocity using (2.73) + v_eb_n = C_e_n * v_eb_e; + + C_b_n = C_e_n * C_b_e; // Attitude conversion from ECEF to NED +} + +void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e) +{ + // Parameters + double R_0 = 6378137; //WGS84 Equatorial radius in meters + double e = 0.0818191908425; //WGS84 eccentricity + + // Calculate transverse radius of curvature using (2.105) + double R_E = R_0 / sqrt(1 - (e * sin(LLH(0))) * (e * sin(LLH(0)))); + + // Convert position using (2.112) + double cos_lat = cos(LLH(0)); + double sin_lat = sin(LLH(0)); + double cos_long = cos(LLH(1)); + double sin_long = sin(LLH(1)); + r_eb_e = {(R_E + LLH(2)) * cos_lat * cos_long, + (R_E + LLH(2)) * cos_lat * sin_long, + ((1 - e * e) * R_E + LLH(2)) * sin_lat}; + + //Calculate ECEF to Geo coordinate transformation matrix using (2.150) + //C++11 and arma>=5.2 + // arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat}, + // {-sin_long, cos_long, 0}, + // {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}}; + //C++98 arma <5.2 + //Calculate ECEF to Geo coordinate transformation matrix using (2.150) + arma::mat C_e_n = arma::zeros(3, 3); + C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr + << -sin_long << cos_long << 0 << arma::endr + << -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr; + + // Transform velocity using (2.73) + v_eb_e = C_e_n.t() * v_eb_n; + + // Transform attitude using (2.15) + C_b_e = C_e_n.t() * C_b_n; +} + + +void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, arma::vec v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e) +{ + //% Parameters + double R_0 = 6378137; //WGS84 Equatorial radius in meters + double e = 0.0818191908425; //WGS84 eccentricity + + // Calculate transverse radius of curvature using (2.105) + double R_E = R_0 / sqrt(1 - pow(e * sin(L_b), 2)); + + // Convert position using (2.112) + double cos_lat = cos(L_b); + double sin_lat = sin(L_b); + double cos_long = cos(lambda_b); + double sin_long = sin(lambda_b); + r_eb_e = {(R_E + h_b) * cos_lat * cos_long, + (R_E + h_b) * cos_lat * sin_long, + ((1 - pow(e, 2)) * R_E + h_b) * sin_lat}; + + // Calculate ECEF to Geo coordinate transformation matrix using (2.150) + arma::mat C_e_n = arma::zeros(3, 3); + C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr + << -sin_long << cos_long << 0 << arma::endr + << -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr; + + // Transform velocity using (2.73) + v_eb_e = C_e_n.t() * v_eb_n; +} diff --git a/src/tests/system-tests/libs/geofunctions.h b/src/tests/system-tests/libs/geofunctions.h new file mode 100644 index 000000000..4cd0fb90d --- /dev/null +++ b/src/tests/system-tests/libs/geofunctions.h @@ -0,0 +1,156 @@ +/*! + * \file geofunctions.h + * \brief A set of coordinate transformations functions and helpers, + * some of them migrated from MATLAB, for geographic information systems. + * \author Javier Arribas, 2018. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GEOFUNCTIONS_H +#define GEOFUNCTIONS_H + +#include +// %Skew_symmetric - Calculates skew-symmetric matrix +arma::mat Skew_symmetric(arma::vec a); + +double WGS84_g0(double Lat_rad); + +double WGS84_geocentric_radius(double Lat_geodetic_rad); + +/* Transformation of vector dx into topocentric coordinate + system with origin at x + Inputs: + x - vector origin coordinates (in ECEF system [X; Y; Z;]) + dx - vector ([dX; dY; dZ;]). + + Outputs: + D - vector length. Units like the input + Az - azimuth from north positive clockwise, degrees + El - elevation angle, degrees + + Based on a Matlab function by Kai Borre + */ +int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx); + +/* Subroutine to calculate geodetic coordinates latitude, longitude, + height given Cartesian coordinates X,Y,Z, and reference ellipsoid + values semi-major axis (a) and the inverse of flattening (finv). + + The output units of angular quantities will be in decimal degrees + (15.5 degrees not 15 deg 30 min). The output units of h will be the + same as the units of X,Y,Z,a. + + Inputs: + a - semi-major axis of the reference ellipsoid + finv - inverse of flattening of the reference ellipsoid + X,Y,Z - Cartesian coordinates + + Outputs: + dphi - latitude + dlambda - longitude + h - height above reference ellipsoid + + Based in a Matlab function by Kai Borre + */ +int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z); + + +//Gravitation_ECI - Calculates acceleration due to gravity resolved about +//ECEF-frame +arma::mat Gravity_ECEF(arma::vec r_eb_e); + +/* Conversion of Cartesian coordinates (X,Y,Z) to geographical + coordinates (latitude, longitude, h) on a selected reference ellipsoid. + + Choices of Reference Ellipsoid for Geographical Coordinates + 0. International Ellipsoid 1924 + 1. International Ellipsoid 1967 + 2. World Geodetic System 1972 + 3. Geodetic Reference System 1980 + 4. World Geodetic System 1984 + */ +arma::vec cart2geo(arma::vec XYZ, int elipsoid_selection); + +arma::vec LLH_to_deg(arma::vec LLH); + +double degtorad(double angleInDegrees); + +double radtodeg(double angleInRadians); + +double mstoknotsh(double MetersPerSeconds); + +double mstokph(double Kph); + + +arma::vec CTM_to_Euler(arma::mat C); + +arma::mat Euler_to_CTM(arma::vec eul); + +void ECEF_to_Geo(arma::vec r_eb_e, arma::vec v_eb_e, arma::mat C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n); + + +// % +// % Inputs: +// % L_b latitude (rad) +// % lambda_b longitude (rad) +// % h_b height (m) +// % v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along +// % north, east, and down (m/s) +// % C_b_n body-to-NED coordinate transformation matrix +// % +// % Outputs: +// % r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved +// % along ECEF-frame axes (m) +// % v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along +// % ECEF-frame axes (m/s) +// % C_b_e body-to-ECEF-frame coordinate transformation matrix +// +// % Copyright 2012, Paul Groves +// % License: BSD; see license.txt for details + +void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e); + + +//pv_Geo_to_ECEF - Converts curvilinear to Cartesian position and velocity +//resolving axes from NED to ECEF +//This function created 11/4/2012 by Paul Groves +//% +//% Inputs: +//% L_b latitude (rad) +//% lambda_b longitude (rad) +//% h_b height (m) +//% v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along +//% north, east, and down (m/s) +//% +//% Outputs: +//% r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved +//% along ECEF-frame axes (m) +//% v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along +//% ECEF-frame axes (m/s) + +void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, arma::vec v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e); + +#endif diff --git a/src/tests/system-tests/libs/position_test_flags.h b/src/tests/system-tests/libs/position_test_flags.h new file mode 100644 index 000000000..e911fb892 --- /dev/null +++ b/src/tests/system-tests/libs/position_test_flags.h @@ -0,0 +1,46 @@ +/*! + * \file signal_generator_flags.h + * \brief Helper file for unit testing + * \author Javier Arribas, 2018. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_POSITION_TEST_FLAGS_H_ +#define GNSS_SDR_POSITION_TEST_FLAGS_H_ + +#include +#include + +DEFINE_string(config_file_ptest, std::string(""), "File containing the configuration parameters for the position test."); +DEFINE_bool(plot_position_test, false, "Plots results of with gnuplot"); +DEFINE_bool(static_scenario, true, "Compute figures of merit for static user position (DRMS, CEP, etc..)"); +DEFINE_bool(use_pvt_solver_dump, false, "Use PVT solver binary dump or fall back to KML PVT file (contains only position information)"); +DEFINE_bool(use_ref_motion_file, false, "Enable or disable the use of a reference file containing the true receiver position, velocity and acceleration."); +DEFINE_int32(ref_motion_file_type, 1, "Type of reference motion file: 1- Spirent CSV motion file"); +DEFINE_string(ref_motion_filename, std::string("motion.csv"), "Path and filename for the reference motion file"); +DEFINE_string(pvt_solver_dump_filename, std::string("PVT_pvt.dat"), "Path and filename for the PVT solver binary dump file"); + +#endif diff --git a/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc b/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc new file mode 100644 index 000000000..286a5fc54 --- /dev/null +++ b/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc @@ -0,0 +1,153 @@ +/*! + * \file rtklib_solver_dump_reader.cc + * \brief Helper file for unit testing + * \author Javier Arribas, 2017. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "rtklib_solver_dump_reader.h" + +#include + +bool rtklib_solver_dump_reader::read_binary_obs() +{ + try + { + d_dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms), sizeof(TOW_at_current_symbol_ms)); + // std::cout << "TOW_at_current_symbol_ms: " << TOW_at_current_symbol_ms << std::endl; + d_dump_file.read(reinterpret_cast(&week), sizeof(week)); + // std::cout << "week: " << week << std::endl; + d_dump_file.read(reinterpret_cast(&RX_time), sizeof(RX_time)); + // std::cout << "RX_time: " << RX_time << std::endl; + d_dump_file.read(reinterpret_cast(&clk_offset_s), sizeof(clk_offset_s)); + // std::cout << "clk_offset_s: " << clk_offset_s << std::endl; + d_dump_file.read(reinterpret_cast(&rr[0]), sizeof(rr)); + // for (int n = 0; n < 6; n++) + // { + // std::cout << "rr: " << rr[n] << std::endl; + // } + d_dump_file.read(reinterpret_cast(&qr[0]), sizeof(qr)); + // for (int n = 0; n < 6; n++) + // { + // std::cout << "qr" << qr[n] << std::endl; + // } + d_dump_file.read(reinterpret_cast(&latitude), sizeof(latitude)); + //std::cout << "latitude: " << latitude << std::endl; + d_dump_file.read(reinterpret_cast(&longitude), sizeof(longitude)); + //std::cout << "longitude: " << longitude << std::endl; + d_dump_file.read(reinterpret_cast(&height), sizeof(height)); + //std::cout << "height: " << height << std::endl; + d_dump_file.read(reinterpret_cast(&ns), sizeof(ns)); + // std::cout << "ns: " << (int)ns << std::endl; + d_dump_file.read(reinterpret_cast(&status), sizeof(status)); + // std::cout << "status: " << (int)status << std::endl; + d_dump_file.read(reinterpret_cast(&type), sizeof(type)); + // std::cout << "type: " << (int)type << std::endl; + d_dump_file.read(reinterpret_cast(&AR_ratio), sizeof(AR_ratio)); + // std::cout << "AR_ratio: " << AR_ratio << std::endl; + d_dump_file.read(reinterpret_cast(&AR_thres), sizeof(AR_thres)); + // std::cout << "AR_thres: " << AR_thres << std::endl; + d_dump_file.read(reinterpret_cast(&dop[0]), sizeof(dop)); + + // for (int n = 0; n < 4; n++) + // { + // std::cout << "dop" << dop[n] << std::endl; + // } + // getchar(); + } + catch (const std::ifstream::failure &e) + { + return false; + } + return true; +} + + +bool rtklib_solver_dump_reader::restart() +{ + if (d_dump_file.is_open()) + { + d_dump_file.clear(); + d_dump_file.seekg(0, std::ios::beg); + return true; + } + else + { + return false; + } +} + + +int64_t rtklib_solver_dump_reader::num_epochs() +{ + std::ifstream::pos_type size; + int epoch_size_bytes = sizeof(TOW_at_current_symbol_ms) + sizeof(week) + sizeof(RX_time) + sizeof(clk_offset_s) + sizeof(rr) + sizeof(qr) + sizeof(latitude) + sizeof(longitude) + sizeof(height) + sizeof(ns) + sizeof(status) + sizeof(type) + sizeof(AR_ratio) + sizeof(AR_thres) + sizeof(dop); + + std::ifstream tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); + if (tmpfile.is_open()) + { + size = tmpfile.tellg(); + int64_t nepoch = size / epoch_size_bytes; + return nepoch; + } + else + { + return 0; + } +} + + +bool rtklib_solver_dump_reader::open_obs_file(std::string out_file) +{ + if (d_dump_file.is_open() == false) + { + try + { + d_dump_filename = out_file; + d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(d_dump_filename.c_str(), std::ios::in | std::ios::binary); + return true; + } + catch (const std::ifstream::failure &e) + { + std::cout << "Problem opening rtklib_solver dump Log file: " << d_dump_filename.c_str() << std::endl; + return false; + } + } + else + { + return false; + } +} + + +rtklib_solver_dump_reader::~rtklib_solver_dump_reader() +{ + if (d_dump_file.is_open() == true) + { + d_dump_file.close(); + } +} diff --git a/src/tests/system-tests/libs/rtklib_solver_dump_reader.h b/src/tests/system-tests/libs/rtklib_solver_dump_reader.h new file mode 100644 index 000000000..e89659b37 --- /dev/null +++ b/src/tests/system-tests/libs/rtklib_solver_dump_reader.h @@ -0,0 +1,88 @@ +/*! + * \file rtklib_solver_dump_reader.h + * \brief Helper file for unit testing + * \author Javier Arribas, 2017. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_RTKLIB_SOLVER_DUMP_READER_H +#define GNSS_SDR_RTKLIB_SOLVER_DUMP_READER_H + +#include +#include +#include +#include + +class rtklib_solver_dump_reader +{ +public: + ~rtklib_solver_dump_reader(); + bool read_binary_obs(); + bool restart(); + int64_t num_epochs(); + bool open_obs_file(std::string out_file); + + //rtklib_solver dump variables + // TOW + uint32_t TOW_at_current_symbol_ms; + // WEEK + uint32_t week; + // PVT GPS time + double RX_time; + // User clock offset [s] + double clk_offset_s; + // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double) + double rr[6]; + // position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double) + float qr[6]; + + // GEO user position Latitude [deg] + double latitude; + // GEO user position Longitude [deg] + double longitude; + // GEO user position Height [m] + double height; + + // NUMBER OF VALID SATS + uint8_t ns; + // RTKLIB solution status + uint8_t status; + // RTKLIB solution type (0:xyz-ecef,1:enu-baseline) + uint8_t type; + //AR ratio factor for validation + float AR_ratio; + //AR ratio threshold for validation + float AR_thres; + + //GDOP//PDOP//HDOP//VDOP + double dop[4]; + +private: + std::string d_dump_filename; + std::ifstream d_dump_file; +}; + +#endif //GNSS_SDR_RTKLIB_SOLVER_DUMP_READER_H diff --git a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc new file mode 100644 index 000000000..edbf12ad0 --- /dev/null +++ b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc @@ -0,0 +1,205 @@ +/*! + * \file spirent_motion_csv_dump_reader.cc + * \brief Helper file for unit testing + * \author Javier Arribas, 2017. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "spirent_motion_csv_dump_reader.h" +#include + +bool spirent_motion_csv_dump_reader::read_csv_obs() +{ + try + { + std::vector vec; + std::string line; + if (getline(d_dump_file, line)) + { + boost::tokenizer> tk( + line, boost::escaped_list_separator('\\', ',', '\"')); + for (boost::tokenizer>::iterator i( + tk.begin()); + i != tk.end(); ++i) + { + try + { + vec.push_back(std::stod(*i)); + } + catch (const std::exception &ex) + { + vec.push_back(0.0); + } + } + parse_vector(vec); + } + } + catch (const std::ifstream::failure &e) + { + return false; + } + return true; +} + +bool spirent_motion_csv_dump_reader::parse_vector(std::vector &vec) +{ + try + { + int n = 0; + TOW_ms = vec.at(n++); + Pos_X = vec.at(n++); + Pos_Y = vec.at(n++); + Pos_Z = vec.at(n++); + Vel_X = vec.at(n++); + Vel_Y = vec.at(n++); + Vel_Z = vec.at(n++); + Acc_X = vec.at(n++); + Acc_Y = vec.at(n++); + Acc_Z = vec.at(n++); + Jerk_X = vec.at(n++); + Jerk_Y = vec.at(n++); + Jerk_Z = vec.at(n++); + Lat = vec.at(n++); + Long = vec.at(n++); + Height = vec.at(n++); + Heading = vec.at(n++); + Elevation = vec.at(n++); + Bank = vec.at(n++); + Ang_vel_X = vec.at(n++); + Ang_vel_Y = vec.at(n++); + Ang_vel_Z = vec.at(n++); + Ang_acc_X = vec.at(n++); + Ang_acc_Y = vec.at(n++); + Ang_acc_Z = vec.at(n++); + Ant1_Pos_X = vec.at(n++); + Ant1_Pos_Y = vec.at(n++); + Ant1_Pos_Z = vec.at(n++); + Ant1_Vel_X = vec.at(n++); + Ant1_Vel_Y = vec.at(n++); + Ant1_Vel_Z = vec.at(n++); + Ant1_Acc_X = vec.at(n++); + Ant1_Acc_Y = vec.at(n++); + Ant1_Acc_Z = vec.at(n++); + Ant1_Lat = vec.at(n++); + Ant1_Long = vec.at(n++); + Ant1_Height = vec.at(n++); + Ant1_DOP = vec.at(n++); + return true; + } + catch (const std::exception &ex) + { + return false; + } +} +bool spirent_motion_csv_dump_reader::restart() +{ + if (d_dump_file.is_open()) + { + d_dump_file.clear(); + d_dump_file.seekg(0, std::ios::beg); + std::string line; + for (int n = 0; n < header_lines; n++) + { + getline(d_dump_file, line); + } + return true; + } + else + { + return false; + } +} + + +int64_t spirent_motion_csv_dump_reader::num_epochs() +{ + int64_t nepoch = 0; + std::string line; + std::ifstream tmpfile(d_dump_filename.c_str()); + if (tmpfile.is_open()) + { + while (std::getline(tmpfile, line)) + { + ++nepoch; + } + return nepoch - header_lines; + } + else + { + return 0; + } +} + + +bool spirent_motion_csv_dump_reader::open_obs_file(std::string out_file) +{ + if (d_dump_file.is_open() == false) + { + try + { + d_dump_filename = out_file; + d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(d_dump_filename.c_str()); + std::string line; + for (int n = 0; n < header_lines; n++) + { + getline(d_dump_file, line); + } + return true; + } + catch (const std::ifstream::failure &e) + { + std::cout << "Problem opening Spirent CSV dump Log file: " << d_dump_filename.c_str() << std::endl; + return false; + } + } + else + { + return false; + } +} + +void spirent_motion_csv_dump_reader::close_obs_file() +{ + if (d_dump_file.is_open() == false) + { + d_dump_file.close(); + } +} + +spirent_motion_csv_dump_reader::spirent_motion_csv_dump_reader() +{ + header_lines = 2; +} + + +spirent_motion_csv_dump_reader::~spirent_motion_csv_dump_reader() +{ + if (d_dump_file.is_open() == true) + { + d_dump_file.close(); + } +} diff --git a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h new file mode 100644 index 000000000..c7fe736cf --- /dev/null +++ b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h @@ -0,0 +1,98 @@ +/*! + * \file spirent_motion_csv_dump_reader.h + * \brief Helper file for unit testing + * \author Javier Arribas, 2018. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_spirent_motion_csv_dump_READER_H +#define GNSS_SDR_spirent_motion_csv_dump_READER_H + +#include +#include +#include +#include +#include + +class spirent_motion_csv_dump_reader +{ +public: + spirent_motion_csv_dump_reader(); + ~spirent_motion_csv_dump_reader(); + bool read_csv_obs(); + bool restart(); + int64_t num_epochs(); + bool open_obs_file(std::string out_file); + void close_obs_file(); + + int header_lines; + //dump variables + double TOW_ms; + double Pos_X; + double Pos_Y; + double Pos_Z; + double Vel_X; + double Vel_Y; + double Vel_Z; + double Acc_X; + double Acc_Y; + double Acc_Z; + double Jerk_X; + double Jerk_Y; + double Jerk_Z; + double Lat; + double Long; + double Height; + double Heading; + double Elevation; + double Bank; + double Ang_vel_X; + double Ang_vel_Y; + double Ang_vel_Z; + double Ang_acc_X; + double Ang_acc_Y; + double Ang_acc_Z; + double Ant1_Pos_X; + double Ant1_Pos_Y; + double Ant1_Pos_Z; + double Ant1_Vel_X; + double Ant1_Vel_Y; + double Ant1_Vel_Z; + double Ant1_Acc_X; + double Ant1_Acc_Y; + double Ant1_Acc_Z; + double Ant1_Lat; + double Ant1_Long; + double Ant1_Height; + double Ant1_DOP; + +private: + std::string d_dump_filename; + std::ifstream d_dump_file; + bool parse_vector(std::vector &vec); +}; + +#endif //GNSS_SDR_spirent_motion_csv_dump_READER_H diff --git a/src/tests/system-tests/obs_gps_l1_system_test.cc b/src/tests/system-tests/obs_gps_l1_system_test.cc deleted file mode 100644 index 7af66e15d..000000000 --- a/src/tests/system-tests/obs_gps_l1_system_test.cc +++ /dev/null @@ -1,719 +0,0 @@ -/*! - * \file obs_gps_l1_system_test.cc - * \brief This class implements a test for the validation of generated observables. - * \author Carles Fernandez-Prades, 2016. cfernandez(at)cttc.es - * - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include "concurrent_map.h" -#include "concurrent_queue.h" -#include "control_thread.h" -#include "in_memory_configuration.h" -#include "signal_generator_flags.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -// For GPS NAVIGATION (L1) -concurrent_queue global_gps_acq_assist_queue; -concurrent_map global_gps_acq_assist_map; - - -class ObsGpsL1SystemTest : public ::testing::Test -{ -public: - std::string generator_binary; - std::string p1; - std::string p2; - std::string p3; - std::string p4; - std::string p5; - - const double baseband_sampling_freq = 2.6e6; - - std::string filename_rinex_obs = FLAGS_filename_rinex_obs; - std::string filename_raw_data = FLAGS_filename_raw_data; - std::string generated_rinex_obs; - int configure_generator(); - int generate_signal(); - int configure_receiver(); - int run_receiver(); - void check_results(); - bool check_valid_rinex_nav(std::string filename); // return true if the file is a valid Rinex navigation file. - bool check_valid_rinex_obs(std::string filename); // return true if the file is a valid Rinex observation file. - double compute_stdev(const std::vector& vec); - - std::shared_ptr config; -}; - - -bool ObsGpsL1SystemTest::check_valid_rinex_nav(std::string filename) -{ - bool res = false; - res = gpstk::isRinexNavFile(filename); - return res; -} - - -double ObsGpsL1SystemTest::compute_stdev(const std::vector& vec) -{ - double sum__ = std::accumulate(vec.begin(), vec.end(), 0.0); - double mean__ = sum__ / vec.size(); - double accum__ = 0.0; - std::for_each(std::begin(vec), std::end(vec), [&](const double d) { - accum__ += (d - mean__) * (d - mean__); - }); - double stdev__ = std::sqrt(accum__ / (vec.size() - 1)); - return stdev__; -} - - -bool ObsGpsL1SystemTest::check_valid_rinex_obs(std::string filename) -{ - bool res = false; - res = gpstk::isRinexObsFile(filename); - return res; -} - - -int ObsGpsL1SystemTest::configure_generator() -{ - // Configure signal generator - generator_binary = FLAGS_generator_binary; - - p1 = std::string("-rinex_nav_file=") + FLAGS_rinex_nav_file; - if (FLAGS_dynamic_position.empty()) - { - p2 = std::string("-static_position=") + FLAGS_static_position + std::string(",") + std::to_string(std::min(FLAGS_duration * 10, 3000)); - if (FLAGS_duration > 300) std::cout << "WARNING: Duration has been set to its maximum value of 300 s" << std::endl; - } - else - { - p2 = std::string("-obs_pos_file=") + std::string(FLAGS_dynamic_position); - } - p3 = std::string("-rinex_obs_file=") + FLAGS_filename_rinex_obs; // RINEX 2.10 observation file output - p4 = std::string("-sig_out_file=") + FLAGS_filename_raw_data; // Baseband signal output file. Will be stored in int8_t IQ multiplexed samples - p5 = std::string("-sampling_freq=") + std::to_string(baseband_sampling_freq); //Baseband sampling frequency [MSps] - return 0; -} - - -int ObsGpsL1SystemTest::generate_signal() -{ - pid_t wait_result; - int child_status; - - char* const parmList[] = {&generator_binary[0], &generator_binary[0], &p1[0], &p2[0], &p3[0], &p4[0], &p5[0], NULL}; - - int pid; - if ((pid = fork()) == -1) - perror("fork error"); - else if (pid == 0) - { - execv(&generator_binary[0], parmList); - std::cout << "Return not expected. Must be an execv error." << std::endl; - std::terminate(); - } - - wait_result = waitpid(pid, &child_status, 0); - if (wait_result == -1) perror("waitpid error"); - EXPECT_EQ(true, check_valid_rinex_obs(filename_rinex_obs)); - std::cout << "Signal and Observables RINEX files created." << std::endl; - return 0; -} - - -int ObsGpsL1SystemTest::configure_receiver() -{ - config = std::make_shared(); - - const int sampling_rate_internal = baseband_sampling_freq; - - const int number_of_taps = 11; - const int number_of_bands = 2; - const float band1_begin = 0.0; - const float band1_end = 0.48; - const float band2_begin = 0.52; - const float band2_end = 1.0; - const float ampl1_begin = 1.0; - const float ampl1_end = 1.0; - const float ampl2_begin = 0.0; - const float ampl2_end = 0.0; - const float band1_error = 1.0; - const float band2_error = 1.0; - const int grid_density = 16; - - const float zero = 0.0; - const int number_of_channels = 8; - const int in_acquisition = 1; - - const float threshold = 0.01; - const float doppler_max = 8000.0; - const float doppler_step = 500.0; - const int max_dwells = 1; - const int tong_init_val = 2; - const int tong_max_val = 10; - const int tong_max_dwells = 30; - const int coherent_integration_time_ms = 1; - - const float pll_bw_hz = 30.0; - const float dll_bw_hz = 4.0; - const float early_late_space_chips = 0.5; - const float pll_bw_narrow_hz = 20.0; - const float dll_bw_narrow_hz = 2.0; - const int extend_correlation_ms = 1; - - const int display_rate_ms = 500; - const int output_rate_ms = 10; - - config->set_property("GNSS-SDR.internal_fs_sps", std::to_string(sampling_rate_internal)); - - // Set the assistance system parameters - config->set_property("GNSS-SDR.SUPL_read_gps_assistance_xml", "false"); - config->set_property("GNSS-SDR.SUPL_gps_enabled", "false"); - config->set_property("GNSS-SDR.SUPL_gps_ephemeris_server", "supl.google.com"); - config->set_property("GNSS-SDR.SUPL_gps_ephemeris_port", std::to_string(7275)); - config->set_property("GNSS-SDR.SUPL_gps_acquisition_server", "supl.google.com"); - config->set_property("GNSS-SDR.SUPL_gps_acquisition_port", std::to_string(7275)); - config->set_property("GNSS-SDR.SUPL_MCC", std::to_string(244)); - config->set_property("GNSS-SDR.SUPL_MNS", std::to_string(5)); - config->set_property("GNSS-SDR.SUPL_LAC", "0x59e2"); - config->set_property("GNSS-SDR.SUPL_CI", "0x31b0"); - - // Set the Signal Source - config->set_property("SignalSource.implementation", "File_Signal_Source"); - config->set_property("SignalSource.filename", "./" + filename_raw_data); - config->set_property("SignalSource.sampling_frequency", std::to_string(sampling_rate_internal)); - config->set_property("SignalSource.item_type", "ibyte"); - config->set_property("SignalSource.samples", std::to_string(zero)); - - // Set the Signal Conditioner - config->set_property("SignalConditioner.implementation", "Signal_Conditioner"); - config->set_property("DataTypeAdapter.implementation", "Ibyte_To_Complex"); - config->set_property("InputFilter.implementation", "Fir_Filter"); - config->set_property("InputFilter.dump", "false"); - config->set_property("InputFilter.input_item_type", "gr_complex"); - config->set_property("InputFilter.output_item_type", "gr_complex"); - config->set_property("InputFilter.taps_item_type", "float"); - config->set_property("InputFilter.number_of_taps", std::to_string(number_of_taps)); - config->set_property("InputFilter.number_of_bands", std::to_string(number_of_bands)); - config->set_property("InputFilter.band1_begin", std::to_string(band1_begin)); - config->set_property("InputFilter.band1_end", std::to_string(band1_end)); - config->set_property("InputFilter.band2_begin", std::to_string(band2_begin)); - config->set_property("InputFilter.band2_end", std::to_string(band2_end)); - config->set_property("InputFilter.ampl1_begin", std::to_string(ampl1_begin)); - config->set_property("InputFilter.ampl1_end", std::to_string(ampl1_end)); - config->set_property("InputFilter.ampl2_begin", std::to_string(ampl2_begin)); - config->set_property("InputFilter.ampl2_end", std::to_string(ampl2_end)); - config->set_property("InputFilter.band1_error", std::to_string(band1_error)); - config->set_property("InputFilter.band2_error", std::to_string(band2_error)); - config->set_property("InputFilter.filter_type", "bandpass"); - config->set_property("InputFilter.grid_density", std::to_string(grid_density)); - config->set_property("InputFilter.sampling_frequency", std::to_string(sampling_rate_internal)); - config->set_property("InputFilter.IF", std::to_string(zero)); - config->set_property("Resampler.implementation", "Pass_Through"); - config->set_property("Resampler.dump", "false"); - config->set_property("Resampler.item_type", "gr_complex"); - config->set_property("Resampler.sample_freq_in", std::to_string(sampling_rate_internal)); - config->set_property("Resampler.sample_freq_out", std::to_string(sampling_rate_internal)); - - // Set the number of Channels - config->set_property("Channels_1C.count", std::to_string(number_of_channels)); - config->set_property("Channels.in_acquisition", std::to_string(in_acquisition)); - config->set_property("Channel.signal", "1C"); - - // Set Acquisition - config->set_property("Acquisition_1C.implementation", "GPS_L1_CA_PCPS_Tong_Acquisition"); - config->set_property("Acquisition_1C.item_type", "gr_complex"); - config->set_property("Acquisition_1C.coherent_integration_time_ms", std::to_string(coherent_integration_time_ms)); - config->set_property("Acquisition_1C.threshold", std::to_string(threshold)); - config->set_property("Acquisition_1C.doppler_max", std::to_string(doppler_max)); - config->set_property("Acquisition_1C.doppler_step", std::to_string(doppler_step)); - config->set_property("Acquisition_1C.bit_transition_flag", "false"); - config->set_property("Acquisition_1C.max_dwells", std::to_string(max_dwells)); - config->set_property("Acquisition_1C.tong_init_val", std::to_string(tong_init_val)); - config->set_property("Acquisition_1C.tong_max_val", std::to_string(tong_max_val)); - config->set_property("Acquisition_1C.tong_max_dwells", std::to_string(tong_max_dwells)); - - // Set Tracking - config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_Tracking"); - //config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_C_Aid_Tracking"); - config->set_property("Tracking_1C.item_type", "gr_complex"); - config->set_property("Tracking_1C.dump", "false"); - config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); - config->set_property("Tracking_1C.pll_bw_hz", std::to_string(pll_bw_hz)); - config->set_property("Tracking_1C.dll_bw_hz", std::to_string(dll_bw_hz)); - config->set_property("Tracking_1C.early_late_space_chips", std::to_string(early_late_space_chips)); - - config->set_property("Tracking_1C.pll_bw_narrow_hz", std::to_string(pll_bw_narrow_hz)); - config->set_property("Tracking_1C.dll_bw_narrow_hz", std::to_string(dll_bw_narrow_hz)); - config->set_property("Tracking_1C.extend_correlation_ms", std::to_string(extend_correlation_ms)); - - // Set Telemetry - config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder"); - config->set_property("TelemetryDecoder_1C.dump", "false"); - - // Set Observables - config->set_property("Observables.implementation", "Hybrid_Observables"); - config->set_property("Observables.dump", "false"); - config->set_property("Observables.dump_filename", "./observables.dat"); - config->set_property("Observables.averaging_depth", std::to_string(100)); - - // Set PVT - config->set_property("PVT.implementation", "RTKLIB_PVT"); - config->set_property("PVT.positioning_mode", "Single"); - config->set_property("PVT.output_rate_ms", std::to_string(output_rate_ms)); - config->set_property("PVT.display_rate_ms", std::to_string(display_rate_ms)); - config->set_property("PVT.dump_filename", "./PVT"); - config->set_property("PVT.nmea_dump_filename", "./gnss_sdr_pvt.nmea"); - config->set_property("PVT.flag_nmea_tty_port", "false"); - config->set_property("PVT.nmea_dump_devname", "/dev/pts/4"); - config->set_property("PVT.flag_rtcm_server", "false"); - config->set_property("PVT.flag_rtcm_tty_port", "false"); - config->set_property("PVT.rtcm_dump_devname", "/dev/pts/1"); - config->set_property("PVT.dump", "false"); - config->set_property("PVT.rinex_version", std::to_string(2)); - - return 0; -} - - -int ObsGpsL1SystemTest::run_receiver() -{ - std::shared_ptr control_thread; - control_thread = std::make_shared(config); - // start receiver - try - { - control_thread->run(); - } - catch (const boost::exception& e) - { - std::cout << "Boost exception: " << boost::diagnostic_information(e); - } - catch (const std::exception& ex) - { - std::cout << "STD exception: " << ex.what(); - } - // Get the name of the RINEX obs file generated by the receiver - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - FILE* fp; - std::string argum2 = std::string("/bin/ls *O | grep GSDR | tail -1"); - char buffer[1035]; - fp = popen(&argum2[0], "r"); - if (fp == NULL) - { - std::cout << "Failed to run command: " << argum2 << std::endl; - return -1; - } - while (fgets(buffer, sizeof(buffer), fp) != NULL) - { - std::string aux = std::string(buffer); - ObsGpsL1SystemTest::generated_rinex_obs = aux.erase(aux.length() - 1, 1); - } - pclose(fp); - return 0; -} - - -void ObsGpsL1SystemTest::check_results() -{ - std::vector>> pseudorange_ref(33); - std::vector>> carrierphase_ref(33); - std::vector>> doppler_ref(33); - - std::vector>> pseudorange_meas(33); - std::vector>> carrierphase_meas(33); - std::vector>> doppler_meas(33); - - // Open and read reference RINEX observables file - try - { - gpstk::Rinex3ObsStream r_ref(FLAGS_filename_rinex_obs); - r_ref.exceptions(std::ios::failbit); - gpstk::Rinex3ObsData r_ref_data; - gpstk::Rinex3ObsHeader r_ref_header; - - gpstk::RinexDatum dataobj; - - r_ref >> r_ref_header; - - while (r_ref >> r_ref_data) - { - for (int myprn = 1; myprn < 33; myprn++) - { - gpstk::SatID prn(myprn, gpstk::SatID::systemGPS); - gpstk::CommonTime time = r_ref_data.time; - double sow(static_cast(time).sow); - - gpstk::Rinex3ObsData::DataMap::iterator pointer = r_ref_data.obs.find(prn); - if (pointer == r_ref_data.obs.end()) - { - // PRN not present; do nothing - } - else - { - dataobj = r_ref_data.getObs(prn, "C1C", r_ref_header); - double P1 = dataobj.data; - std::pair pseudo(sow, P1); - pseudorange_ref.at(myprn).push_back(pseudo); - - dataobj = r_ref_data.getObs(prn, "L1C", r_ref_header); - double L1 = dataobj.data; - std::pair carrier(sow, L1); - carrierphase_ref.at(myprn).push_back(carrier); - - dataobj = r_ref_data.getObs(prn, "D1C", r_ref_header); - double D1 = dataobj.data; - std::pair doppler(sow, D1); - doppler_ref.at(myprn).push_back(doppler); - } // End of 'if( pointer == roe.obs.end() )' - } // end for - } // end while - } // End of 'try' block - catch (const gpstk::FFStreamError& e) - { - std::cout << e; - exit(1); - } - catch (const gpstk::Exception& e) - { - std::cout << e; - exit(1); - } - catch (...) - { - std::cout << "unknown error. I don't feel so well..." << std::endl; - exit(1); - } - - try - { - std::string arg2_gen = std::string("./") + ObsGpsL1SystemTest::generated_rinex_obs; - gpstk::Rinex3ObsStream r_meas(arg2_gen); - r_meas.exceptions(std::ios::failbit); - gpstk::Rinex3ObsData r_meas_data; - gpstk::Rinex3ObsHeader r_meas_header; - gpstk::RinexDatum dataobj; - - r_meas >> r_meas_header; - - while (r_meas >> r_meas_data) - { - for (int myprn = 1; myprn < 33; myprn++) - { - gpstk::SatID prn(myprn, gpstk::SatID::systemGPS); - gpstk::CommonTime time = r_meas_data.time; - double sow(static_cast(time).sow); - - gpstk::Rinex3ObsData::DataMap::iterator pointer = r_meas_data.obs.find(prn); - if (pointer == r_meas_data.obs.end()) - { - // PRN not present; do nothing - } - else - { - dataobj = r_meas_data.getObs(prn, "C1C", r_meas_header); - double P1 = dataobj.data; - std::pair pseudo(sow, P1); - pseudorange_meas.at(myprn).push_back(pseudo); - - dataobj = r_meas_data.getObs(prn, "L1C", r_meas_header); - double L1 = dataobj.data; - std::pair carrier(sow, L1); - carrierphase_meas.at(myprn).push_back(carrier); - - dataobj = r_meas_data.getObs(prn, "D1C", r_meas_header); - double D1 = dataobj.data; - std::pair doppler(sow, D1); - doppler_meas.at(myprn).push_back(doppler); - } // End of 'if( pointer == roe.obs.end() )' - } // end for - } // end while - } // End of 'try' block - catch (const gpstk::FFStreamError& e) - { - std::cout << e; - exit(1); - } - catch (const gpstk::Exception& e) - { - std::cout << e; - exit(1); - } - catch (...) - { - std::cout << "unknown error. I don't feel so well..." << std::endl; - exit(1); - } - - // Time alignment - std::vector>> pseudorange_ref_aligned(33); - std::vector>> carrierphase_ref_aligned(33); - std::vector>> doppler_ref_aligned(33); - - std::vector>>::iterator iter; - std::vector>::iterator it; - std::vector>::iterator it2; - - std::vector> pr_diff(33); - std::vector> cp_diff(33); - std::vector> doppler_diff(33); - - std::vector>::iterator iter_diff; - std::vector::iterator iter_v; - - int prn_id = 0; - for (iter = pseudorange_ref.begin(); iter != pseudorange_ref.end(); iter++) - { - for (it = iter->begin(); it != iter->end(); it++) - { - // If a measure exists for this sow, store it - for (it2 = pseudorange_meas.at(prn_id).begin(); it2 != pseudorange_meas.at(prn_id).end(); it2++) - { - if (std::abs(it->first - it2->first) < 0.1) // store measures closer than 10 ms. - { - pseudorange_ref_aligned.at(prn_id).push_back(*it); - pr_diff.at(prn_id).push_back(it->second - it2->second); - //std::cout << "Sat " << prn_id << ": " << "PR_ref=" << it->second << " PR_meas=" << it2->second << " Diff:" << it->second - it2->second << std::endl; - } - } - } - prn_id++; - } - - prn_id = 0; - for (iter = carrierphase_ref.begin(); iter != carrierphase_ref.end(); iter++) - { - for (it = iter->begin(); it != iter->end(); it++) - { - // If a measure exists for this sow, store it - for (it2 = carrierphase_meas.at(prn_id).begin(); it2 != carrierphase_meas.at(prn_id).end(); it2++) - { - if (std::abs(it->first - it2->first) < 0.1) // store measures closer than 10 ms. - { - carrierphase_ref_aligned.at(prn_id).push_back(*it); - cp_diff.at(prn_id).push_back(it->second - it2->second); - // std::cout << "Sat " << prn_id << ": " << "Carrier_ref=" << it->second << " Carrier_meas=" << it2->second << " Diff:" << it->second - it2->second << std::endl; - } - } - } - prn_id++; - } - prn_id = 0; - for (iter = doppler_ref.begin(); iter != doppler_ref.end(); iter++) - { - for (it = iter->begin(); it != iter->end(); it++) - { - // If a measure exists for this sow, store it - for (it2 = doppler_meas.at(prn_id).begin(); it2 != doppler_meas.at(prn_id).end(); it2++) - { - if (std::abs(it->first - it2->first) < 0.01) // store measures closer than 10 ms. - { - doppler_ref_aligned.at(prn_id).push_back(*it); - doppler_diff.at(prn_id).push_back(it->second - it2->second); - } - } - } - prn_id++; - } - - // Compute pseudorange error - prn_id = 0; - std::vector mean_pr_diff_v; - for (iter_diff = pr_diff.begin(); iter_diff != pr_diff.end(); iter_diff++) - { - // For each satellite with reference and measurements aligned in time - int number_obs = 0; - double mean_diff = 0.0; - for (iter_v = iter_diff->begin(); iter_v != iter_diff->end(); iter_v++) - { - mean_diff = mean_diff + *iter_v; - number_obs = number_obs + 1; - } - if (number_obs > 0) - { - mean_diff = mean_diff / number_obs; - mean_pr_diff_v.push_back(mean_diff); - std::cout << "-- Mean pseudorange difference for sat " << prn_id << ": " << mean_diff; - double stdev_ = compute_stdev(*iter_diff); - std::cout << " +/- " << stdev_; - std::cout << " [m]" << std::endl; - } - else - { - mean_diff = 0.0; - } - - prn_id++; - } - double stdev_pr = compute_stdev(mean_pr_diff_v); - std::cout << "Pseudorange diff error stdev = " << stdev_pr << " [m]" << std::endl; - ASSERT_LT(stdev_pr, 10.0); - - // Compute carrier phase error - prn_id = 0; - std::vector mean_cp_diff_v; - for (iter_diff = cp_diff.begin(); iter_diff != cp_diff.end(); iter_diff++) - { - // For each satellite with reference and measurements aligned in time - int number_obs = 0; - double mean_diff = 0.0; - for (iter_v = iter_diff->begin(); iter_v != iter_diff->end(); iter_v++) - { - mean_diff = mean_diff + *iter_v; - number_obs = number_obs + 1; - } - if (number_obs > 0) - { - mean_diff = mean_diff / number_obs; - mean_cp_diff_v.push_back(mean_diff); - std::cout << "-- Mean carrier phase difference for sat " << prn_id << ": " << mean_diff; - double stdev_pr_ = compute_stdev(*iter_diff); - std::cout << " +/- " << stdev_pr_ << " whole cycles (19 cm)" << std::endl; - } - else - { - mean_diff = 0.0; - } - - prn_id++; - } - - // Compute Doppler error - prn_id = 0; - std::vector mean_doppler_v; - for (iter_diff = doppler_diff.begin(); iter_diff != doppler_diff.end(); iter_diff++) - { - // For each satellite with reference and measurements aligned in time - int number_obs = 0; - double mean_diff = 0.0; - for (iter_v = iter_diff->begin(); iter_v != iter_diff->end(); iter_v++) - { - //std::cout << *iter_v << std::endl; - mean_diff = mean_diff + *iter_v; - number_obs = number_obs + 1; - } - if (number_obs > 0) - { - mean_diff = mean_diff / number_obs; - mean_doppler_v.push_back(mean_diff); - std::cout << "-- Mean Doppler difference for sat " << prn_id << ": " << mean_diff << " [Hz]" << std::endl; - } - else - { - mean_diff = 0.0; - } - - prn_id++; - } - - double stdev_dp = compute_stdev(mean_doppler_v); - std::cout << "Doppler error stdev = " << stdev_dp << " [Hz]" << std::endl; - ASSERT_LT(stdev_dp, 10.0); -} - - -TEST_F(ObsGpsL1SystemTest, Observables_system_test) -{ - std::cout << "Validating input RINEX nav file: " << FLAGS_rinex_nav_file << " ..." << std::endl; - bool is_rinex_nav_valid = check_valid_rinex_nav(FLAGS_rinex_nav_file); - EXPECT_EQ(true, is_rinex_nav_valid) << "The RINEX navigation file " << FLAGS_rinex_nav_file << " is not well formed."; - std::cout << "The file is valid." << std::endl; - - // Configure the signal generator - configure_generator(); - - // Generate signal raw signal samples and observations RINEX file - if (!FLAGS_disable_generator) - { - generate_signal(); - } - - std::cout << "Validating generated reference RINEX obs file: " << FLAGS_filename_rinex_obs << " ..." << std::endl; - bool is_gen_rinex_obs_valid = check_valid_rinex_obs("./" + FLAGS_filename_rinex_obs); - EXPECT_EQ(true, is_gen_rinex_obs_valid) << "The RINEX observation file " << FLAGS_filename_rinex_obs << ", generated by gnss-sim, is not well formed."; - std::cout << "The file is valid." << std::endl; - - // Configure receiver - configure_receiver(); - - // Run the receiver - EXPECT_EQ(run_receiver(), 0) << "Problem executing the software-defined signal generator"; - - std::cout << "Validating RINEX obs file obtained by GNSS-SDR: " << ObsGpsL1SystemTest::generated_rinex_obs << " ..." << std::endl; - is_gen_rinex_obs_valid = check_valid_rinex_obs("./" + ObsGpsL1SystemTest::generated_rinex_obs); - EXPECT_EQ(true, is_gen_rinex_obs_valid) << "The RINEX observation file " << ObsGpsL1SystemTest::generated_rinex_obs << ", generated by GNSS-SDR, is not well formed."; - std::cout << "The file is valid." << std::endl; - - // Check results - check_results(); -} - - -int main(int argc, char** argv) -{ - std::cout << "Running Observables validation test..." << std::endl; - int res = 0; - try - { - testing::InitGoogleTest(&argc, argv); - } - catch (...) - { - } // catch the "testing::internal::::ClassUniqueToAlwaysTrue" from gtest - - google::ParseCommandLineFlags(&argc, &argv, true); - google::InitGoogleLogging(argv[0]); - - // Run the Tests - try - { - res = RUN_ALL_TESTS(); - } - catch (...) - { - LOG(WARNING) << "Unexpected catch"; - } - google::ShutDownCommandLineFlags(); - return res; -} diff --git a/src/tests/system-tests/obs_system_test.cc b/src/tests/system-tests/obs_system_test.cc deleted file mode 100644 index 7764d865d..000000000 --- a/src/tests/system-tests/obs_system_test.cc +++ /dev/null @@ -1,1051 +0,0 @@ -/*! - * \file obs_system_test.cc - * \brief This class implements a test for the validation of generated observables. - * \author Carles Fernandez-Prades, 2016. cfernandez(at)cttc.es - * Antonio Ramos, 2017. antonio.ramos(at)cttc.es - * - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include "gnuplot_i.h" -#include "test_flags.h" -#include "concurrent_map.h" -#include "concurrent_queue.h" -#include "control_thread.h" -#include "file_configuration.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -// For GPS NAVIGATION (L1) -concurrent_queue global_gps_acq_assist_queue; -concurrent_map global_gps_acq_assist_map; - -DEFINE_string(configuration_file, "./default_configuration.conf", "Path of configuration file"); -DEFINE_string(filename_rinex_true, "./default_rinex.txt", "Path of RINEX true observations"); -DEFINE_string(filename_rinex_obs, "default_string", "Path of RINEX true observations"); -DEFINE_double(pr_error_mean_max, 25.0, "Maximum mean error in pseudorange"); -DEFINE_double(pr_error_std_max, 15.0, "Maximum standard deviation in pseudorange"); -DEFINE_double(cp_error_mean_max, 5.0, "Maximum mean error in carrier phase"); -DEFINE_double(cp_error_std_max, 2.5, "Maximum standard deviation in carrier phase"); -DEFINE_double(dp_error_mean_max, 75.0, "Maximum mean error in Doppler frequency"); -DEFINE_double(dp_error_std_max, 25.0, "Maximum standard deviation in Doppler frequency"); -DEFINE_bool(plot_obs_sys_test, false, "Plots results of ObsSystemTest with gnuplot"); - -class ObsSystemTest : public ::testing::Test -{ -public: - int configure_receiver(); - int run_receiver(); - void check_results(); - bool check_valid_rinex_obs(std::string filename, int rinex_ver); // return true if the file is a valid Rinex observation file. - void read_rinex_files( - std::vector& pseudorange_ref, - std::vector& carrierphase_ref, - std::vector& doppler_ref, - std::vector& pseudorange_meas, - std::vector& carrierphase_meas, - std::vector& doppler_meas, - arma::mat& sow_prn_ref, - int signal_type); - void time_alignment_diff( - std::vector& ref, - std::vector& meas, - std::vector& diff); - void time_alignment_diff_cp( - std::vector& ref, - std::vector& meas, - std::vector& diff); - void time_alignment_diff_pr( - std::vector& ref, - std::vector& meas, - std::vector& diff, - arma::mat& sow_prn_ref); - void compute_pseudorange_error(std::vector& diff, - double error_th_mean, double error_th_std, - std::string signal_name); - void compute_carrierphase_error( - std::vector& diff, - double error_th_mean, double error_th_std, - std::string signal_name); - void compute_doppler_error( - std::vector& diff, - double error_th_mean, double error_th_std, - std::string signal_name); - std::string filename_rinex_obs = FLAGS_filename_rinex_true; - std::string generated_rinex_obs = FLAGS_filename_rinex_obs; - std::string configuration_file_ = FLAGS_configuration_file; - std::shared_ptr config; - bool gps_1C = false; - bool gps_L5 = false; - bool gal_1B = false; - bool gal_E5a = false; - bool internal_rinex_generation = false; - - /****************/ - const int num_prn_gps = 33; - const int num_prn_gal = 31; - - double pseudorange_error_th_mean = FLAGS_pr_error_mean_max; - double pseudorange_error_th_std = FLAGS_pr_error_std_max; - double carrierphase_error_th_mean = FLAGS_cp_error_mean_max; - double carrierphase_error_th_std = FLAGS_cp_error_std_max; - double doppler_error_th_mean = FLAGS_dp_error_mean_max; - double doppler_error_th_std = FLAGS_dp_error_std_max; -}; - - -bool ObsSystemTest::check_valid_rinex_obs(std::string filename, int rinex_ver) -{ - bool res = false; - if (rinex_ver == 2) - { - res = gpstk::isRinexObsFile(filename); - } - if (rinex_ver == 3) - { - res = gpstk::isRinex3ObsFile(filename); - } - return res; -} - - -void ObsSystemTest::read_rinex_files( - std::vector& pseudorange_ref, - std::vector& carrierphase_ref, - std::vector& doppler_ref, - std::vector& pseudorange_meas, - std::vector& carrierphase_meas, - std::vector& doppler_meas, - arma::mat& sow_prn_ref, - int signal_type) -{ - bool ref_exist = false; - bool meas_exist = false; - gpstk::SatID::SatelliteSystem sat_type = gpstk::SatID::systemUnknown; - int max_prn = 0; - std::string pr_string; - std::string cp_string; - std::string dp_string; - std::string signal_type_string; - sow_prn_ref.reset(); - - switch (signal_type) - { - case 0: //GPS L1 - - sat_type = gpstk::SatID::systemGPS; - max_prn = num_prn_gps; - pr_string = "C1C"; - cp_string = "L1C"; - dp_string = "D1C"; - signal_type_string = "GPS L1 C/A"; - break; - - case 1: //Galileo E1B - - sat_type = gpstk::SatID::systemGalileo; - max_prn = num_prn_gal; - pr_string = "C1B"; - cp_string = "L1B"; - dp_string = "D1B"; - signal_type_string = "Galileo E1B"; - break; - - case 2: //GPS L5 - - sat_type = gpstk::SatID::systemGPS; - max_prn = num_prn_gps; - pr_string = "C5X"; - cp_string = "L5X"; - dp_string = "D5X"; - signal_type_string = "GPS L5"; - break; - - case 3: //Galileo E5a - - sat_type = gpstk::SatID::systemGalileo; - max_prn = num_prn_gal; - pr_string = "C5X"; - cp_string = "L5X"; - dp_string = "D5X"; - signal_type_string = "Galileo E5a"; - break; - } - - // Open and read reference RINEX observables file - std::cout << "Read: RINEX " << signal_type_string << " True" << std::endl; - try - { - gpstk::Rinex3ObsStream r_ref(filename_rinex_obs); - r_ref.exceptions(std::ios::failbit); - gpstk::Rinex3ObsData r_ref_data; - gpstk::Rinex3ObsHeader r_ref_header; - gpstk::RinexDatum dataobj; - r_ref >> r_ref_header; - - while (r_ref >> r_ref_data) - { - for (int myprn = 1; myprn < max_prn; myprn++) - { - gpstk::SatID prn(myprn, sat_type); - gpstk::CommonTime time = r_ref_data.time; - double sow(static_cast(time).sow); - gpstk::Rinex3ObsData::DataMap::iterator pointer = r_ref_data.obs.find(prn); - if (pointer == r_ref_data.obs.end()) - { - // PRN not present; do nothing - } - else - { - dataobj = r_ref_data.getObs(prn, pr_string, r_ref_header); - double P1 = dataobj.data; - pseudorange_ref.at(myprn).insert_rows(pseudorange_ref.at(myprn).n_rows, arma::rowvec({sow, P1})); - - dataobj = r_ref_data.getObs(prn, cp_string, r_ref_header); - double L1 = dataobj.data; - carrierphase_ref.at(myprn).insert_rows(carrierphase_ref.at(myprn).n_rows, arma::rowvec({sow, L1})); - - dataobj = r_ref_data.getObs(prn, dp_string, r_ref_header); - double D1 = dataobj.data; - doppler_ref.at(myprn).insert_rows(doppler_ref.at(myprn).n_rows, arma::rowvec({sow, D1})); - - ref_exist = true; - } // End of 'if( pointer == roe.obs.end() )' - } // end for - } // end while - } // End of 'try' block - catch (const gpstk::FFStreamError& e) - { - std::cout << e; - exit(1); - } - catch (const gpstk::Exception& e) - { - std::cout << e; - exit(1); - } - catch (...) - { - std::cout << "unknown error. I don't feel so well..." << std::endl; - exit(1); - } - - // Open and read measured RINEX observables file - std::cout << "Read: RINEX " << signal_type_string << " measures" << std::endl; - try - { - std::string arg2_gen; - if (internal_rinex_generation) - { - arg2_gen = std::string("./") + generated_rinex_obs; - } - else - { - arg2_gen = generated_rinex_obs; - } - gpstk::Rinex3ObsStream r_meas(arg2_gen); - r_meas.exceptions(std::ios::failbit); - gpstk::Rinex3ObsData r_meas_data; - gpstk::Rinex3ObsHeader r_meas_header; - gpstk::RinexDatum dataobj; - r_meas >> r_meas_header; - - while (r_meas >> r_meas_data) - { - double pr_min = 0.0; - double sow_insert = 0.0; - double prn_min = 0.0; - bool set_pr_min = true; - for (int myprn = 1; myprn < max_prn; myprn++) - { - gpstk::SatID prn(myprn, sat_type); - gpstk::CommonTime time = r_meas_data.time; - double sow(static_cast(time).sow); - gpstk::Rinex3ObsData::DataMap::iterator pointer = r_meas_data.obs.find(prn); - if (pointer == r_meas_data.obs.end()) - { - // PRN not present; do nothing - } - else - { - dataobj = r_meas_data.getObs(prn, pr_string, r_meas_header); - double P1 = dataobj.data; - pseudorange_meas.at(myprn).insert_rows(pseudorange_meas.at(myprn).n_rows, arma::rowvec({sow, P1})); - if (set_pr_min || (P1 < pr_min)) - { - set_pr_min = false; - pr_min = P1; - sow_insert = sow; - prn_min = static_cast(myprn); - } - - dataobj = r_meas_data.getObs(prn, cp_string, r_meas_header); - double L1 = dataobj.data; - carrierphase_meas.at(myprn).insert_rows(carrierphase_meas.at(myprn).n_rows, arma::rowvec({sow, L1})); - - dataobj = r_meas_data.getObs(prn, dp_string, r_meas_header); - double D1 = dataobj.data; - doppler_meas.at(myprn).insert_rows(doppler_meas.at(myprn).n_rows, arma::rowvec({sow, D1})); - - meas_exist = true; - } // End of 'if( pointer == roe.obs.end() )' - } // end for - if (!set_pr_min) - { - sow_prn_ref.insert_rows(sow_prn_ref.n_rows, arma::rowvec({sow_insert, pr_min, prn_min})); - } - } // end while - } // End of 'try' block - catch (const gpstk::FFStreamError& e) - { - std::cout << e; - exit(1); - } - catch (const gpstk::Exception& e) - { - std::cout << e; - exit(1); - } - catch (...) - { - std::cout << "unknown error. I don't feel so well..." << std::endl; - exit(1); - } - EXPECT_TRUE(ref_exist) << "RINEX reference file does not contain " << signal_type_string << " information"; - EXPECT_TRUE(meas_exist) << "RINEX generated file does not contain " << signal_type_string << " information"; -} - - -void ObsSystemTest::time_alignment_diff( - std::vector& ref, - std::vector& meas, - std::vector& diff) -{ - std::vector::iterator iter_ref; - std::vector::iterator iter_meas; - std::vector::iterator iter_diff; - arma::mat mat_aux; - - iter_ref = ref.begin(); - iter_diff = diff.begin(); - for (iter_meas = meas.begin(); iter_meas != meas.end(); iter_meas++) - { - if (!iter_meas->is_empty() && !iter_ref->is_empty()) - { - arma::uvec index_ = arma::find(iter_meas->col(0) > iter_ref->at(0, 0)); - arma::uword index_min = arma::min(index_); - index_ = arma::find(iter_meas->col(0) < iter_ref->at(iter_ref->n_rows - 1, 0)); - arma::uword index_max = arma::max(index_); - mat_aux = iter_meas->rows(index_min, index_max); - arma::vec ref_aligned; - arma::interp1(iter_ref->col(0), iter_ref->col(1), mat_aux.col(0), ref_aligned); - *iter_diff = ref_aligned - mat_aux.col(1); - } - iter_ref++; - iter_diff++; - } -} - - -void ObsSystemTest::time_alignment_diff_cp( - std::vector& ref, - std::vector& meas, - std::vector& diff) -{ - std::vector::iterator iter_ref; - std::vector::iterator iter_meas; - std::vector::iterator iter_diff; - arma::mat mat_aux; - - iter_ref = ref.begin(); - iter_diff = diff.begin(); - for (iter_meas = meas.begin(); iter_meas != meas.end(); iter_meas++) - { - if (!iter_meas->is_empty() && !iter_ref->is_empty()) - { - arma::uvec index_ = arma::find(iter_meas->col(0) > iter_ref->at(0, 0)); - arma::uword index_min = arma::min(index_); - index_ = arma::find(iter_meas->col(0) < iter_ref->at(iter_ref->n_rows - 1, 0)); - arma::uword index_max = arma::max(index_); - mat_aux = iter_meas->rows(index_min, index_max); - mat_aux.col(1) -= mat_aux.col(1)(0); - arma::vec ref_aligned; - arma::interp1(iter_ref->col(0), iter_ref->col(1), mat_aux.col(0), ref_aligned); - ref_aligned -= ref_aligned(0); - *iter_diff = ref_aligned - mat_aux.col(1); - } - iter_ref++; - iter_diff++; - } -} - - -void ObsSystemTest::time_alignment_diff_pr( - std::vector& ref, - std::vector& meas, - std::vector& diff, - arma::mat& sow_prn_ref) -{ - std::vector::iterator iter_ref; - std::vector::iterator iter_meas; - std::vector::iterator iter_diff; - arma::mat mat_aux; - arma::vec subtraction_meas; - arma::vec subtraction_ref; - - arma::mat subtraction_pr_ref = sow_prn_ref; - arma::vec::iterator iter_vec0 = subtraction_pr_ref.begin_col(0); - arma::vec::iterator iter_vec1 = subtraction_pr_ref.begin_col(1); - arma::vec::iterator iter_vec2 = subtraction_pr_ref.begin_col(2); - - for (iter_vec1 = subtraction_pr_ref.begin_col(1); iter_vec1 != subtraction_pr_ref.end_col(1); iter_vec1++) - { - arma::vec aux_pr; //vector with only 1 element - arma::vec aux_sow = {*iter_vec0}; //vector with only 1 element - arma::interp1(ref.at(static_cast(*iter_vec2)).col(0), - ref.at(static_cast(*iter_vec2)).col(1), - aux_sow, - aux_pr); - *iter_vec1 = aux_pr(0); - iter_vec0++; - iter_vec2++; - } - - iter_ref = ref.begin(); - iter_diff = diff.begin(); - for (iter_meas = meas.begin(); iter_meas != meas.end(); iter_meas++) - { - if (!iter_meas->is_empty() && !iter_ref->is_empty()) - { - arma::uvec index_ = arma::find(iter_meas->col(0) > iter_ref->at(0, 0)); - arma::uword index_min = arma::min(index_); - index_ = arma::find(iter_meas->col(0) < iter_ref->at(iter_ref->n_rows - 1, 0)); - arma::uword index_max = arma::max(index_); - mat_aux = iter_meas->rows(index_min, index_max); - arma::interp1(sow_prn_ref.col(0), sow_prn_ref.col(1), mat_aux.col(0), subtraction_meas); - mat_aux.col(1) -= subtraction_meas; - arma::vec ref_aligned; - arma::interp1(iter_ref->col(0), iter_ref->col(1), mat_aux.col(0), ref_aligned); - arma::interp1(subtraction_pr_ref.col(0), subtraction_pr_ref.col(1), mat_aux.col(0), subtraction_ref); - ref_aligned -= subtraction_ref; - *iter_diff = ref_aligned - mat_aux.col(1); - } - iter_ref++; - iter_diff++; - } -} - - -int ObsSystemTest::configure_receiver() -{ - config = std::make_shared(configuration_file_); - - if (config->property("Channels_1C.count", 0) > 0) - { - gps_1C = true; - } - if (config->property("Channels_1B.count", 0) > 0) - { - gal_1B = true; - } - if (config->property("Channels_5X.count", 0) > 0) - { - gal_E5a = true; - } - if (config->property("Channels_7X.count", 0) > 0) //NOT DEFINITIVE!!!!! - { - gps_L5 = true; - } - - return 0; -} - - -int ObsSystemTest::run_receiver() -{ - std::shared_ptr control_thread; - control_thread = std::make_shared(config); - // start receiver - try - { - control_thread->run(); - } - catch (const boost::exception& e) - { - std::cout << "Boost exception: " << boost::diagnostic_information(e); - } - catch (const std::exception& ex) - { - std::cout << "STD exception: " << ex.what(); - } - // Get the name of the RINEX obs file generated by the receiver - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - FILE* fp; - std::string argum2 = std::string("/bin/ls *O | grep GSDR | tail -1"); - char buffer[1035]; - fp = popen(&argum2[0], "r"); - if (fp == NULL) - { - std::cout << "Failed to run command: " << argum2 << std::endl; - return -1; - } - while (fgets(buffer, sizeof(buffer), fp) != NULL) - { - std::string aux = std::string(buffer); - generated_rinex_obs = aux.erase(aux.length() - 1, 1); - internal_rinex_generation = true; - } - pclose(fp); - return 0; -} - - -void ObsSystemTest::compute_pseudorange_error( - std::vector& diff, - double error_th_mean, double error_th_std, - std::string signal_name) -{ - int prn_id = 0; - std::vector::iterator iter_diff; - std::vector means; - std::vector stddevs; - std::vector prns; - for (iter_diff = diff.begin(); iter_diff != diff.end(); iter_diff++) - { - if (!iter_diff->is_empty()) - { - while (iter_diff->has_nan()) - { - bool nan_found = false; - int k_aux = 0; - while (!nan_found) - { - if (!iter_diff->row(k_aux).is_finite()) - { - nan_found = true; - iter_diff->shed_row(k_aux); - } - k_aux++; - } - } - double d_mean = std::sqrt(arma::mean(arma::square(*iter_diff))); - means.push_back(d_mean); - double d_stddev = arma::stddev(*iter_diff); - stddevs.push_back(d_stddev); - prns.push_back(static_cast(prn_id)); - std::cout << "-- RMS pseudorange difference for sat " << prn_id << ": " << d_mean; - std::cout << ". Std. dev.: " << d_stddev; - std::cout << " [m]" << std::endl; - EXPECT_LT(d_mean, error_th_mean); - EXPECT_LT(d_stddev, error_th_std); - } - prn_id++; - } - if (FLAGS_plot_obs_sys_test == true) - { - const std::string gnuplot_executable(FLAGS_gnuplot_executable); - if (gnuplot_executable.empty()) - { - std::cout << "WARNING: Although the flag plot_obs_sys_test has been set to TRUE," << std::endl; - std::cout << "gnuplot has not been found in your system." << std::endl; - std::cout << "Test results will not be plotted." << std::endl; - } - else - { - try - { - boost::filesystem::path p(gnuplot_executable); - boost::filesystem::path dir = p.parent_path(); - std::string gnuplot_path = dir.native(); - Gnuplot::set_GNUPlotPath(gnuplot_path); - - Gnuplot g1("linespoints"); - if (FLAGS_show_plots) - { - g1.showonscreen(); // window output - } - else - { - g1.disablescreen(); - } - g1.set_title(signal_name + " Pseudorange error"); - g1.set_grid(); - g1.set_xlabel("PRN"); - g1.set_ylabel("Pseudorange error [m]"); - g1.plot_xy(prns, means, "RMS error"); - g1.plot_xy(prns, stddevs, "Standard deviation"); - size_t char_pos = signal_name.find(" "); - while (char_pos != std::string::npos) - { - signal_name.replace(char_pos, 1, "_"); - char_pos = signal_name.find(" "); - } - char_pos = signal_name.find("/"); - while (char_pos != std::string::npos) - { - signal_name.replace(char_pos, 1, "_"); - char_pos = signal_name.find("/"); - } - g1.savetops("Pseudorange_error_" + signal_name); - g1.savetopdf("Pseudorange_error_" + signal_name, 18); - } - catch (const GnuplotException& ge) - { - std::cout << ge.what() << std::endl; - } - } - } -} - - -void ObsSystemTest::compute_carrierphase_error( - std::vector& diff, - double error_th_mean, double error_th_std, - std::string signal_name) -{ - int prn_id = 0; - std::vector means; - std::vector stddevs; - std::vector prns; - std::vector::iterator iter_diff; - for (iter_diff = diff.begin(); iter_diff != diff.end(); iter_diff++) - { - if (!iter_diff->is_empty()) - { - while (iter_diff->has_nan()) - { - bool nan_found = false; - int k_aux = 0; - while (!nan_found) - { - if (!iter_diff->row(k_aux).is_finite()) - { - nan_found = true; - iter_diff->shed_row(k_aux); - } - k_aux++; - } - } - double d_mean = std::sqrt(arma::mean(arma::square(*iter_diff))); - means.push_back(d_mean); - double d_stddev = arma::stddev(*iter_diff); - stddevs.push_back(d_stddev); - prns.push_back(static_cast(prn_id)); - std::cout << "-- RMS carrier phase difference for sat " << prn_id << ": " << d_mean; - std::cout << ". Std. dev.: " << d_stddev; - std::cout << " whole cycles" << std::endl; - EXPECT_LT(d_mean, error_th_mean); - EXPECT_LT(d_stddev, error_th_std); - } - prn_id++; - } - if (FLAGS_plot_obs_sys_test == true) - { - const std::string gnuplot_executable(FLAGS_gnuplot_executable); - if (gnuplot_executable.empty()) - { - std::cout << "WARNING: Although the flag plot_obs_sys_test has been set to TRUE," << std::endl; - std::cout << "gnuplot has not been found in your system." << std::endl; - std::cout << "Test results will not be plotted." << std::endl; - } - else - { - try - { - boost::filesystem::path p(gnuplot_executable); - boost::filesystem::path dir = p.parent_path(); - std::string gnuplot_path = dir.native(); - Gnuplot::set_GNUPlotPath(gnuplot_path); - - Gnuplot g1("linespoints"); - if (FLAGS_show_plots) - { - g1.showonscreen(); // window output - } - else - { - g1.disablescreen(); - } - g1.set_title(signal_name + " Carrier phase error"); - g1.set_grid(); - g1.set_xlabel("PRN"); - g1.set_ylabel("Carrier phase error [whole cycles]"); - g1.plot_xy(prns, means, "RMS error"); - g1.plot_xy(prns, stddevs, "Standard deviation"); - size_t char_pos = signal_name.find(" "); - while (char_pos != std::string::npos) - { - signal_name.replace(char_pos, 1, "_"); - char_pos = signal_name.find(" "); - } - char_pos = signal_name.find("/"); - while (char_pos != std::string::npos) - { - signal_name.replace(char_pos, 1, "_"); - char_pos = signal_name.find("/"); - } - g1.savetops("Carrier_phase_error_" + signal_name); - g1.savetopdf("Carrier_phase_error_" + signal_name, 18); - } - catch (const GnuplotException& ge) - { - std::cout << ge.what() << std::endl; - } - } - } -} - - -void ObsSystemTest::compute_doppler_error( - std::vector& diff, - double error_th_mean, double error_th_std, - std::string signal_name) -{ - int prn_id = 0; - std::vector means; - std::vector stddevs; - std::vector prns; - std::vector::iterator iter_diff; - for (iter_diff = diff.begin(); iter_diff != diff.end(); iter_diff++) - { - if (!iter_diff->is_empty()) - { - while (iter_diff->has_nan()) - { - bool nan_found = false; - int k_aux = 0; - while (!nan_found) - { - if (!iter_diff->row(k_aux).is_finite()) - { - nan_found = true; - iter_diff->shed_row(k_aux); - } - k_aux++; - } - } - double d_mean = std::sqrt(arma::mean(arma::square(*iter_diff))); - means.push_back(d_mean); - double d_stddev = arma::stddev(*iter_diff); - stddevs.push_back(d_stddev); - prns.push_back(static_cast(prn_id)); - std::cout << "-- RMS Doppler difference for sat " << prn_id << ": " << d_mean; - std::cout << ". Std. dev.: " << d_stddev; - std::cout << " [Hz]" << std::endl; - EXPECT_LT(d_mean, error_th_mean); - EXPECT_LT(d_stddev, error_th_std); - } - prn_id++; - } - if (FLAGS_plot_obs_sys_test == true) - { - const std::string gnuplot_executable(FLAGS_gnuplot_executable); - if (gnuplot_executable.empty()) - { - std::cout << "WARNING: Although the flag plot_obs_sys_test has been set to TRUE," << std::endl; - std::cout << "gnuplot has not been found in your system." << std::endl; - std::cout << "Test results will not be plotted." << std::endl; - } - else - { - try - { - boost::filesystem::path p(gnuplot_executable); - boost::filesystem::path dir = p.parent_path(); - std::string gnuplot_path = dir.native(); - Gnuplot::set_GNUPlotPath(gnuplot_path); - - Gnuplot g1("linespoints"); - if (FLAGS_show_plots) - { - g1.showonscreen(); // window output - } - else - { - g1.disablescreen(); - } - g1.set_title(signal_name + " Doppler error"); - g1.set_grid(); - g1.set_xlabel("PRN"); - g1.set_ylabel("Doppler error [Hz]"); - g1.plot_xy(prns, means, "RMS error"); - g1.plot_xy(prns, stddevs, "Standard deviation"); - size_t char_pos = signal_name.find(" "); - while (char_pos != std::string::npos) - { - signal_name.replace(char_pos, 1, "_"); - char_pos = signal_name.find(" "); - } - char_pos = signal_name.find("/"); - while (char_pos != std::string::npos) - { - signal_name.replace(char_pos, 1, "_"); - char_pos = signal_name.find("/"); - } - g1.savetops("Doppler_error_" + signal_name); - g1.savetopdf("Doppler_error_" + signal_name, 18); - } - catch (const GnuplotException& ge) - { - std::cout << ge.what() << std::endl; - } - } - } -} - - -void ObsSystemTest::check_results() -{ - arma::mat sow_prn_ref; - if (gps_1C) - { - std::vector pseudorange_ref(num_prn_gps); - std::vector carrierphase_ref(num_prn_gps); - std::vector doppler_ref(num_prn_gps); - - std::vector pseudorange_meas(num_prn_gps); - std::vector carrierphase_meas(num_prn_gps); - std::vector doppler_meas(num_prn_gps); - - read_rinex_files(pseudorange_ref, carrierphase_ref, doppler_ref, pseudorange_meas, carrierphase_meas, doppler_meas, sow_prn_ref, 0); - - // Time alignment and difference computation - - std::vector pr_diff(num_prn_gps); - std::vector cp_diff(num_prn_gps); - std::vector dp_diff(num_prn_gps); - time_alignment_diff_pr(pseudorange_ref, pseudorange_meas, pr_diff, sow_prn_ref); - time_alignment_diff_cp(carrierphase_ref, carrierphase_meas, cp_diff); - time_alignment_diff(doppler_ref, doppler_meas, dp_diff); - - // Results - std::cout << std::endl; - std::cout << std::endl; - std::cout << "GPS L1 C/A obs. results" << std::endl; - - // Compute pseudorange error - - compute_pseudorange_error(pr_diff, pseudorange_error_th_mean, pseudorange_error_th_std, "GPS L1 C/A"); - - // Compute carrier phase error - - compute_carrierphase_error(cp_diff, carrierphase_error_th_mean, carrierphase_error_th_std, "GPS L1 C/A"); - - // Compute Doppler error - - compute_doppler_error(dp_diff, doppler_error_th_mean, doppler_error_th_std, "GPS L1 C/A"); - } - if (gps_L5) - { - std::vector pseudorange_ref(num_prn_gps); - std::vector carrierphase_ref(num_prn_gps); - std::vector doppler_ref(num_prn_gps); - - std::vector pseudorange_meas(num_prn_gps); - std::vector carrierphase_meas(num_prn_gps); - std::vector doppler_meas(num_prn_gps); - - read_rinex_files(pseudorange_ref, carrierphase_ref, doppler_ref, pseudorange_meas, carrierphase_meas, doppler_meas, sow_prn_ref, 2); - - // Time alignment and difference computation - - std::vector pr_diff(num_prn_gps); - std::vector cp_diff(num_prn_gps); - std::vector dp_diff(num_prn_gps); - time_alignment_diff_pr(pseudorange_ref, pseudorange_meas, pr_diff, sow_prn_ref); - time_alignment_diff_cp(carrierphase_ref, carrierphase_meas, cp_diff); - time_alignment_diff(doppler_ref, doppler_meas, dp_diff); - - // Results - std::cout << std::endl; - std::cout << std::endl; - std::cout << "GPS L5 obs. results" << std::endl; - - // Compute pseudorange error - - compute_pseudorange_error(pr_diff, pseudorange_error_th_mean, pseudorange_error_th_std, "GPS L5"); - - // Compute carrier phase error - - compute_carrierphase_error(cp_diff, carrierphase_error_th_mean, carrierphase_error_th_std, "GPS L5"); - - // Compute Doppler error - - compute_doppler_error(dp_diff, doppler_error_th_mean, doppler_error_th_std, "GPS L5"); - } - if (gal_1B) - { - std::vector pseudorange_ref(num_prn_gal); - std::vector carrierphase_ref(num_prn_gal); - std::vector doppler_ref(num_prn_gal); - - std::vector pseudorange_meas(num_prn_gal); - std::vector carrierphase_meas(num_prn_gal); - std::vector doppler_meas(num_prn_gal); - - read_rinex_files(pseudorange_ref, carrierphase_ref, doppler_ref, pseudorange_meas, carrierphase_meas, doppler_meas, sow_prn_ref, 1); - - // Time alignment and difference computation - - std::vector pr_diff(num_prn_gal); - std::vector cp_diff(num_prn_gal); - std::vector dp_diff(num_prn_gal); - time_alignment_diff_pr(pseudorange_ref, pseudorange_meas, pr_diff, sow_prn_ref); - time_alignment_diff_cp(carrierphase_ref, carrierphase_meas, cp_diff); - time_alignment_diff(doppler_ref, doppler_meas, dp_diff); - - // Results - std::cout << std::endl; - std::cout << std::endl; - std::cout << "Galileo E1B obs. results" << std::endl; - - // Compute pseudorange error - - compute_pseudorange_error(pr_diff, pseudorange_error_th_mean, pseudorange_error_th_std, "Galileo E1B"); - - // Compute carrier phase error - - compute_carrierphase_error(cp_diff, carrierphase_error_th_mean, carrierphase_error_th_std, "Galileo E1B"); - - // Compute Doppler error - - compute_doppler_error(dp_diff, doppler_error_th_mean, doppler_error_th_std, "Galileo E1B"); - } - if (gal_E5a) - { - std::vector pseudorange_ref(num_prn_gal); - std::vector carrierphase_ref(num_prn_gal); - std::vector doppler_ref(num_prn_gal); - - std::vector pseudorange_meas(num_prn_gal); - std::vector carrierphase_meas(num_prn_gal); - std::vector doppler_meas(num_prn_gal); - - read_rinex_files(pseudorange_ref, carrierphase_ref, doppler_ref, pseudorange_meas, carrierphase_meas, doppler_meas, sow_prn_ref, 3); - - // Time alignment and difference computation - - std::vector pr_diff(num_prn_gal); - std::vector cp_diff(num_prn_gal); - std::vector dp_diff(num_prn_gal); - time_alignment_diff_pr(pseudorange_ref, pseudorange_meas, pr_diff, sow_prn_ref); - time_alignment_diff_cp(carrierphase_ref, carrierphase_meas, cp_diff); - time_alignment_diff(doppler_ref, doppler_meas, dp_diff); - - // Results - std::cout << std::endl; - std::cout << std::endl; - std::cout << "Galileo E5a obs. results" << std::endl; - - // Compute pseudorange error - - compute_pseudorange_error(pr_diff, pseudorange_error_th_mean, pseudorange_error_th_std, "Galileo E5a"); - - // Compute carrier phase error - - compute_carrierphase_error(cp_diff, carrierphase_error_th_mean, carrierphase_error_th_std, "Galileo E5a"); - - // Compute Doppler error - - compute_doppler_error(dp_diff, doppler_error_th_mean, doppler_error_th_std, "Galileo E5a"); - } -} - - -TEST_F(ObsSystemTest, Observables_system_test) -{ - std::cout << "Validating input RINEX obs (TRUE) file: " << filename_rinex_obs << " ..." << std::endl; - bool is_rinex_obs_valid = check_valid_rinex_obs(filename_rinex_obs, 3); - ASSERT_EQ(true, is_rinex_obs_valid) << "The RINEX observation file " << filename_rinex_obs << " is not well formed. Only RINEX v. 3.00 files are allowed"; - std::cout << "The file is valid." << std::endl; - // Configure receiver - configure_receiver(); - if (generated_rinex_obs.compare("default_string") == 0) - { - // Run the receiver - ASSERT_EQ(run_receiver(), 0) << "Problem executing the software-defined signal generator"; - } - std::cout << "Validating RINEX obs file obtained by GNSS-SDR: " << generated_rinex_obs << " ..." << std::endl; - bool is_gen_rinex_obs_valid = false; - if (internal_rinex_generation) - { - is_gen_rinex_obs_valid = check_valid_rinex_obs("./" + generated_rinex_obs, config->property("PVT.rinex_version", 3)); - } - else - { - is_gen_rinex_obs_valid = check_valid_rinex_obs(generated_rinex_obs, config->property("PVT.rinex_version", 3)); - } - ASSERT_EQ(true, is_gen_rinex_obs_valid) << "The RINEX observation file " << generated_rinex_obs << ", generated by GNSS-SDR, is not well formed."; - std::cout << "The file is valid." << std::endl; - // Check results - check_results(); -} - - -int main(int argc, char** argv) -{ - std::cout << "Running GNSS-SDR in Space Observables validation test..." << std::endl; - int res = 0; - try - { - testing::InitGoogleTest(&argc, argv); - } - catch (...) - { - } // catch the "testing::internal::::ClassUniqueToAlwaysTrue" from gtest - - google::ParseCommandLineFlags(&argc, &argv, true); - google::InitGoogleLogging(argv[0]); - - // Run the Tests - try - { - res = RUN_ALL_TESTS(); - } - catch (...) - { - LOG(WARNING) << "Unexpected catch"; - } - google::ShutDownCommandLineFlags(); - return res; -} diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 63bc18016..5adc6636f 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -1,7 +1,10 @@ /*! * \file position_test.cc * \brief This class implements a test for the validation of computed position. - * \author Carles Fernandez-Prades, 2016. cfernandez(at)cttc.es + * \authors
    + *
  • Carles Fernandez-Prades, 2016. cfernandez(at)cttc.es + *
  • Javier Arribas, 2018. jarribas(at)cttc.es + *
* * * ------------------------------------------------------------------------- @@ -29,6 +32,9 @@ * ------------------------------------------------------------------------- */ +#include "position_test_flags.h" +#include "rtklib_solver_dump_reader.h" +#include "spirent_motion_csv_dump_reader.h" #include "concurrent_map.h" #include "concurrent_queue.h" #include "control_thread.h" @@ -39,6 +45,7 @@ #include "test_flags.h" #include "signal_generator_flags.h" #include +#include #include #include #include @@ -48,10 +55,6 @@ #include #include - -DEFINE_string(config_file_ptest, std::string(""), "File containing the configuration parameters for the position test."); -DEFINE_bool(plot_position_test, false, "Plots results of FFTLengthTest with gnuplot"); - // For GPS NAVIGATION (L1) concurrent_queue global_gps_acq_assist_queue; concurrent_map global_gps_acq_assist_map; @@ -144,9 +147,9 @@ void StaticPositionSystemTest::geodetic2Enu(double latitude, double longitude, d geodetic2Ecef(ref_lat * d2r, ref_long * d2r, ref_h, &ref_x, &ref_y, &ref_z); - double aux_x = x - ref_x; - double aux_y = y - ref_y; - double aux_z = z - ref_z; + double aux_x = x; // - ref_x; + double aux_y = y; // - ref_y; + double aux_z = z; // - ref_z; // ECEF to NED matrix double phiP = atan2(ref_z, sqrt(std::pow(ref_x, 2.0) + std::pow(ref_y, 2.0))); @@ -386,7 +389,7 @@ int StaticPositionSystemTest::configure_receiver() config->set_property("PVT.flag_rtcm_server", "false"); config->set_property("PVT.flag_rtcm_tty_port", "false"); config->set_property("PVT.rtcm_dump_devname", "/dev/pts/1"); - config->set_property("PVT.dump", "false"); + config->set_property("PVT.dump", "true"); config->set_property("PVT.rinex_version", std::to_string(2)); config->set_property("PVT.iono_model", "OFF"); config->set_property("PVT.trop_model", "OFF"); @@ -455,123 +458,354 @@ int StaticPositionSystemTest::run_receiver() void StaticPositionSystemTest::check_results() { - std::fstream myfile(StaticPositionSystemTest::generated_kml_file, std::ios_base::in); - ASSERT_TRUE(myfile.is_open()) << "No valid kml file could be opened"; - std::string line; - std::vector pos_e; std::vector pos_n; std::vector pos_u; - // Skip header - std::getline(myfile, line); - bool is_header = true; - while (is_header) + arma::mat R_eb_e; //ECEF position (x,y,z) estimation in the Earth frame (Nx3) + arma::mat V_eb_e; //ECEF velocity (x,y,z) estimation in the Earth frame (Nx3) + arma::mat LLH; //Geodetic coordinates (latitude, longitude, height) estimation in WGS84 datum + arma::vec receiver_time_s; + + arma::mat ref_R_eb_e; //ECEF position (x,y,z) reference in the Earth frame (Nx3) + arma::mat ref_V_eb_e; //ECEF velocity (x,y,z) reference in the Earth frame (Nx3) + arma::mat ref_LLH; //Geodetic coordinates (latitude, longitude, height) reference in WGS84 datum + arma::vec ref_time_s; + + std::istringstream iss2(FLAGS_static_position); + std::string str_aux; + std::getline(iss2, str_aux, ','); + double ref_lat = std::stod(str_aux); + std::getline(iss2, str_aux, ','); + double ref_long = std::stod(str_aux); + std::getline(iss2, str_aux, '\n'); + double ref_h = std::stod(str_aux); + double ref_e, ref_n, ref_u; + geodetic2Enu(ref_lat, ref_long, ref_h, + &ref_e, &ref_n, &ref_u); + + if (!FLAGS_use_pvt_solver_dump) { + //fall back to read receiver KML output (position only) + std::fstream myfile(StaticPositionSystemTest::generated_kml_file, std::ios_base::in); + ASSERT_TRUE(myfile.is_open()) << "No valid kml file could be opened"; + std::string line; + // Skip header std::getline(myfile, line); - ASSERT_FALSE(myfile.eof()) << "No valid kml file found."; - std::size_t found = line.find(""); - if (found != std::string::npos) is_header = false; - } - bool is_data = true; - - //read data - while (is_data) - { - if (!std::getline(myfile, line)) + bool is_header = true; + while (is_header) { - is_data = false; - break; + std::getline(myfile, line); + ASSERT_FALSE(myfile.eof()) << "No valid kml file found."; + std::size_t found = line.find(""); + if (found != std::string::npos) is_header = false; } - std::size_t found = line.find(""); - if (found != std::string::npos) - is_data = false; - else - { - std::string str2; - std::istringstream iss(line); - double value; - double lat = 0.0; - double longitude = 0.0; - double h = 0.0; - for (int i = 0; i < 3; i++) - { - std::getline(iss, str2, ','); - value = std::stod(str2); - if (i == 0) lat = value; - if (i == 1) longitude = value; - if (i == 2) h = value; - } + bool is_data = true; + //read data + while (is_data) + { + if (!std::getline(myfile, line)) + { + is_data = false; + break; + } + std::size_t found = line.find(""); + if (found != std::string::npos) + is_data = false; + else + { + std::string str2; + std::istringstream iss(line); + double value; + double lat = 0.0; + double longitude = 0.0; + double h = 0.0; + for (int i = 0; i < 3; i++) + { + std::getline(iss, str2, ','); + value = std::stod(str2); + if (i == 0) longitude = value; + if (i == 1) lat = value; + if (i == 2) h = value; + } + + double north, east, up; + geodetic2Enu(lat, longitude, h, &east, &north, &up); + // std::cout << "lat = " << lat << ", longitude = " << longitude << " h = " << h << std::endl; + // std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl; + pos_e.push_back(east); + pos_n.push_back(north); + pos_u.push_back(up); + // getchar(); + } + } + myfile.close(); + ASSERT_FALSE(pos_e.size() == 0) << "KML file is empty"; + } + else + { + //use complete binary dump from pvt solver + rtklib_solver_dump_reader pvt_reader; + pvt_reader.open_obs_file(FLAGS_pvt_solver_dump_filename); + int64_t n_epochs = pvt_reader.num_epochs(); + R_eb_e = arma::zeros(n_epochs, 3); + V_eb_e = arma::zeros(n_epochs, 3); + LLH = arma::zeros(n_epochs, 3); + receiver_time_s = arma::zeros(n_epochs, 1); + int64_t current_epoch = 0; + while (pvt_reader.read_binary_obs()) + { double north, east, up; - geodetic2Enu(lat, longitude, h, &east, &north, &up); - //std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl; + geodetic2Enu(pvt_reader.latitude, pvt_reader.longitude, pvt_reader.height, &east, &north, &up); + // std::cout << "lat = " << pvt_reader.latitude << ", longitude = " << pvt_reader.longitude << " h = " << pvt_reader.height << std::endl; + // std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl; pos_e.push_back(east); pos_n.push_back(north); pos_u.push_back(up); + // getchar(); + + // receiver_time_s(current_epoch) = static_cast(pvt_reader.TOW_at_current_symbol_ms) / 1000.0; + receiver_time_s(current_epoch) = pvt_reader.RX_time - pvt_reader.clk_offset_s; + R_eb_e(current_epoch, 0) = pvt_reader.rr[0]; + R_eb_e(current_epoch, 1) = pvt_reader.rr[1]; + R_eb_e(current_epoch, 2) = pvt_reader.rr[2]; + V_eb_e(current_epoch, 0) = pvt_reader.rr[3]; + V_eb_e(current_epoch, 1) = pvt_reader.rr[4]; + V_eb_e(current_epoch, 2) = pvt_reader.rr[5]; + LLH(current_epoch, 0) = pvt_reader.latitude; + LLH(current_epoch, 1) = pvt_reader.longitude; + LLH(current_epoch, 2) = pvt_reader.height; + + //debug check + // std::cout << "t1: " << pvt_reader.RX_time << std::endl; + // std::cout << "t2: " << pvt_reader.TOW_at_current_symbol_ms << std::endl; + // std::cout << "offset: " << pvt_reader.clk_offset_s << std::endl; + // getchar(); + current_epoch++; + } + ASSERT_FALSE(current_epoch == 0) << "PVT dump is empty"; + } + + // compute results + + if (FLAGS_static_scenario) + { + double sigma_E_2_precision = std::pow(compute_stdev_precision(pos_e), 2.0); + double sigma_N_2_precision = std::pow(compute_stdev_precision(pos_n), 2.0); + double sigma_U_2_precision = std::pow(compute_stdev_precision(pos_u), 2.0); + + double sigma_E_2_accuracy = std::pow(compute_stdev_accuracy(pos_e, ref_e), 2.0); + double sigma_N_2_accuracy = std::pow(compute_stdev_accuracy(pos_n, ref_n), 2.0); + double sigma_U_2_accuracy = std::pow(compute_stdev_accuracy(pos_u, ref_u), 2.0); + + double sum__e = std::accumulate(pos_e.begin(), pos_e.end(), 0.0); + double mean__e = sum__e / pos_e.size(); + double sum__n = std::accumulate(pos_n.begin(), pos_n.end(), 0.0); + double mean__n = sum__n / pos_n.size(); + double sum__u = std::accumulate(pos_u.begin(), pos_u.end(), 0.0); + double mean__u = sum__u / pos_u.size(); + + std::stringstream stm; + std::ofstream position_test_file; + + if (FLAGS_config_file_ptest.empty()) + { + stm << "---- ACCURACY ----" << std::endl; + stm << "2DRMS = " << 2 * sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; + stm << "DRMS = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; + stm << "CEP = " << 0.62 * compute_stdev_accuracy(pos_n, 0.0) + 0.56 * compute_stdev_accuracy(pos_e, 0.0) << " [m]" << std::endl; + stm << "99% SAS = " << 1.122 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; + stm << "90% SAS = " << 0.833 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; + stm << "MRSE = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; + stm << "SEP = " << 0.51 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; + stm << "Bias 2D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0)) << " [m]" << std::endl; + stm << "Bias 3D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0) + std::pow(mean__u, 2.0)) << " [m]" << std::endl; + stm << std::endl; + } + + stm << "---- PRECISION ----" << std::endl; + stm << "2DRMS = " << 2 * sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl; + stm << "DRMS = " << sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl; + stm << "CEP = " << 0.62 * compute_stdev_precision(pos_n) + 0.56 * compute_stdev_precision(pos_e) << " [m]" << std::endl; + stm << "99% SAS = " << 1.122 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; + stm << "90% SAS = " << 0.833 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; + stm << "MRSE = " << sqrt(sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; + stm << "SEP = " << 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; + + std::cout << stm.rdbuf(); + std::string output_filename = "position_test_output_" + StaticPositionSystemTest::generated_kml_file.erase(StaticPositionSystemTest::generated_kml_file.length() - 3, 3) + "txt"; + position_test_file.open(output_filename.c_str()); + if (position_test_file.is_open()) + { + position_test_file << stm.str(); + position_test_file.close(); + } + + // Sanity Check + double precision_SEP = 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision); + ASSERT_LT(precision_SEP, 20.0); + + if (FLAGS_plot_position_test == true) + { + print_results(pos_e, pos_n, pos_u); } } - myfile.close(); - ASSERT_FALSE(pos_e.size() == 0) << "KML file is empty"; - - double sigma_E_2_precision = std::pow(compute_stdev_precision(pos_e), 2.0); - double sigma_N_2_precision = std::pow(compute_stdev_precision(pos_n), 2.0); - double sigma_U_2_precision = std::pow(compute_stdev_precision(pos_u), 2.0); - - double sigma_E_2_accuracy = std::pow(compute_stdev_accuracy(pos_e, 0.0), 2.0); - double sigma_N_2_accuracy = std::pow(compute_stdev_accuracy(pos_n, 0.0), 2.0); - double sigma_U_2_accuracy = std::pow(compute_stdev_accuracy(pos_u, 0.0), 2.0); - - double sum__e = std::accumulate(pos_e.begin(), pos_e.end(), 0.0); - double mean__e = sum__e / pos_e.size(); - double sum__n = std::accumulate(pos_n.begin(), pos_n.end(), 0.0); - double mean__n = sum__n / pos_n.size(); - double sum__u = std::accumulate(pos_u.begin(), pos_u.end(), 0.0); - double mean__u = sum__u / pos_u.size(); - - std::stringstream stm; - std::ofstream position_test_file; - - if (FLAGS_config_file_ptest.empty()) + else { - stm << "---- ACCURACY ----" << std::endl; - stm << "2DRMS = " << 2 * sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; - stm << "DRMS = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; - stm << "CEP = " << 0.62 * compute_stdev_accuracy(pos_n, 0.0) + 0.56 * compute_stdev_accuracy(pos_e, 0.0) << " [m]" << std::endl; - stm << "99% SAS = " << 1.122 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; - stm << "90% SAS = " << 0.833 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; - stm << "MRSE = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; - stm << "SEP = " << 0.51 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; - stm << "Bias 2D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0)) << " [m]" << std::endl; - stm << "Bias 3D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0) + std::pow(mean__u, 2.0)) << " [m]" << std::endl; - stm << std::endl; - } + //dynamic position + spirent_motion_csv_dump_reader ref_reader; + ref_reader.open_obs_file(FLAGS_ref_motion_filename); + int64_t n_epochs = ref_reader.num_epochs(); + ref_R_eb_e = arma::zeros(n_epochs, 3); + ref_V_eb_e = arma::zeros(n_epochs, 3); + ref_LLH = arma::zeros(n_epochs, 3); + ref_time_s = arma::zeros(n_epochs, 1); + int64_t current_epoch = 0; + while (ref_reader.read_csv_obs()) + { + ref_time_s(current_epoch) = ref_reader.TOW_ms / 1000.0; + ref_R_eb_e(current_epoch, 0) = ref_reader.Pos_X; + ref_R_eb_e(current_epoch, 1) = ref_reader.Pos_Y; + ref_R_eb_e(current_epoch, 2) = ref_reader.Pos_Z; + ref_V_eb_e(current_epoch, 0) = ref_reader.Vel_X; + ref_V_eb_e(current_epoch, 1) = ref_reader.Vel_Y; + ref_V_eb_e(current_epoch, 2) = ref_reader.Vel_Z; + ref_LLH(current_epoch, 0) = ref_reader.Lat; + ref_LLH(current_epoch, 1) = ref_reader.Long; + ref_LLH(current_epoch, 2) = ref_reader.Height; + current_epoch++; + } - stm << "---- PRECISION ----" << std::endl; - stm << "2DRMS = " << 2 * sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl; - stm << "DRMS = " << sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl; - stm << "CEP = " << 0.62 * compute_stdev_precision(pos_n) + 0.56 * compute_stdev_precision(pos_e) << " [m]" << std::endl; - stm << "99% SAS = " << 1.122 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; - stm << "90% SAS = " << 0.833 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; - stm << "MRSE = " << sqrt(sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; - stm << "SEP = " << 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; + //interpolation of reference data to receiver epochs timestamps + arma::mat ref_interp_R_eb_e = arma::zeros(R_eb_e.n_rows, 3); + arma::mat ref_interp_V_eb_e = arma::zeros(V_eb_e.n_rows, 3); + arma::mat ref_interp_LLH = arma::zeros(LLH.n_rows, 3); + arma::vec tmp_vector; + for (int n = 0; n < 3; n++) + { + arma::interp1(ref_time_s, ref_R_eb_e.col(n), receiver_time_s, tmp_vector); + ref_interp_R_eb_e.col(n) = tmp_vector; + arma::interp1(ref_time_s, ref_V_eb_e.col(n), receiver_time_s, tmp_vector); + ref_interp_V_eb_e.col(n) = tmp_vector; + arma::interp1(ref_time_s, ref_LLH.col(n), receiver_time_s, tmp_vector); + ref_interp_LLH.col(n) = tmp_vector; + } - std::cout << stm.rdbuf(); - std::string output_filename = "position_test_output_" + StaticPositionSystemTest::generated_kml_file.erase(StaticPositionSystemTest::generated_kml_file.length() - 3, 3) + "txt"; - position_test_file.open(output_filename.c_str()); - if (position_test_file.is_open()) - { - position_test_file << stm.str(); - position_test_file.close(); - } + //compute error vectors - // Sanity Check - double precision_SEP = 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision); - ASSERT_LT(precision_SEP, 20.0); + arma::mat error_R_eb_e = arma::zeros(R_eb_e.n_rows, 3); + arma::mat error_V_eb_e = arma::zeros(V_eb_e.n_rows, 3); + arma::mat error_LLH = arma::zeros(LLH.n_rows, 3); + error_R_eb_e = R_eb_e - ref_interp_R_eb_e; + error_V_eb_e = V_eb_e - ref_interp_V_eb_e; + error_LLH = LLH - ref_interp_LLH; + arma::vec error_module_R_eb_e = arma::zeros(R_eb_e.n_rows, 1); + arma::vec error_module_V_eb_e = arma::zeros(V_eb_e.n_rows, 1); + for (uint64_t n = 0; n < R_eb_e.n_rows; n++) + { + error_module_R_eb_e(n) = arma::norm(error_R_eb_e.row(n)); + error_module_V_eb_e(n) = arma::norm(error_V_eb_e.row(n)); + } + //Error statistics + arma::vec tmp_vec; + //RMSE, Mean, Variance and peaks + tmp_vec = arma::square(error_module_R_eb_e); + double rmse_R_eb_e = sqrt(arma::mean(tmp_vec)); + double error_mean_R_eb_e = arma::mean(error_module_R_eb_e); + double error_var_R_eb_e = arma::var(error_module_R_eb_e); + double max_error_R_eb_e = arma::max(error_module_R_eb_e); + double min_error_R_eb_e = arma::min(error_module_R_eb_e); - if (FLAGS_plot_position_test == true) - { - print_results(pos_e, pos_n, pos_u); + tmp_vec = arma::square(error_module_V_eb_e); + double rmse_V_eb_e = sqrt(arma::mean(tmp_vec)); + double error_mean_V_eb_e = arma::mean(error_module_V_eb_e); + double error_var_V_eb_e = arma::var(error_module_V_eb_e); + double max_error_V_eb_e = arma::max(error_module_V_eb_e); + double min_error_V_eb_e = arma::min(error_module_V_eb_e); + + //report + std::cout << "----- Position and Velocity 3D ECEF error statistics -----" << std::endl; + std::streamsize ss = std::cout.precision(); + std::cout << std::setprecision(10) << "---- 3D ECEF Position RMSE = " + << rmse_R_eb_e << ", mean = " << error_mean_R_eb_e + << ", stdev = " << sqrt(error_var_R_eb_e) + << " (max,min) = " << max_error_R_eb_e + << "," << min_error_R_eb_e + << " [m]" << std::endl; + std::cout << "---- 3D ECEF Velocity RMSE = " + << rmse_V_eb_e << ", mean = " << error_mean_V_eb_e + << ", stdev = " << sqrt(error_var_V_eb_e) + << " (max,min) = " << max_error_V_eb_e + << "," << min_error_V_eb_e + << " [m/s]" << std::endl; + std::cout.precision(ss); + + //plots + Gnuplot g1("points"); + if (FLAGS_show_plots) + { + g1.showonscreen(); // window output + } + else + { + g1.disablescreen(); + } + g1.set_title("3D ECEF error coordinates"); + g1.set_grid(); + //conversion between arma::vec and std:vector + std::vector X(error_R_eb_e.colptr(0), error_R_eb_e.colptr(0) + error_R_eb_e.n_rows); + std::vector Y(error_R_eb_e.colptr(1), error_R_eb_e.colptr(1) + error_R_eb_e.n_rows); + std::vector Z(error_R_eb_e.colptr(2), error_R_eb_e.colptr(2) + error_R_eb_e.n_rows); + + g1.cmd("set key box opaque"); + g1.plot_xyz(X, Y, Z, "ECEF_3d_error"); + g1.set_legend(); + g1.savetops("ECEF_3d_error"); + + + arma::vec time_vector_from_start_s = receiver_time_s - receiver_time_s(0); + Gnuplot g3("linespoints"); + if (FLAGS_show_plots) + { + g3.showonscreen(); // window output + } + else + { + g3.disablescreen(); + } + g3.set_title("3D Position estimation error module [m]"); + g3.set_grid(); + g3.set_xlabel("Receiver epoch time from first valid PVT [s]"); + g3.set_ylabel("3D Position error [m]"); + //conversion between arma::vec and std:vector + std::vector error_vec(error_module_R_eb_e.colptr(0), error_module_R_eb_e.colptr(0) + error_module_R_eb_e.n_rows); + g3.cmd("set key box opaque"); + g3.plot_xy(time_vector_from_start_s, error_vec, + "Position_3d_error"); + g3.set_legend(); + g3.savetops("Position_3d_error"); + + Gnuplot g4("linespoints"); + if (FLAGS_show_plots) + { + g4.showonscreen(); // window output + } + else + { + g4.disablescreen(); + } + g4.set_title("3D Velocity estimation error module [m/s]"); + g4.set_grid(); + g4.set_xlabel("Receiver epoch time from first valid PVT [s]"); + g4.set_ylabel("3D Velocity error [m/s]"); + //conversion between arma::vec and std:vector + std::vector error_vec2(error_module_V_eb_e.colptr(0), error_module_V_eb_e.colptr(0) + error_module_V_eb_e.n_rows); + g4.cmd("set key box opaque"); + g4.plot_xy(time_vector_from_start_s, error_vec2, + "Velocity_3d_error"); + g4.set_legend(); + g4.savetops("Velocity_3d_error"); } } @@ -698,7 +932,7 @@ TEST_F(StaticPositionSystemTest, Position_system_test) configure_receiver(); // Run the receiver - EXPECT_EQ(run_receiver(), 0) << "Problem executing the software-defined signal generator"; + EXPECT_EQ(run_receiver(), 0) << "Problem executing GNSS-SDR"; // Check results check_results(); diff --git a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc index 7e086ce6b..961757087 100644 --- a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc @@ -1632,7 +1632,6 @@ TEST_F(HybridObservablesTest, ValidationOfResults) //Do not compare E5a with E5 RINEX due to the Doppler frequency discrepancy caused by the different center frequencies //E5a_fc=1176.45e6, E5b_fc=1207.14e6, E5_fc=1191.795e6; - std::cout << "s:" << gnss_synchro_vec.at(n).Signal << std::endl; if (strcmp("5X\0", gnss_synchro_vec.at(n).Signal) != 0 or FLAGS_compare_with_5X) { check_results_carrier_phase_double_diff(true_obs_vec.at(n),