mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-15 12:40:35 +00:00
Add positioning error threshold for the RTKLib solver unit test
This commit is contained in:
parent
366401a595
commit
f0d43999bb
@ -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}
|
||||
|
@ -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
|
||||
}
|
||||
|
@ -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
|
||||
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user