/*! * \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 * * * ------------------------------------------------------------------------- * * Copyright (C) 2010-2017 (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 #include #include #include #include #include #include #include "concurrent_map.h" #include "concurrent_queue.h" #include "control_thread.h" #include "in_memory_configuration.h" #include "file_configuration.h" #include "MATH_CONSTANTS.h" #include "signal_generator_flags.h" DEFINE_string(config_file_ptest, std::string(""), "File containing the configuration parameters for the position test."); // For GPS NAVIGATION (L1) concurrent_queue global_gps_acq_assist_queue; concurrent_map global_gps_acq_assist_map; class Static_Position_System_Test: 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 = static_cast(FLAGS_fs_gen_hz); std::string filename_rinex_obs = FLAGS_filename_rinex_obs; std::string filename_raw_data = FLAGS_filename_raw_data; int configure_generator(); int generate_signal(); int configure_receiver(); int run_receiver(); void check_results(); double compute_stdev_precision(const std::vector & vec); double compute_stdev_accuracy(const std::vector & vec, double ref); void geodetic2Enu(const double latitude, const double longitude, const double altitude, double* east, double* north, double* up); std::shared_ptr config; std::shared_ptr config_f; std::string generated_kml_file; private: void geodetic2Ecef(const double latitude, const double longitude, const double altitude, double* x, double* y, double* z); }; void Static_Position_System_Test::geodetic2Ecef(const double latitude, const double longitude, const double altitude, double* x, double* y, double* z) { const double a = 6378137.0; // WGS84 const double b = 6356752.314245; // WGS84 double aux_x, aux_y, aux_z; // Convert to ECEF (See https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates ) const double cLat = cos(latitude); const double cLon = cos(longitude); const double sLon = sin(longitude); const double sLat = sin(latitude); double N = std::pow(a, 2.0) / sqrt(std::pow(a, 2.0) * std::pow(cLat, 2.0) + std::pow(b, 2.0) * std::pow(sLat, 2.0)); aux_x = (N + altitude) * cLat * cLon; aux_y = (N + altitude) * cLat * sLon; aux_z = ((std::pow(b, 2.0) / std::pow(a, 2.0)) * N + altitude) * sLat; *x = aux_x; *y = aux_y; *z = aux_z; } void Static_Position_System_Test::geodetic2Enu(const double latitude, const double longitude, const double altitude, double* east, double* north, double* up) { double x, y, z; const double d2r = PI / 180.0; geodetic2Ecef(latitude * d2r, longitude * d2r, altitude, &x, &y, &z); double aux_north, aux_east, aux_down; std::istringstream iss2(FLAGS_static_position); std::string str_aux; std::getline(iss2, str_aux, ','); double ref_long = std::stod(str_aux); std::getline(iss2, str_aux, ','); double ref_lat = std::stod(str_aux); std::getline(iss2, str_aux, '\n'); double ref_h = std::stod(str_aux); double ref_x, ref_y, ref_z; 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; // ECEF to NED matrix double phiP = atan2(ref_z, sqrt(std::pow(ref_x, 2.0) + std::pow(ref_y, 2.0))); const double sLat = sin(phiP); const double sLon = sin(ref_long * d2r); const double cLat = cos(phiP); const double cLon = cos(ref_long * d2r); aux_north = -aux_x * sLat * cLon - aux_y * sLon + aux_z * cLat * cLon; aux_east = -aux_x * sLat * sLon + aux_y * cLon + aux_z * cLat * sLon; aux_down = aux_x * cLat + aux_z * sLat; *east = aux_east; *north = aux_north; *up = -aux_down; } double Static_Position_System_Test::compute_stdev_precision(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__; } double Static_Position_System_Test::compute_stdev_accuracy(const std::vector & vec, const double ref) { const double mean__ = ref; 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__; } int Static_Position_System_Test::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 Static_Position_System_Test::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"); return 0; } int Static_Position_System_Test::configure_receiver() { if(FLAGS_config_file_ptest.empty()) { 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 int decimation_factor = 1; 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 = 1000; const int output_rate_ms = 1000; config->set_property("GNSS-SDR.internal_fs_hz", 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.if", std::to_string(zero)); 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.if", std::to_string(zero)); 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"); config->set_property("TelemetryDecoder_1C.decimation_factor", std::to_string(decimation_factor)); // Set Observables config->set_property("Observables.implementation", "Hybrid_Observables"); config->set_property("Observables.dump", "false"); config->set_property("Observables.dump_filename", "./observables.dat"); // Set PVT config->set_property("PVT.implementation", "RTKLIB_PVT"); 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)); config->set_property("PVT.positioning_mode", "PPP_Static"); config->set_property("PVT.iono_model", "OFF"); config->set_property("PVT.trop_model", "OFF"); config->set_property("PVT.AR_GPS", "PPP-AR"); config_f = 0; } else { config_f = std::make_shared(FLAGS_config_file_ptest); config = 0; } return 0; } int Static_Position_System_Test::run_receiver() { std::shared_ptr control_thread; if(FLAGS_config_file_ptest.empty()) { control_thread = std::make_shared(config); } else { control_thread = std::make_shared(config_f); } // start receiver try { control_thread->run(); } catch( boost::exception & e ) { std::cout << "Boost exception: " << boost::diagnostic_information(e); } catch(std::exception const& ex) { std::cout << "STD exception: " << ex.what(); } // Get the name of the KML file generated by the receiver std::this_thread::sleep_for(std::chrono::milliseconds(2000)); FILE *fp; std::string argum2 = std::string("/bin/ls *kml | 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); EXPECT_EQ(aux.empty(), false); Static_Position_System_Test::generated_kml_file = aux.erase(aux.length() - 1, 1); } pclose(fp); EXPECT_EQ(Static_Position_System_Test::generated_kml_file.empty(), false); return 0; } void Static_Position_System_Test::check_results() { std::fstream myfile(Static_Position_System_Test::generated_kml_file, std::ios_base::in); 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) { std::getline(myfile, line); std::size_t found = line.find(""); if (found != std::string::npos) is_header = false; } bool is_data = true; //read data while(is_data) { std::getline(myfile, line); 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, longitude, h; 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; } double north, east, up; geodetic2Enu(lat, longitude, h, &east, &north, &up); //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); } } myfile.close(); 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(); if(FLAGS_config_file_ptest.empty()) { std::cout << "---- ACCURACY ----" << std::endl; std::cout << "2DRMS = " << 2 * sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; std::cout << "DRMS = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; std::cout << "CEP = " << 0.62 * compute_stdev_accuracy(pos_n, 0.0) + 0.56 * compute_stdev_accuracy(pos_e, 0.0) << " [m]" << std::endl; std::cout << "99% SAS = " << 1.122 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; std::cout << "90% SAS = " << 0.833 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; std::cout << "MRSE = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; std::cout << "SEP = " << 0.51 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; std::cout << "Bias 2D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0)) << " [m]" << std::endl; std::cout << "Bias 3D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0) + std::pow(mean__u, 2.0)) << " [m]" << std::endl; std::cout << std::endl; } std::cout << "---- PRECISION ----" << std::endl; std::cout << "2DRMS = " << 2 * sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl; std::cout << "DRMS = " << sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl; std::cout << "CEP = " << 0.62 * compute_stdev_precision(pos_n) + 0.56 * compute_stdev_precision(pos_e) << " [m]" << std::endl; std::cout << "99% SAS = " << 1.122 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; std::cout << "90% SAS = " << 0.833 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; std::cout << "MRSE = " << sqrt(sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; std::cout << "SEP = " << 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; // 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); } TEST_F(Static_Position_System_Test, Position_system_test) { if(FLAGS_config_file_ptest.empty()) { // Configure the signal generator configure_generator(); // Generate signal raw signal samples and observations RINEX file if(!FLAGS_disable_generator) { generate_signal(); } } // Configure receiver configure_receiver(); // Run the receiver EXPECT_EQ( run_receiver(), 0) << "Problem executing the software-defined signal generator"; // Check results check_results(); } int main(int argc, char **argv) { std::cout << "Running Position precision 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; }