mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-15 20:50:33 +00:00
Code Issues Releases Wiki Activity

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

Browse Source
This commit is contained in:
Carles Fernandez 2018-10-18 09:31:27 +02:00
commit c0fbc29488
6 changed files with 210 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/core/libs/gnss_sdr_supl_client.cc
View File

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

16
src/core/libs/gnss_sdr_supl_client.h
View File

@ -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.
*/

43
src/core/receiver/control_thread.cc
View File

@ -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

2
src/core/receiver/control_thread.h
View File

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

128
src/tests/system-tests/position_test.cc
View File

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

5
src/utils/rinex2assist/main.cc
View File

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