Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

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<int, Gps_Ephemeris> eph_map)
 {
     if (eph_map.empty() == false)

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 <boost/archive/xml_oarchive.hpp>
 #include <boost/archive/xml_iarchive.hpp>
 #include <boost/serialization/map.hpp>
@@ -77,6 +79,8 @@ public:
     int request;
     // ephemeris map
     std::map<int, Gps_Ephemeris> gps_ephemeris_map;
+    std::map<int, Galileo_Ephemeris> gal_ephemeris_map;
+    std::map<int, Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map;
     // almanac map
     std::map<int, Gps_Almanac> 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.
      */

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<int, Gps_Ephemeris>::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<Gps_Ephemeris> tmp_obj = std::make_shared<Gps_Ephemeris>(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<int, Galileo_Ephemeris>::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<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(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<int, Gps_CNAV_Ephemeris>::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<Gps_CNAV_Ephemeris> tmp_obj = std::make_shared<Gps_CNAV_Ephemeris>(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

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_*/

src/tests/system-tests/position_test.cc

@@ -56,6 +56,7 @@
 #include <numeric>
 #include <thread>
 #include <armadillo>
+#include <matio.h>
 
 // For GPS NAVIGATION (L1)
 concurrent_queue<Gps_Acq_Assist> 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<double>& x, std::vector<double>& y, std::string filename);
+    bool save_mat_x(std::vector<double>& 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<double>& x, std::vector<double>& 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<int64_t*>(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<double>& 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<int64_t*>(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;
+            arma::rowvec error_east_m;
-            tmp_vec = R_eb_enu.row(0) - ref_r_enu(0);
+            error_east_m = 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);
+            double sigma_E_2_accuracy = arma::as_scalar(error_east_m * error_east_m.t());
-            tmp_vec = R_eb_enu.row(1) - ref_r_enu(1);
+            sigma_E_2_accuracy = sigma_E_2_accuracy / error_east_m.n_elem;
-            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);
 
-            double mean__e = arma::mean(R_eb_enu.row(0));
+            arma::rowvec error_north_m;
-            double mean__n = arma::mean(R_eb_enu.row(1));
+            error_north_m = R_eb_enu.row(1) - ref_r_enu(1);
-            double mean__u = arma::mean(R_eb_enu.row(2));
+            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 << "Static Bias 2D = " << static_2D_error_m << " [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 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)
         }
 }
 

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);