diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 7a0b25218..29489e66b 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -403,6 +403,7 @@ if(ENABLE_UNIT_TESTING) signal_generator_adapters pvt_gr_blocks signal_processing_testing_lib + system_testing_lib ${VOLK_GNSSSDR_LIBRARIES} ${MATIO_LIBRARIES} ${GNSS_SDR_TEST_OPTIONAL_LIBS} diff --git a/src/tests/system-tests/libs/geofunctions.cc b/src/tests/system-tests/libs/geofunctions.cc index 8c1b985a0..e06654ac9 100644 --- a/src/tests/system-tests/libs/geofunctions.cc +++ b/src/tests/system-tests/libs/geofunctions.cc @@ -458,3 +458,18 @@ void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_ // Transform velocity using (2.73) v_eb_e = C_e_n.t() * v_eb_n; } + +double great_circle_distance(double lat1, double lon1, double lat2, double lon2) +{ + //The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes. + // generally used geo measurement function + double R = 6378.137; // Radius of earth in KM + double dLat = lat2 * STRP_PI / 180.0 - lat1 * STRP_PI / 180.0; + double dLon = lon2 * STRP_PI / 180.0 - lon1 * STRP_PI / 180.0; + double a = sin(dLat / 2.0) * sin(dLat / 2.0) + + cos(lat1 * STRP_PI / 180.0) * cos(lat2 * STRP_PI / 180.0) * + sin(dLon / 2) * sin(dLon / 2.0); + double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a)); + double d = R * c; + return d * 1000.0; // meters +} diff --git a/src/tests/system-tests/libs/geofunctions.h b/src/tests/system-tests/libs/geofunctions.h index c03655e17..bc111cfb1 100644 --- a/src/tests/system-tests/libs/geofunctions.h +++ b/src/tests/system-tests/libs/geofunctions.h @@ -151,4 +151,10 @@ void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat */ void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e); +/*! + * \brief The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes. + */ +double great_circle_distance(double lat1, double lon1, double lat2, double lon2); + + #endif diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc index d7584210d..bc073d587 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc @@ -38,6 +38,8 @@ #include "rtklib_solver.h" #include "in_memory_configuration.h" #include "gnss_sdr_supl_client.h" +#include "geofunctions.h" +#include rtk_t configure_rtklib_options() @@ -366,10 +368,6 @@ TEST(RTKLibSolverTest, test1) // // gnss_synchro_map[0] = tmp_obs; // gnss_synchro_map[0].PRN = 1; - // gnss_synchro_map[0].RX_time = 518449.000000; - // gnss_synchro_map[0].Pseudorange_m = 22816591.664859; - // gnss_synchro_map[0].Carrier_Doppler_hz = -2579.334343; - // gnss_synchro_map[0].Carrier_phase_rads = 794858.014183; //load from xml (boost serialize) std::string file_name = path + "data/rtklib_test/obs_test1.xml"; @@ -417,10 +415,6 @@ TEST(RTKLibSolverTest, test1) // 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; - std::cout << "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::endl; - std::cout << "RTKLIB Position at RX TOW = " << gnss_synchro_map.begin()->second.RX_time << " in ECEF (X,Y,Z,t[meters]) = " << std::fixed << std::setprecision(16) << d_ls_pvt->pvt_sol.rr[0] << "," @@ -433,8 +427,32 @@ TEST(RTKLibSolverTest, test1) //todo: check here the positioning error against the reference position generated with gnss-sim //reference position on in WGS84: Lat (deg), Long (deg) , H (m): 30.286502,120.032669,100 + arma::vec LLH = {30.286502, 120.032669, 100}; //ref position for this scenario + double error_LLH_m = great_circle_distance(LLH(0), LLH(1), d_ls_pvt->get_latitude(), d_ls_pvt->get_longitude()); + std::cout << "Lat, Long, H error: " << d_ls_pvt->get_latitude() - LLH(0) + << "," << d_ls_pvt->get_longitude() - LLH(1) + << "," << d_ls_pvt->get_height() - LLH(2) << " [deg,deg,meters]" << std::endl; + std::cout << "Haversine Great Circle error LLH distance: " << error_LLH_m << " [meters]" << std::endl; + + arma::vec v_eb_n = {0.0, 0.0, 0.0}; + arma::vec true_r_eb_e; + arma::vec true_v_eb_e; + pv_Geo_to_ECEF(degtorad(LLH(0)), degtorad(LLH(1)), LLH(2), v_eb_n, true_r_eb_e, true_v_eb_e); + + arma::vec measured_r_eb_e = {d_ls_pvt->pvt_sol.rr[0], d_ls_pvt->pvt_sol.rr[1], d_ls_pvt->pvt_sol.rr[2]}; + + arma::vec error_r_eb_e = measured_r_eb_e - true_r_eb_e; + + std::cout << "ECEF position error vector: " << error_r_eb_e << " [meters]" << std::endl; + + double error_3d_m = arma::norm(error_r_eb_e, 2); + + std::cout << "3D positioning error: " << error_3d_m << " [meters]" << std::endl; + + //check results against the test tolerance + ASSERT_LT(error_3d_m, 1.0); pvt_valid = true; } }