diff --git a/src/core/libs/gnss_sdr_supl_client.cc b/src/core/libs/gnss_sdr_supl_client.cc index 4d3668adc..2e43e9708 100644 --- a/src/core/libs/gnss_sdr_supl_client.cc +++ b/src/core/libs/gnss_sdr_supl_client.cc @@ -388,6 +388,46 @@ bool gnss_sdr_supl_client::load_ephemeris_xml(const std::string file_name) } +bool gnss_sdr_supl_client::load_gal_ephemeris_xml(const std::string file_name) +{ + std::ifstream ifs; + try + { + ifs.open(file_name.c_str(), std::ifstream::binary | std::ifstream::in); + boost::archive::xml_iarchive xml(ifs); + gal_ephemeris_map.clear(); + xml >> boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", this->gal_ephemeris_map); + LOG(INFO) << "Loaded Ephemeris map data with " << this->gal_ephemeris_map.size() << " satellites"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what() << "File: " << file_name; + return false; + } + return true; +} + + +bool gnss_sdr_supl_client::load_cnav_ephemeris_xml(const std::string file_name) +{ + std::ifstream ifs; + try + { + ifs.open(file_name.c_str(), std::ifstream::binary | std::ifstream::in); + boost::archive::xml_iarchive xml(ifs); + gps_cnav_ephemeris_map.clear(); + xml >> boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", this->gps_cnav_ephemeris_map); + LOG(INFO) << "Loaded Ephemeris map data with " << this->gps_cnav_ephemeris_map.size() << " satellites"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what() << "File: " << file_name; + return false; + } + return true; +} + + bool gnss_sdr_supl_client::save_ephemeris_map_xml(const std::string file_name, std::map eph_map) { if (eph_map.empty() == false) diff --git a/src/core/libs/gnss_sdr_supl_client.h b/src/core/libs/gnss_sdr_supl_client.h index 381045edb..eaa594390 100644 --- a/src/core/libs/gnss_sdr_supl_client.h +++ b/src/core/libs/gnss_sdr_supl_client.h @@ -46,6 +46,8 @@ extern "C" #include "gps_acq_assist.h" #include "gps_ref_time.h" #include "gps_ref_location.h" +#include "gps_cnav_ephemeris.h" +#include "galileo_ephemeris.h" #include #include #include @@ -77,6 +79,8 @@ public: int request; // ephemeris map std::map gps_ephemeris_map; + std::map gal_ephemeris_map; + std::map gps_cnav_ephemeris_map; // almanac map std::map gps_almanac_map; @@ -107,10 +111,20 @@ public: void read_supl_data(); /*! - * \brief Read ephemeris map from XML file + * \brief Read GPS NAV ephemeris map from XML file */ bool load_ephemeris_xml(const std::string file_name); + /*! + * \brief Read GPS CNAV ephemeris map from XML file + */ + bool load_cnav_ephemeris_xml(const std::string file_name); + + /*! + * \brief Read Galileo ephemeris map from XML file + */ + bool load_gal_ephemeris_xml(const std::string file_name); + /*! * \brief Save ephemeris map to XML file. */ diff --git a/src/core/receiver/control_thread.cc b/src/core/receiver/control_thread.cc index 8eb49563c..d92a846b1 100644 --- a/src/core/receiver/control_thread.cc +++ b/src/core/receiver/control_thread.cc @@ -203,8 +203,15 @@ bool ControlThread::read_assistance_from_XML() std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename); std::string ref_time_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_time_xml", ref_time_default_xml_filename); std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename); + std::string eph_gal_xml_filename = configuration_->property("GNSS-SDR.SUPL_gal_ephemeris_xml", eph_gal_default_xml_filename); + std::string eph_cnav_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_cnav_ephemeris_xml", eph_cnav_default_xml_filename); + + std::cout << "Trying to read GNSS ephemeris from XML file(s): " + << ((eph_xml_filename.compare(eph_default_xml_filename) != 0) ? eph_xml_filename + " " : "") + << ((eph_gal_xml_filename.compare(eph_gal_default_xml_filename) != 0) ? eph_gal_xml_filename + " " : "") + << ((eph_cnav_xml_filename.compare(eph_cnav_default_xml_filename) != 0) ? eph_gal_xml_filename : "") + << std::endl; - std::cout << "SUPL: Try read GPS ephemeris from XML file " << eph_xml_filename << std::endl; if (supl_client_ephemeris_.load_ephemeris_xml(eph_xml_filename) == true) { std::map::const_iterator gps_eph_iter; @@ -212,13 +219,41 @@ bool ControlThread::read_assistance_from_XML() gps_eph_iter != supl_client_ephemeris_.gps_ephemeris_map.cend(); gps_eph_iter++) { - std::cout << "SUPL: Read XML Ephemeris for GPS SV " << gps_eph_iter->first << std::endl; + std::cout << "From XML file: Read NAV ephemeris for satellite " << Gnss_Satellite("GPS", gps_eph_iter->first) << std::endl; std::shared_ptr tmp_obj = std::make_shared(gps_eph_iter->second); flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); } ret = true; } - else + + if (supl_client_ephemeris_.load_gal_ephemeris_xml(eph_gal_xml_filename) == true) + { + std::map::const_iterator gal_eph_iter; + for (gal_eph_iter = supl_client_ephemeris_.gal_ephemeris_map.cbegin(); + gal_eph_iter != supl_client_ephemeris_.gal_ephemeris_map.cend(); + gal_eph_iter++) + { + std::cout << "From XML file: Read ephemeris for satellite " << Gnss_Satellite("Galileo", gal_eph_iter->first) << std::endl; + std::shared_ptr tmp_obj = std::make_shared(gal_eph_iter->second); + flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); + } + ret = true; + } + + if (supl_client_ephemeris_.load_cnav_ephemeris_xml(eph_cnav_xml_filename) == true) + { + std::map::const_iterator gps_cnav_eph_iter; + for (gps_cnav_eph_iter = supl_client_ephemeris_.gps_cnav_ephemeris_map.cbegin(); + gps_cnav_eph_iter != supl_client_ephemeris_.gps_cnav_ephemeris_map.cend(); + gps_cnav_eph_iter++) + { + std::cout << "From XML file: Read CNAV ephemeris for satellite " << Gnss_Satellite("GPS", gps_cnav_eph_iter->first) << std::endl; + std::shared_ptr tmp_obj = std::make_shared(gps_cnav_eph_iter->second); + flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj)); + } + ret = true; + } + if (ret == false) { std::cout << "ERROR: SUPL client error reading XML" << std::endl; std::cout << "Disabling SUPL assistance..." << std::endl; @@ -324,7 +359,7 @@ void ControlThread::assist_GNSS() // read assistance from file if (read_assistance_from_XML()) { - std::cout << "GPS assistance data loaded from local XML file." << std::endl; + std::cout << "GNSS assistance data loaded from local XML file(s)." << std::endl; } } else diff --git a/src/core/receiver/control_thread.h b/src/core/receiver/control_thread.h index c49ddba53..d4f1dccf3 100644 --- a/src/core/receiver/control_thread.h +++ b/src/core/receiver/control_thread.h @@ -167,6 +167,8 @@ private: const std::string iono_default_xml_filename = "./gps_iono.xml"; const std::string ref_time_default_xml_filename = "./gps_ref_time.xml"; const std::string ref_location_default_xml_filename = "./gps_ref_location.xml"; + const std::string eph_gal_default_xml_filename = "./gal_ephemeris.xml"; + const std::string eph_cnav_default_xml_filename = "./gps_cnav_ephemeris.xml"; }; #endif /*GNSS_SDR_CONTROL_THREAD_H_*/ diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index a306b30f1..d6a46f3db 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -56,6 +56,7 @@ #include #include #include +#include // For GPS NAVIGATION (L1) concurrent_queue global_gps_acq_assist_queue; @@ -69,6 +70,8 @@ public: int configure_receiver(); int run_receiver(); void check_results(); + bool save_mat_xy(std::vector& x, std::vector& y, std::string filename); + bool save_mat_x(std::vector& x, std::string filename); std::string config_filename_no_extension; private: @@ -356,6 +359,72 @@ int PositionSystemTest::run_receiver() } +bool PositionSystemTest::save_mat_xy(std::vector& x, std::vector& y, std::string filename) +{ + try + { + // WRITE MAT FILE + mat_t* matfp; + matvar_t* matvar; + filename.append(".mat"); + std::cout << "save_mat_xy write " << filename << std::endl; + matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT5); + if (reinterpret_cast(matfp) != NULL) + { + size_t dims[2] = {1, x.size()}; + matvar = Mat_VarCreate("x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, &x[0], 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, &y[0], 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + else + { + std::cout << "save_mat_xy: error creating file" << std::endl; + } + Mat_Close(matfp); + return true; + } + catch (const std::exception& ex) + { + std::cout << "save_mat_xy: " << ex.what() << std::endl; + return false; + } +} + +bool PositionSystemTest::save_mat_x(std::vector& x, std::string filename) +{ + try + { + // WRITE MAT FILE + mat_t* matfp; + matvar_t* matvar; + filename.append(".mat"); + std::cout << "save_mat_x write " << filename << std::endl; + matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT5); + if (reinterpret_cast(matfp) != NULL) + { + size_t dims[2] = {1, x.size()}; + matvar = Mat_VarCreate("x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, &x[0], 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + else + { + std::cout << "save_mat_x: error creating file" << std::endl; + } + Mat_Close(matfp); + return true; + } + catch (const std::exception& ex) + { + std::cout << "save_mat_x: " << ex.what() << std::endl; + return false; + } +} + void PositionSystemTest::check_results() { arma::mat R_eb_e; //ECEF position (x,y,z) estimation in the Earth frame (Nx3) @@ -493,17 +562,36 @@ void PositionSystemTest::check_results() double sigma_N_2_precision = arma::var(R_eb_enu.row(1)); double sigma_U_2_precision = arma::var(R_eb_enu.row(2)); - arma::rowvec tmp_vec; - tmp_vec = R_eb_enu.row(0) - ref_r_enu(0); - double sigma_E_2_accuracy = sqrt(arma::sum(arma::square(tmp_vec)) / tmp_vec.n_cols); - tmp_vec = R_eb_enu.row(1) - ref_r_enu(1); - double sigma_N_2_accuracy = sqrt(arma::sum(arma::square(tmp_vec)) / tmp_vec.n_cols); - tmp_vec = R_eb_enu.row(2) - ref_r_enu(2); - double sigma_U_2_accuracy = sqrt(arma::sum(arma::square(tmp_vec)) / tmp_vec.n_cols); + arma::rowvec error_east_m; + error_east_m = R_eb_enu.row(0) - ref_r_enu(0); + double sigma_E_2_accuracy = arma::as_scalar(error_east_m * error_east_m.t()); + sigma_E_2_accuracy = sigma_E_2_accuracy / error_east_m.n_elem; - double mean__e = arma::mean(R_eb_enu.row(0)); - double mean__n = arma::mean(R_eb_enu.row(1)); - double mean__u = arma::mean(R_eb_enu.row(2)); + arma::rowvec error_north_m; + error_north_m = R_eb_enu.row(1) - ref_r_enu(1); + double sigma_N_2_accuracy = arma::as_scalar(error_north_m * error_north_m.t()); + sigma_N_2_accuracy = sigma_N_2_accuracy / error_north_m.n_elem; + + arma::rowvec error_up_m; + error_up_m = R_eb_enu.row(2) - ref_r_enu(2); + double sigma_U_2_accuracy = arma::as_scalar(error_up_m * error_up_m.t()); + sigma_U_2_accuracy = sigma_U_2_accuracy / error_up_m.n_elem; + + // arma::mat error_enu_mat = arma::zeros(3, error_east_m.n_elem); + // error_enu_mat.row(0) = error_east_m; + // error_enu_mat.row(1) = error_north_m; + // error_enu_mat.row(2) = error_up_m; + // + // arma::vec error_2D_m = arma::zeros(error_enu_mat.n_cols, 1); + // arma::vec error_3D_m = arma::zeros(error_enu_mat.n_cols, 1); + // for (uint64_t n = 0; n < error_enu_mat.n_cols; n++) + // { + // error_2D_m(n) = arma::norm(error_enu_mat.submat(0, n, 1, n)); + // error_3D_m(n) = arma::norm(error_enu_mat.col(n)); + // } + + double static_2D_error_m = sqrt(pow(arma::mean(error_east_m), 2.0) + pow(arma::mean(error_north_m), 2.0)); + double static_3D_error_m = sqrt(pow(arma::mean(error_east_m), 2.0) + pow(arma::mean(error_north_m), 2.0) + pow(arma::mean(error_up_m), 2.0)); std::stringstream stm; std::ofstream position_test_file; @@ -512,7 +600,7 @@ void PositionSystemTest::check_results() stm << "Configuration file: " << FLAGS_config_file_ptest << std::endl; } - stm << "---- ACCURACY ----" << std::endl; + stm << "---- STATIC 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 * sqrt(sigma_N_2_accuracy) + 0.56 * sqrt(sigma_E_2_accuracy) << " [m]" << std::endl; @@ -520,11 +608,11 @@ void PositionSystemTest::check_results() 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(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0)) << " [m]" << std::endl; - stm << "Bias 3D = " << sqrt(std::pow(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0) + std::pow(fabs(mean__u - ref_r_enu(2)), 2.0)) << " [m]" << std::endl; + stm << "Static Bias 2D = " << static_2D_error_m << " [m]" << std::endl; + stm << "Static Bias 3D = " << static_3D_error_m << " [m]" << std::endl; stm << std::endl; - stm << "---- PRECISION ----" << std::endl; + stm << "---- STATIC 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 * sqrt(sigma_N_2_precision) + 0.56 * sqrt(sigma_E_2_precision) << " [m]" << std::endl; @@ -543,8 +631,13 @@ void PositionSystemTest::check_results() } // Sanity Check + double accuracy_CEP = 0.62 * sqrt(sigma_N_2_accuracy) + 0.56 * sqrt(sigma_E_2_accuracy); double precision_SEP = 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision); - ASSERT_LT(precision_SEP, 1.0); + + EXPECT_LT(static_2D_error_m, 2.0); + EXPECT_LT(static_2D_error_m, 5.0); + ASSERT_LT(accuracy_CEP, 2.0); + ASSERT_LT(precision_SEP, 5.0); if (FLAGS_plot_position_test == true) { @@ -745,6 +838,11 @@ void PositionSystemTest::check_results() } } } + + //ERROR CHECK + //todo: reduce the error tolerance or enable the option to pass the error tolerance by parameter + EXPECT_LT(rmse_R_eb_e, 10.0); //3D RMS positioning error less than 10 meters + EXPECT_LT(rmse_V_eb_e, 5.0); //3D RMS speed error less than 5 meters/s (18 km/h) } } diff --git a/src/utils/rinex2assist/main.cc b/src/utils/rinex2assist/main.cc index f003e5c35..11792d040 100644 --- a/src/utils/rinex2assist/main.cc +++ b/src/utils/rinex2assist/main.cc @@ -209,10 +209,7 @@ int main(int argc, char** argv) if (j != 0) { std::ofstream ofs2; - if (xml_filename.empty()) - { - xml_filename = "eph_Galileo_E1.xml"; - } + xml_filename = "eph_Galileo_E1.xml"; try { ofs2.open(xml_filename.c_str(), std::ofstream::trunc | std::ofstream::out);