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

Add positioning error threshold for the RTKLib solver unit test

Browse Source
This commit is contained in:
Javier 2018-10-09 18:19:13 +02:00
parent 366401a595
commit f0d43999bb
4 changed files with 48 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/tests/CMakeLists.txt
View File

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

15
src/tests/system-tests/libs/geofunctions.cc
View File

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

6
src/tests/system-tests/libs/geofunctions.h
View File

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

34
src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc
View File

@ -38,6 +38,8 @@
#include "rtklib_solver.h"
#include "in_memory_configuration.h"
#include "gnss_sdr_supl_client.h"
#include "geofunctions.h"
#include <armadillo>
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;
}
}