mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-15 12:40:35 +00:00
Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next
This commit is contained in:
Carles Fernandez
commit
86dfea348d
@ -523,7 +523,6 @@ if(NOT GNURADIO_RUNTIME_FOUND)
|
||||
message("You can install it easily via Macports:")
|
||||
message(" sudo port install gnuradio ")
|
||||
message("Alternatively, you can use homebrew:")
|
||||
message(" brew tap odrisci/gnuradio")
|
||||
message(" brew install gnuradio" )
|
||||
message(FATAL_ERROR "GNU Radio ${GNSSSDR_GNURADIO_MIN_VERSION} or later is required to build gnss-sdr")
|
||||
endif(OS_IS_MACOSX)
|
||||
|
||||
10
MANIFEST.md
10
MANIFEST.md
@ -12,11 +12,17 @@ author:
|
||||
- et altri (see AUTHORS file for a list of contributors)
|
||||
copyright_owner:
|
||||
- The Authors
|
||||
dependencies: gnuradio (>= 3.7.3), armadillo, gflags, glog, gnutls, matio
|
||||
dependencies:
|
||||
- gnuradio (>= 3.7.3)
|
||||
- armadillo
|
||||
- gflags
|
||||
- glog
|
||||
- gnutls
|
||||
- matio
|
||||
license: GPLv3+
|
||||
repo: https://github.com/gnss-sdr/gnss-sdr
|
||||
website: https://gnss-sdr.org
|
||||
icon: https://raw.githubusercontent.com/gnss-sdr/gnss-sdr/master/docs/doxygen/images/gnss-sdr_logo.png
|
||||
icon: https://gnss-sdr.org/assets/images/logo400x400.jpg
|
||||
---
|
||||
Global Navigation Satellite Systems receiver defined by software. It performs all the signal
|
||||
processing from raw signal samples up to the computation of the Position-Velocity-Time solution,
|
||||
|
||||
@ -364,8 +364,7 @@ rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels,
|
||||
d_rinexobs_rate_ms = rinexobs_rate_ms;
|
||||
d_rinexnav_rate_ms = rinexnav_rate_ms;
|
||||
|
||||
d_dump_filename.append("_raw.dat");
|
||||
dump_ls_pvt_filename.append("_ls_pvt.dat");
|
||||
dump_ls_pvt_filename.append("_pvt.dat");
|
||||
|
||||
d_ls_pvt = std::make_shared<rtklib_solver>(static_cast<int32_t>(nchannels), dump_ls_pvt_filename, d_dump, rtk);
|
||||
d_ls_pvt->set_averaging_depth(1);
|
||||
@ -374,23 +373,6 @@ rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels,
|
||||
|
||||
d_last_status_print_seg = 0;
|
||||
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_dump == true)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
LOG(INFO) << "PVT dump enabled Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception opening PVT dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Create Sys V message queue
|
||||
first_fix = true;
|
||||
@ -500,18 +482,6 @@ rtklib_pvt_cc::~rtklib_pvt_cc()
|
||||
{
|
||||
LOG(WARNING) << "Failed to save GLONASS GNAV Ephemeris, map is empty";
|
||||
}
|
||||
|
||||
if (d_dump_file.is_open() == true)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_file.close();
|
||||
}
|
||||
catch (const std::exception& ex)
|
||||
{
|
||||
LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -2102,7 +2072,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
<< std::fixed << std::setprecision(3)
|
||||
<< " [deg], Height = " << d_ls_pvt->get_height() << " [m]" << TEXT_RESET << std::endl;
|
||||
std::cout << std::setprecision(ss);
|
||||
LOG(INFO) << "RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]";
|
||||
DLOG(INFO) << "RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]";
|
||||
|
||||
// boost::posix_time::ptime p_time;
|
||||
// gtime_t rtklib_utc_time = gpst2time(adjgpsweek(d_ls_pvt->gps_ephemeris_map.cbegin()->second.i_GPS_week), d_rx_time);
|
||||
@ -2110,36 +2080,15 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
// 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;
|
||||
|
||||
LOG(INFO) << "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]";
|
||||
DLOG(INFO) << "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::cout << "Dilution of Precision 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 HDOP = " << d_ls_pvt->get_hdop() << " VDOP = "
|
||||
<< d_ls_pvt->get_vdop()
|
||||
<< " GDOP = " << d_ls_pvt->get_gdop() << std::endl; */
|
||||
}
|
||||
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
if (d_dump == true)
|
||||
{
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
for (uint32_t i = 0; i < d_nchannels; i++)
|
||||
{
|
||||
tmp_double = in[i][epoch].Pseudorange_m;
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
tmp_double = 0;
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_rx_time), sizeof(double));
|
||||
}
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing observables dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -122,7 +122,6 @@ private:
|
||||
|
||||
uint32_t d_nchannels;
|
||||
std::string d_dump_filename;
|
||||
std::ofstream d_dump_file;
|
||||
|
||||
int32_t d_output_rate_ms;
|
||||
int32_t d_display_rate_ms;
|
||||
|
||||
@ -86,7 +86,7 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
|
||||
LOG(WARNING) << "Exception opening RTKLIB dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -103,7 +103,7 @@ rtklib_solver::~rtklib_solver()
|
||||
}
|
||||
catch (const std::exception& ex)
|
||||
{
|
||||
LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what();
|
||||
LOG(WARNING) << "Exception in destructor closing the RTKLIB dump file " << ex.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -556,34 +556,55 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
uint32_t tmp_uint32;
|
||||
// TOW
|
||||
tmp_uint32 = gnss_observables_map.begin()->second.TOW_at_current_symbol_ms;
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_uint32), sizeof(uint32_t));
|
||||
// WEEK
|
||||
tmp_uint32 = adjgpsweek(nav_data.eph[0].week);
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_uint32), sizeof(uint32_t));
|
||||
// PVT GPS time
|
||||
tmp_double = gnss_observables_map.begin()->second.RX_time;
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double = rx_position_and_time(0);
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double = rx_position_and_time(1);
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double = rx_position_and_time(2);
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double = rx_position_and_time(3);
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
|
||||
// ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double)
|
||||
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.rr[0]), sizeof(pvt_sol.rr));
|
||||
|
||||
// position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double)
|
||||
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.qr[0]), sizeof(pvt_sol.qr));
|
||||
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double = this->get_latitude();
|
||||
tmp_double = get_latitude();
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double = this->get_longitude();
|
||||
tmp_double = get_longitude();
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double = this->get_height();
|
||||
tmp_double = get_height();
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
|
||||
// NUMBER OF VALID SATS
|
||||
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.ns), sizeof(uint8_t));
|
||||
// RTKLIB solution status
|
||||
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.stat), sizeof(uint8_t));
|
||||
// RTKLIB solution type (0:xyz-ecef,1:enu-baseline)
|
||||
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.type), sizeof(uint8_t));
|
||||
//AR ratio factor for validation
|
||||
tmp_double = pvt_sol.ratio;
|
||||
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.ratio), sizeof(float));
|
||||
//AR ratio threshold for validation
|
||||
tmp_double = pvt_sol.thres;
|
||||
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.thres), sizeof(float));
|
||||
|
||||
//GDOP//PDOP//HDOP//VDOP
|
||||
d_dump_file.write(reinterpret_cast<char*>(&dop_[0]), sizeof(dop_));
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
|
||||
LOG(WARNING) << "Exception writing RTKLIB dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -89,38 +89,6 @@ double leaps[MAXLEAPS + 1][7] = {/* leap seconds (y,m,d,h,m,s,utc-gpst) */
|
||||
{}};
|
||||
|
||||
|
||||
const prcopt_t prcopt_default = { /* defaults processing options */
|
||||
PMODE_SINGLE, 0, 2, SYS_GPS, /* mode, soltype, nf, navsys */
|
||||
15.0 * D2R, {{}, {{}, {}}}, /* elmin, snrmask */
|
||||
0, 1, 1, 1, /* sateph, modear, glomodear, bdsmodear */
|
||||
5, 0, 10, 1, /* maxout, minlock, minfix, armaxiter */
|
||||
0, 0, 0, 0, /* estion, esttrop, dynamics, tidecorr */
|
||||
1, 0, 0, 0, 0, /* niter, codesmooth, intpref, sbascorr, sbassatsel */
|
||||
0, 0, /* rovpos, refpos */
|
||||
{100.0, 100.0, 100.0}, /* eratio[] */
|
||||
{100.0, 0.003, 0.003, 0.0, 1.0}, /* err[] */
|
||||
{30.0, 0.03, 0.3}, /* std[] */
|
||||
{1e-4, 1e-3, 1e-4, 1e-1, 1e-2, 0.0}, /* prn[] */
|
||||
5E-12, /* sclkstab */
|
||||
{3.0, 0.9999, 0.25, 0.1, 0.05, 0, 0, 0}, /* thresar */
|
||||
0.0, 0.0, 0.05, /* elmaskar, almaskhold, thresslip */
|
||||
30.0, 30.0, 30.0, /* maxtdif, maxinno, maxgdop */
|
||||
{}, {}, {}, /* baseline, ru, rb */
|
||||
{"", ""}, /* anttype */
|
||||
{}, {}, {}, /* antdel, pcv, exsats */
|
||||
0, 0, 0, {"", ""}, {}, 0, {{}, {}}, {{}, {{}, {}}, {{}, {}}, {}, {}}, 0, {}};
|
||||
|
||||
|
||||
const solopt_t solopt_default = {
|
||||
/* defaults solution output options */
|
||||
SOLF_LLH, TIMES_GPST, 1, 3, /* posf, times, timef, timeu */
|
||||
0, 1, 0, 0, 0, 0, /* degf, outhead, outopt, datum, height, geoid */
|
||||
0, 0, 0, /* solstatic, sstat, trace */
|
||||
{0.0, 0.0}, /* nmeaintv */
|
||||
" ", "", 0 /* separator/program name */
|
||||
};
|
||||
|
||||
|
||||
const char *formatstrs[32] = {/* stream format strings */
|
||||
"RTCM 2", /* 0 */
|
||||
"RTCM 3", /* 1 */
|
||||
|
||||
@ -18,6 +18,7 @@
|
||||
|
||||
|
||||
add_subdirectory(unit-tests/signal-processing-blocks/libs)
|
||||
add_subdirectory(system-tests/libs)
|
||||
|
||||
################################################################################
|
||||
# Google Test - https://github.com/google/googletest
|
||||
@ -342,6 +343,7 @@ set(LIST_INCLUDE_DIRS
|
||||
${CMAKE_SOURCE_DIR}/src/algorithms/acquisition/gnuradio_blocks
|
||||
${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs
|
||||
${CMAKE_SOURCE_DIR}/src/tests/unit-tests/signal-processing-blocks/libs
|
||||
${CMAKE_SOURCE_DIR}/src/tests/system-tests/libs
|
||||
${CMAKE_SOURCE_DIR}/src/tests/common-files
|
||||
${GLOG_INCLUDE_DIRS}
|
||||
${GFlags_INCLUDE_DIRS}
|
||||
@ -489,29 +491,24 @@ if(ENABLE_SYSTEM_TESTING)
|
||||
${GTEST_LIBRARIES} ${GNURADIO_RUNTIME_LIBRARIES}
|
||||
${GNURADIO_BLOCKS_LIBRARIES} ${GNURADIO_FILTER_LIBRARIES}
|
||||
${GNURADIO_ANALOG_LIBRARIES} ${VOLK_GNSSSDR_LIBRARIES}
|
||||
gnss_sp_libs gnss_rx gnss_system_parameters )
|
||||
gnss_sp_libs gnss_rx gnss_system_parameters
|
||||
system_testing_lib)
|
||||
|
||||
add_system_test(position_test)
|
||||
|
||||
if(GPSTK_FOUND OR OWN_GPSTK)
|
||||
#if(GPSTK_FOUND OR OWN_GPSTK)
|
||||
## OBS_SYSTEM_TEST and OBS_GPS_L1_SYSTEM_TEST
|
||||
set(OPT_LIBS_ ${GFlags_LIBS} ${GLOG_LIBRARIES} ${GTEST_LIBRARIES}
|
||||
gnss_sp_libs gnss_rx ${gpstk_libs} )
|
||||
set(OPT_INCLUDES_ ${GPSTK_INCLUDE_DIRS} ${GPSTK_INCLUDE_DIRS}/gpstk)
|
||||
add_system_test(obs_gps_l1_system_test)
|
||||
add_system_test(obs_system_test)
|
||||
endif(GPSTK_FOUND OR OWN_GPSTK)
|
||||
# set(OPT_LIBS_ ${GFlags_LIBS} ${GLOG_LIBRARIES} ${GTEST_LIBRARIES}
|
||||
# gnss_sp_libs gnss_rx ${gpstk_libs} )
|
||||
# set(OPT_INCLUDES_ ${GPSTK_INCLUDE_DIRS} ${GPSTK_INCLUDE_DIRS}/gpstk)
|
||||
# add_system_test(obs_gps_l1_system_test)
|
||||
# add_system_test(obs_system_test)
|
||||
#endif(GPSTK_FOUND OR OWN_GPSTK)
|
||||
else(ENABLE_SYSTEM_TESTING_EXTRA)
|
||||
# Avoid working with old executables if they were switched ON and then OFF
|
||||
if(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test)
|
||||
file(REMOVE ${CMAKE_SOURCE_DIR}/install/position_test)
|
||||
endif(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test)
|
||||
if(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test)
|
||||
file(REMOVE ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test)
|
||||
endif(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test)
|
||||
if(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_system_test)
|
||||
file(REMOVE ${CMAKE_SOURCE_DIR}/install/obs_system_test)
|
||||
endif(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_system_test)
|
||||
endif(ENABLE_SYSTEM_TESTING_EXTRA)
|
||||
else(ENABLE_SYSTEM_TESTING)
|
||||
# Avoid working with old executables if they were switched ON and then OFF
|
||||
@ -521,12 +518,6 @@ else(ENABLE_SYSTEM_TESTING)
|
||||
if(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test)
|
||||
file(REMOVE ${CMAKE_SOURCE_DIR}/install/position_test)
|
||||
endif(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test)
|
||||
if(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test)
|
||||
file(REMOVE ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test)
|
||||
endif(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test)
|
||||
if(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_system_test)
|
||||
file(REMOVE ${CMAKE_SOURCE_DIR}/install/obs_system_test)
|
||||
endif(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_system_test)
|
||||
endif(ENABLE_SYSTEM_TESTING)
|
||||
|
||||
|
||||
|
||||
@ -38,7 +38,7 @@ DEFINE_bool(disable_generator, false, "Disable the signal generator (a external
|
||||
DEFINE_string(generator_binary, std::string(SW_GENERATOR_BIN), "Path of software-defined signal generator binary");
|
||||
DEFINE_string(rinex_nav_file, std::string(DEFAULT_RINEX_NAV), "Input RINEX navigation file");
|
||||
DEFINE_int32(duration, 100, "Duration of the experiment [in seconds, max = 300]");
|
||||
DEFINE_string(static_position, "30.286502,120.032669,100", "Static receiver position [log,lat,height]");
|
||||
DEFINE_string(static_position, "30.286502,120.032669,100", "Static receiver position [latitude,longitude,height]");
|
||||
DEFINE_string(dynamic_position, "", "Observer positions file, in .csv or .nmea format");
|
||||
DEFINE_string(filename_rinex_obs, "sim.16o", "Filename of output RINEX navigation file");
|
||||
DEFINE_string(filename_raw_data, "signal_out.bin", "Filename of output raw data file");
|
||||
|
||||
41
src/tests/system-tests/libs/CMakeLists.txt
Normal file
41
src/tests/system-tests/libs/CMakeLists.txt
Normal file
@ -0,0 +1,41 @@
|
||||
# Copyright (C) 2012-2018 (see AUTHORS file for a list of contributors)
|
||||
#
|
||||
# This file is part of GNSS-SDR.
|
||||
#
|
||||
# GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation, either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# GNSS-SDR is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
||||
#
|
||||
|
||||
|
||||
set(SYSTEM_TESTING_LIB_SOURCES
|
||||
geofunctions.cc
|
||||
spirent_motion_csv_dump_reader.cc
|
||||
rtklib_solver_dump_reader.cc
|
||||
)
|
||||
|
||||
include_directories(
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
${GLOG_INCLUDE_DIRS}
|
||||
${GFlags_INCLUDE_DIRS}
|
||||
${MATIO_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
|
||||
file(GLOB SYSTEM_TESTING_LIB_HEADERS "*.h")
|
||||
list(SORT SYSTEM_TESTING_LIB_HEADERS)
|
||||
add_library(system_testing_lib ${SYSTEM_TESTING_LIB_SOURCES} ${SYSTEM_TESTING_LIB_HEADERS})
|
||||
source_group(Headers FILES ${SYSTEM_TESTING_LIB_HEADERS})
|
||||
|
||||
if(NOT MATIO_FOUND)
|
||||
add_dependencies(system_testing_lib matio-${GNSSSDR_MATIO_LOCAL_VERSION})
|
||||
endif(NOT MATIO_FOUND)
|
||||
473
src/tests/system-tests/libs/geofunctions.cc
Normal file
473
src/tests/system-tests/libs/geofunctions.cc
Normal file
@ -0,0 +1,473 @@
|
||||
/*!
|
||||
* \file geofunctions.h
|
||||
* \brief A set of coordinate transformations functions and helpers,
|
||||
* some of them migrated from MATLAB, for geographic information systems.
|
||||
* \author Javier Arribas, 2018. jarribas(at)cttc.es
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
#include "geofunctions.h"
|
||||
|
||||
#define STRP_G_SI 9.80665
|
||||
#define STRP_PI 3.1415926535898 //!< Pi as defined in IS-GPS-200E
|
||||
|
||||
arma::mat Skew_symmetric(arma::vec a)
|
||||
{
|
||||
arma::mat A = arma::zeros(3, 3);
|
||||
|
||||
A << 0.0 << -a(2) << a(1) << arma::endr
|
||||
<< a(2) << 0.0 << -a(0) << arma::endr
|
||||
<< -a(1) << a(0) << 0 << arma::endr;
|
||||
|
||||
// {{0, -a(2), a(1)},
|
||||
// {a(2), 0, -a(0)},
|
||||
// {-a(1), a(0), 0}};
|
||||
return A;
|
||||
}
|
||||
|
||||
|
||||
double WGS84_g0(double Lat_rad)
|
||||
{
|
||||
double k = 0.001931853; //normal gravity constant
|
||||
double e2 = 0.00669438002290; //the square of the first numerical eccentricity
|
||||
double nge = 9.7803253359; //normal gravity value on the equator (m/sec^2)
|
||||
double b = sin(Lat_rad); //Lat in degrees
|
||||
b = b * b;
|
||||
double g0 = nge * (1 + k * b) / (sqrt(1 - e2 * b));
|
||||
return g0;
|
||||
}
|
||||
|
||||
double WGS84_geocentric_radius(double Lat_geodetic_rad)
|
||||
{
|
||||
//WGS84 earth model Geocentric radius (Eq. 2.88)
|
||||
|
||||
double WGS84_A = 6378137.0; //Semi-major axis of the Earth, a [m]
|
||||
double WGS84_IF = 298.257223563; //Inverse flattening of the Earth
|
||||
double WGS84_F = (1 / WGS84_IF); //The flattening of the Earth
|
||||
//double WGS84_B=(WGS84_A*(1-WGS84_F)); // Semi-minor axis of the Earth [m]
|
||||
double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F)); //Eccentricity of the Earth
|
||||
|
||||
//transverse radius of curvature
|
||||
double R_E = WGS84_A / sqrt(1 -
|
||||
WGS84_E * WGS84_E *
|
||||
sin(Lat_geodetic_rad) *
|
||||
sin(Lat_geodetic_rad)); // (Eq. 2.66)
|
||||
|
||||
//gocentric radius at the Earth surface
|
||||
double r_eS = R_E * sqrt(cos(Lat_geodetic_rad) * cos(Lat_geodetic_rad) +
|
||||
(1 - WGS84_E * WGS84_E) * (1 - WGS84_E * WGS84_E) * sin(Lat_geodetic_rad) * sin(Lat_geodetic_rad)); // (Eq. 2.88)
|
||||
return r_eS;
|
||||
}
|
||||
|
||||
int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx)
|
||||
{
|
||||
double lambda;
|
||||
double phi;
|
||||
double h;
|
||||
double dtr = STRP_PI / 180.0;
|
||||
double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84
|
||||
double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84
|
||||
|
||||
// Transform x into geodetic coordinates
|
||||
togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
|
||||
|
||||
double cl = cos(lambda * dtr);
|
||||
double sl = sin(lambda * dtr);
|
||||
double cb = cos(phi * dtr);
|
||||
double sb = sin(phi * dtr);
|
||||
|
||||
arma::mat F = arma::zeros(3, 3);
|
||||
|
||||
F(0, 0) = -sl;
|
||||
F(0, 1) = -sb * cl;
|
||||
F(0, 2) = cb * cl;
|
||||
|
||||
F(1, 0) = cl;
|
||||
F(1, 1) = -sb * sl;
|
||||
F(1, 2) = cb * sl;
|
||||
|
||||
F(2, 0) = 0;
|
||||
F(2, 1) = cb;
|
||||
F(2, 2) = sb;
|
||||
|
||||
arma::vec local_vector;
|
||||
|
||||
local_vector = arma::htrans(F) * dx;
|
||||
|
||||
double E = local_vector(0);
|
||||
double N = local_vector(1);
|
||||
double U = local_vector(2);
|
||||
|
||||
double hor_dis;
|
||||
hor_dis = sqrt(E * E + N * N);
|
||||
|
||||
if (hor_dis < 1.0E-20)
|
||||
{
|
||||
*Az = 0;
|
||||
*El = 90;
|
||||
}
|
||||
else
|
||||
{
|
||||
*Az = atan2(E, N) / dtr;
|
||||
*El = atan2(U, hor_dis) / dtr;
|
||||
}
|
||||
|
||||
if (*Az < 0)
|
||||
{
|
||||
*Az = *Az + 360.0;
|
||||
}
|
||||
|
||||
*D = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2));
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z)
|
||||
{
|
||||
*h = 0;
|
||||
double tolsq = 1.e-10; // tolerance to accept convergence
|
||||
int maxit = 10; // max number of iterations
|
||||
double rtd = 180.0 / STRP_PI;
|
||||
|
||||
// compute square of eccentricity
|
||||
double esq;
|
||||
if (finv < 1.0E-20)
|
||||
{
|
||||
esq = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
esq = (2.0 - 1.0 / finv) / finv;
|
||||
}
|
||||
|
||||
// first guess
|
||||
double P = sqrt(X * X + Y * Y); // P is distance from spin axis
|
||||
|
||||
//direct calculation of longitude
|
||||
if (P > 1.0E-20)
|
||||
{
|
||||
*dlambda = atan2(Y, X) * rtd;
|
||||
}
|
||||
else
|
||||
{
|
||||
*dlambda = 0.0;
|
||||
}
|
||||
|
||||
// correct longitude bound
|
||||
if (*dlambda < 0)
|
||||
{
|
||||
*dlambda = *dlambda + 360.0;
|
||||
}
|
||||
|
||||
double r = sqrt(P * P + Z * Z); // r is distance from origin (0,0,0)
|
||||
|
||||
double sinphi;
|
||||
if (r > 1.0E-20)
|
||||
{
|
||||
sinphi = Z / r;
|
||||
}
|
||||
else
|
||||
{
|
||||
sinphi = 0.0;
|
||||
}
|
||||
*dphi = asin(sinphi);
|
||||
|
||||
// initial value of height = distance from origin minus
|
||||
// approximate distance from origin to surface of ellipsoid
|
||||
if (r < 1.0E-20)
|
||||
{
|
||||
*h = 0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
*h = r - a * (1 - sinphi * sinphi / finv);
|
||||
|
||||
// iterate
|
||||
double cosphi;
|
||||
double N_phi;
|
||||
double dP;
|
||||
double dZ;
|
||||
double oneesq = 1.0 - esq;
|
||||
|
||||
for (int i = 0; i < maxit; i++)
|
||||
{
|
||||
sinphi = sin(*dphi);
|
||||
cosphi = cos(*dphi);
|
||||
|
||||
// compute radius of curvature in prime vertical direction
|
||||
N_phi = a / sqrt(1 - esq * sinphi * sinphi);
|
||||
|
||||
// compute residuals in P and Z
|
||||
dP = P - (N_phi + (*h)) * cosphi;
|
||||
dZ = Z - (N_phi * oneesq + (*h)) * sinphi;
|
||||
|
||||
// update height and latitude
|
||||
*h = *h + (sinphi * dZ + cosphi * dP);
|
||||
*dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h));
|
||||
|
||||
// test for convergence
|
||||
if ((dP * dP + dZ * dZ) < tolsq)
|
||||
{
|
||||
break;
|
||||
}
|
||||
if (i == (maxit - 1))
|
||||
{
|
||||
// LOG(WARNING) << "The computation of geodetic coordinates did not converge";
|
||||
}
|
||||
}
|
||||
*dphi = (*dphi) * rtd;
|
||||
return 0;
|
||||
}
|
||||
|
||||
arma::mat Gravity_ECEF(arma::vec r_eb_e)
|
||||
{
|
||||
//Parameters
|
||||
double R_0 = 6378137; //WGS84 Equatorial radius in meters
|
||||
double mu = 3.986004418E14; //WGS84 Earth gravitational constant (m^3 s^-2)
|
||||
double J_2 = 1.082627E-3; //WGS84 Earth's second gravitational constant
|
||||
double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s)
|
||||
// Calculate distance from center of the Earth
|
||||
double mag_r = sqrt(arma::as_scalar(r_eb_e.t() * r_eb_e));
|
||||
// If the input position is 0,0,0, produce a dummy output
|
||||
arma::vec g = arma::zeros(3, 1);
|
||||
if (mag_r != 0)
|
||||
{
|
||||
//Calculate gravitational acceleration using (2.142)
|
||||
double z_scale = 5 * pow((r_eb_e(2) / mag_r), 2);
|
||||
arma::vec tmp_vec = {(1 - z_scale) * r_eb_e(0),
|
||||
(1 - z_scale) * r_eb_e(1),
|
||||
(3 - z_scale) * r_eb_e(2)};
|
||||
arma::vec gamma_ = (-mu / pow(mag_r, 3)) * (r_eb_e + 1.5 * J_2 * pow(R_0 / mag_r, 2) * tmp_vec);
|
||||
|
||||
//Add centripetal acceleration using (2.133)
|
||||
g(0) = gamma_(0) + pow(omega_ie, 2) * r_eb_e(0);
|
||||
g(1) = gamma_(1) + pow(omega_ie, 2) * r_eb_e(1);
|
||||
g(2) = gamma_(2);
|
||||
}
|
||||
return g;
|
||||
}
|
||||
|
||||
arma::vec LLH_to_deg(arma::vec LLH)
|
||||
{
|
||||
double rtd = 180.0 / STRP_PI;
|
||||
LLH(0) = LLH(0) * rtd;
|
||||
LLH(1) = LLH(1) * rtd;
|
||||
return LLH;
|
||||
}
|
||||
|
||||
double degtorad(double angleInDegrees)
|
||||
{
|
||||
double angleInRadians = (STRP_PI / 180.0) * angleInDegrees;
|
||||
return angleInRadians;
|
||||
}
|
||||
|
||||
double radtodeg(double angleInRadians)
|
||||
{
|
||||
double angleInDegrees = (180.0 / STRP_PI) * angleInRadians;
|
||||
return angleInDegrees;
|
||||
}
|
||||
|
||||
|
||||
double mstoknotsh(double MetersPerSeconds)
|
||||
{
|
||||
double knots = mstokph(MetersPerSeconds) * 0.539957;
|
||||
return knots;
|
||||
}
|
||||
|
||||
double mstokph(double MetersPerSeconds)
|
||||
{
|
||||
double kph = 3600.0 * MetersPerSeconds / 1e3;
|
||||
return kph;
|
||||
}
|
||||
|
||||
arma::vec CTM_to_Euler(arma::mat C)
|
||||
{
|
||||
//Calculate Euler angles using (2.23)
|
||||
arma::vec eul = arma::zeros(3, 1);
|
||||
eul(0) = atan2(C(1, 2), C(2, 2)); // roll
|
||||
if (C(0, 2) < -1.0) C(0, 2) = -1.0;
|
||||
if (C(0, 2) > 1.0) C(0, 2) = 1.0;
|
||||
eul(1) = -asin(C(0, 2)); // pitch
|
||||
eul(2) = atan2(C(0, 1), C(0, 0)); // yaw
|
||||
return eul;
|
||||
}
|
||||
|
||||
arma::mat Euler_to_CTM(arma::vec eul)
|
||||
{
|
||||
//Eq.2.15
|
||||
//Euler angles to Attitude matrix is equivalent to rotate the body
|
||||
//in the three axes:
|
||||
// arma::mat Ax= {{1,0,0}, {0,cos(Att_phi),sin(Att_phi)} ,{0,-sin(Att_phi),cos(Att_phi)}};
|
||||
// arma::mat Ay= {{cos(Att_theta), 0, -sin(Att_theta)}, {0,1,0} , {sin(Att_theta), 0, cos(Att_theta)}};
|
||||
// arma::mat Az= {{cos(Att_psi), sin(Att_psi), 0}, {-sin(Att_psi), cos(Att_psi), 0},{0,0,1}};
|
||||
// arma::mat C_b_n=Ax*Ay*Az; // Attitude expressed in the LOCAL FRAME (NED)
|
||||
// C_b_n=C_b_n.t();
|
||||
|
||||
//Precalculate sines and cosines of the Euler angles
|
||||
double sin_phi = sin(eul(0));
|
||||
double cos_phi = cos(eul(0));
|
||||
double sin_theta = sin(eul(1));
|
||||
double cos_theta = cos(eul(1));
|
||||
double sin_psi = sin(eul(2));
|
||||
double cos_psi = cos(eul(2));
|
||||
|
||||
arma::mat C = arma::zeros(3, 3);
|
||||
//Calculate coordinate transformation matrix using (2.22)
|
||||
C(0, 0) = cos_theta * cos_psi;
|
||||
C(0, 1) = cos_theta * sin_psi;
|
||||
C(0, 2) = -sin_theta;
|
||||
C(1, 0) = -cos_phi * sin_psi + sin_phi * sin_theta * cos_psi;
|
||||
C(1, 1) = cos_phi * cos_psi + sin_phi * sin_theta * sin_psi;
|
||||
C(1, 2) = sin_phi * cos_theta;
|
||||
C(2, 0) = sin_phi * sin_psi + cos_phi * sin_theta * cos_psi;
|
||||
C(2, 1) = -sin_phi * cos_psi + cos_phi * sin_theta * sin_psi;
|
||||
C(2, 2) = cos_phi * cos_theta;
|
||||
return C;
|
||||
}
|
||||
|
||||
arma::vec cart2geo(arma::vec XYZ, int elipsoid_selection)
|
||||
{
|
||||
const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0};
|
||||
const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563};
|
||||
|
||||
double lambda = atan2(XYZ[1], XYZ[0]);
|
||||
double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection]));
|
||||
double c = a[elipsoid_selection] * sqrt(1.0 + ex2);
|
||||
double phi = atan(XYZ[2] / ((sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection])));
|
||||
|
||||
double h = 0.1;
|
||||
double oldh = 0.0;
|
||||
double N;
|
||||
int iterations = 0;
|
||||
do
|
||||
{
|
||||
oldh = h;
|
||||
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
|
||||
phi = atan(XYZ[2] / ((sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h)))));
|
||||
h = sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) / cos(phi) - N;
|
||||
iterations = iterations + 1;
|
||||
if (iterations > 100)
|
||||
{
|
||||
//std::cout << "Failed to approximate h with desired precision. h-oldh= " << h - oldh;
|
||||
break;
|
||||
}
|
||||
}
|
||||
while (std::abs(h - oldh) > 1.0e-12);
|
||||
|
||||
arma::vec LLH = {{phi, lambda, h}}; //radians
|
||||
return LLH;
|
||||
}
|
||||
|
||||
void ECEF_to_Geo(arma::vec r_eb_e, arma::vec v_eb_e, arma::mat C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n)
|
||||
{
|
||||
//Compute the Latitude of the ECEF position
|
||||
LLH = cart2geo(r_eb_e, 4); //ECEF -> WGS84 geographical
|
||||
|
||||
// Calculate ECEF to Geographical coordinate transformation matrix using (2.150)
|
||||
double cos_lat = cos(LLH(0));
|
||||
double sin_lat = sin(LLH(0));
|
||||
double cos_long = cos(LLH(1));
|
||||
double sin_long = sin(LLH(1));
|
||||
//C++11 and arma >= 5.2
|
||||
// arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat},
|
||||
// {-sin_long, cos_long, 0},
|
||||
// {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}}; //ECEF to Geo
|
||||
|
||||
//C++98 arma <5.2
|
||||
arma::mat C_e_n = arma::zeros(3, 3);
|
||||
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
|
||||
<< -sin_long << cos_long << 0 << arma::endr
|
||||
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr; //ECEF to Geo
|
||||
// Transform velocity using (2.73)
|
||||
v_eb_n = C_e_n * v_eb_e;
|
||||
|
||||
C_b_n = C_e_n * C_b_e; // Attitude conversion from ECEF to NED
|
||||
}
|
||||
|
||||
void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e)
|
||||
{
|
||||
// Parameters
|
||||
double R_0 = 6378137; //WGS84 Equatorial radius in meters
|
||||
double e = 0.0818191908425; //WGS84 eccentricity
|
||||
|
||||
// Calculate transverse radius of curvature using (2.105)
|
||||
double R_E = R_0 / sqrt(1 - (e * sin(LLH(0))) * (e * sin(LLH(0))));
|
||||
|
||||
// Convert position using (2.112)
|
||||
double cos_lat = cos(LLH(0));
|
||||
double sin_lat = sin(LLH(0));
|
||||
double cos_long = cos(LLH(1));
|
||||
double sin_long = sin(LLH(1));
|
||||
r_eb_e = {(R_E + LLH(2)) * cos_lat * cos_long,
|
||||
(R_E + LLH(2)) * cos_lat * sin_long,
|
||||
((1 - e * e) * R_E + LLH(2)) * sin_lat};
|
||||
|
||||
//Calculate ECEF to Geo coordinate transformation matrix using (2.150)
|
||||
//C++11 and arma>=5.2
|
||||
// arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat},
|
||||
// {-sin_long, cos_long, 0},
|
||||
// {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}};
|
||||
//C++98 arma <5.2
|
||||
//Calculate ECEF to Geo coordinate transformation matrix using (2.150)
|
||||
arma::mat C_e_n = arma::zeros(3, 3);
|
||||
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
|
||||
<< -sin_long << cos_long << 0 << arma::endr
|
||||
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr;
|
||||
|
||||
// Transform velocity using (2.73)
|
||||
v_eb_e = C_e_n.t() * v_eb_n;
|
||||
|
||||
// Transform attitude using (2.15)
|
||||
C_b_e = C_e_n.t() * C_b_n;
|
||||
}
|
||||
|
||||
|
||||
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, arma::vec v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e)
|
||||
{
|
||||
//% Parameters
|
||||
double R_0 = 6378137; //WGS84 Equatorial radius in meters
|
||||
double e = 0.0818191908425; //WGS84 eccentricity
|
||||
|
||||
// Calculate transverse radius of curvature using (2.105)
|
||||
double R_E = R_0 / sqrt(1 - pow(e * sin(L_b), 2));
|
||||
|
||||
// Convert position using (2.112)
|
||||
double cos_lat = cos(L_b);
|
||||
double sin_lat = sin(L_b);
|
||||
double cos_long = cos(lambda_b);
|
||||
double sin_long = sin(lambda_b);
|
||||
r_eb_e = {(R_E + h_b) * cos_lat * cos_long,
|
||||
(R_E + h_b) * cos_lat * sin_long,
|
||||
((1 - pow(e, 2)) * R_E + h_b) * sin_lat};
|
||||
|
||||
// Calculate ECEF to Geo coordinate transformation matrix using (2.150)
|
||||
arma::mat C_e_n = arma::zeros(3, 3);
|
||||
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
|
||||
<< -sin_long << cos_long << 0 << arma::endr
|
||||
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr;
|
||||
|
||||
// Transform velocity using (2.73)
|
||||
v_eb_e = C_e_n.t() * v_eb_n;
|
||||
}
|
||||
156
src/tests/system-tests/libs/geofunctions.h
Normal file
156
src/tests/system-tests/libs/geofunctions.h
Normal file
@ -0,0 +1,156 @@
|
||||
/*!
|
||||
* \file geofunctions.h
|
||||
* \brief A set of coordinate transformations functions and helpers,
|
||||
* some of them migrated from MATLAB, for geographic information systems.
|
||||
* \author Javier Arribas, 2018. jarribas(at)cttc.es
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GEOFUNCTIONS_H
|
||||
#define GEOFUNCTIONS_H
|
||||
|
||||
#include <armadillo>
|
||||
// %Skew_symmetric - Calculates skew-symmetric matrix
|
||||
arma::mat Skew_symmetric(arma::vec a);
|
||||
|
||||
double WGS84_g0(double Lat_rad);
|
||||
|
||||
double WGS84_geocentric_radius(double Lat_geodetic_rad);
|
||||
|
||||
/* Transformation of vector dx into topocentric coordinate
|
||||
system with origin at x
|
||||
Inputs:
|
||||
x - vector origin coordinates (in ECEF system [X; Y; Z;])
|
||||
dx - vector ([dX; dY; dZ;]).
|
||||
|
||||
Outputs:
|
||||
D - vector length. Units like the input
|
||||
Az - azimuth from north positive clockwise, degrees
|
||||
El - elevation angle, degrees
|
||||
|
||||
Based on a Matlab function by Kai Borre
|
||||
*/
|
||||
int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx);
|
||||
|
||||
/* Subroutine to calculate geodetic coordinates latitude, longitude,
|
||||
height given Cartesian coordinates X,Y,Z, and reference ellipsoid
|
||||
values semi-major axis (a) and the inverse of flattening (finv).
|
||||
|
||||
The output units of angular quantities will be in decimal degrees
|
||||
(15.5 degrees not 15 deg 30 min). The output units of h will be the
|
||||
same as the units of X,Y,Z,a.
|
||||
|
||||
Inputs:
|
||||
a - semi-major axis of the reference ellipsoid
|
||||
finv - inverse of flattening of the reference ellipsoid
|
||||
X,Y,Z - Cartesian coordinates
|
||||
|
||||
Outputs:
|
||||
dphi - latitude
|
||||
dlambda - longitude
|
||||
h - height above reference ellipsoid
|
||||
|
||||
Based in a Matlab function by Kai Borre
|
||||
*/
|
||||
int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
|
||||
|
||||
|
||||
//Gravitation_ECI - Calculates acceleration due to gravity resolved about
|
||||
//ECEF-frame
|
||||
arma::mat Gravity_ECEF(arma::vec r_eb_e);
|
||||
|
||||
/* Conversion of Cartesian coordinates (X,Y,Z) to geographical
|
||||
coordinates (latitude, longitude, h) on a selected reference ellipsoid.
|
||||
|
||||
Choices of Reference Ellipsoid for Geographical Coordinates
|
||||
0. International Ellipsoid 1924
|
||||
1. International Ellipsoid 1967
|
||||
2. World Geodetic System 1972
|
||||
3. Geodetic Reference System 1980
|
||||
4. World Geodetic System 1984
|
||||
*/
|
||||
arma::vec cart2geo(arma::vec XYZ, int elipsoid_selection);
|
||||
|
||||
arma::vec LLH_to_deg(arma::vec LLH);
|
||||
|
||||
double degtorad(double angleInDegrees);
|
||||
|
||||
double radtodeg(double angleInRadians);
|
||||
|
||||
double mstoknotsh(double MetersPerSeconds);
|
||||
|
||||
double mstokph(double Kph);
|
||||
|
||||
|
||||
arma::vec CTM_to_Euler(arma::mat C);
|
||||
|
||||
arma::mat Euler_to_CTM(arma::vec eul);
|
||||
|
||||
void ECEF_to_Geo(arma::vec r_eb_e, arma::vec v_eb_e, arma::mat C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n);
|
||||
|
||||
|
||||
// %
|
||||
// % Inputs:
|
||||
// % L_b latitude (rad)
|
||||
// % lambda_b longitude (rad)
|
||||
// % h_b height (m)
|
||||
// % v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along
|
||||
// % north, east, and down (m/s)
|
||||
// % C_b_n body-to-NED coordinate transformation matrix
|
||||
// %
|
||||
// % Outputs:
|
||||
// % r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved
|
||||
// % along ECEF-frame axes (m)
|
||||
// % v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along
|
||||
// % ECEF-frame axes (m/s)
|
||||
// % C_b_e body-to-ECEF-frame coordinate transformation matrix
|
||||
//
|
||||
// % Copyright 2012, Paul Groves
|
||||
// % License: BSD; see license.txt for details
|
||||
|
||||
void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e);
|
||||
|
||||
|
||||
//pv_Geo_to_ECEF - Converts curvilinear to Cartesian position and velocity
|
||||
//resolving axes from NED to ECEF
|
||||
//This function created 11/4/2012 by Paul Groves
|
||||
//%
|
||||
//% Inputs:
|
||||
//% L_b latitude (rad)
|
||||
//% lambda_b longitude (rad)
|
||||
//% h_b height (m)
|
||||
//% v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along
|
||||
//% north, east, and down (m/s)
|
||||
//%
|
||||
//% Outputs:
|
||||
//% r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved
|
||||
//% along ECEF-frame axes (m)
|
||||
//% v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along
|
||||
//% ECEF-frame axes (m/s)
|
||||
|
||||
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, arma::vec v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e);
|
||||
|
||||
#endif
|
||||
46
src/tests/system-tests/libs/position_test_flags.h
Normal file
46
src/tests/system-tests/libs/position_test_flags.h
Normal file
@ -0,0 +1,46 @@
|
||||
/*!
|
||||
* \file signal_generator_flags.h
|
||||
* \brief Helper file for unit testing
|
||||
* \author Javier Arribas, 2018. jarribas(at)cttc.es
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_POSITION_TEST_FLAGS_H_
|
||||
#define GNSS_SDR_POSITION_TEST_FLAGS_H_
|
||||
|
||||
#include <gflags/gflags.h>
|
||||
#include <limits>
|
||||
|
||||
DEFINE_string(config_file_ptest, std::string(""), "File containing the configuration parameters for the position test.");
|
||||
DEFINE_bool(plot_position_test, false, "Plots results of with gnuplot");
|
||||
DEFINE_bool(static_scenario, true, "Compute figures of merit for static user position (DRMS, CEP, etc..)");
|
||||
DEFINE_bool(use_pvt_solver_dump, false, "Use PVT solver binary dump or fall back to KML PVT file (contains only position information)");
|
||||
DEFINE_bool(use_ref_motion_file, false, "Enable or disable the use of a reference file containing the true receiver position, velocity and acceleration.");
|
||||
DEFINE_int32(ref_motion_file_type, 1, "Type of reference motion file: 1- Spirent CSV motion file");
|
||||
DEFINE_string(ref_motion_filename, std::string("motion.csv"), "Path and filename for the reference motion file");
|
||||
DEFINE_string(pvt_solver_dump_filename, std::string("PVT_pvt.dat"), "Path and filename for the PVT solver binary dump file");
|
||||
|
||||
#endif
|
||||
153
src/tests/system-tests/libs/rtklib_solver_dump_reader.cc
Normal file
153
src/tests/system-tests/libs/rtklib_solver_dump_reader.cc
Normal file
@ -0,0 +1,153 @@
|
||||
/*!
|
||||
* \file rtklib_solver_dump_reader.cc
|
||||
* \brief Helper file for unit testing
|
||||
* \author Javier Arribas, 2017. jarribas(at)cttc.es
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "rtklib_solver_dump_reader.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
bool rtklib_solver_dump_reader::read_binary_obs()
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_file.read(reinterpret_cast<char *>(&TOW_at_current_symbol_ms), sizeof(TOW_at_current_symbol_ms));
|
||||
// std::cout << "TOW_at_current_symbol_ms: " << TOW_at_current_symbol_ms << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&week), sizeof(week));
|
||||
// std::cout << "week: " << week << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&RX_time), sizeof(RX_time));
|
||||
// std::cout << "RX_time: " << RX_time << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&clk_offset_s), sizeof(clk_offset_s));
|
||||
// std::cout << "clk_offset_s: " << clk_offset_s << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&rr[0]), sizeof(rr));
|
||||
// for (int n = 0; n < 6; n++)
|
||||
// {
|
||||
// std::cout << "rr: " << rr[n] << std::endl;
|
||||
// }
|
||||
d_dump_file.read(reinterpret_cast<char *>(&qr[0]), sizeof(qr));
|
||||
// for (int n = 0; n < 6; n++)
|
||||
// {
|
||||
// std::cout << "qr" << qr[n] << std::endl;
|
||||
// }
|
||||
d_dump_file.read(reinterpret_cast<char *>(&latitude), sizeof(latitude));
|
||||
//std::cout << "latitude: " << latitude << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&longitude), sizeof(longitude));
|
||||
//std::cout << "longitude: " << longitude << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&height), sizeof(height));
|
||||
//std::cout << "height: " << height << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&ns), sizeof(ns));
|
||||
// std::cout << "ns: " << (int)ns << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&status), sizeof(status));
|
||||
// std::cout << "status: " << (int)status << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&type), sizeof(type));
|
||||
// std::cout << "type: " << (int)type << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&AR_ratio), sizeof(AR_ratio));
|
||||
// std::cout << "AR_ratio: " << AR_ratio << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&AR_thres), sizeof(AR_thres));
|
||||
// std::cout << "AR_thres: " << AR_thres << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&dop[0]), sizeof(dop));
|
||||
|
||||
// for (int n = 0; n < 4; n++)
|
||||
// {
|
||||
// std::cout << "dop" << dop[n] << std::endl;
|
||||
// }
|
||||
// getchar();
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool rtklib_solver_dump_reader::restart()
|
||||
{
|
||||
if (d_dump_file.is_open())
|
||||
{
|
||||
d_dump_file.clear();
|
||||
d_dump_file.seekg(0, std::ios::beg);
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int64_t rtklib_solver_dump_reader::num_epochs()
|
||||
{
|
||||
std::ifstream::pos_type size;
|
||||
int epoch_size_bytes = sizeof(TOW_at_current_symbol_ms) + sizeof(week) + sizeof(RX_time) + sizeof(clk_offset_s) + sizeof(rr) + sizeof(qr) + sizeof(latitude) + sizeof(longitude) + sizeof(height) + sizeof(ns) + sizeof(status) + sizeof(type) + sizeof(AR_ratio) + sizeof(AR_thres) + sizeof(dop);
|
||||
|
||||
std::ifstream tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate);
|
||||
if (tmpfile.is_open())
|
||||
{
|
||||
size = tmpfile.tellg();
|
||||
int64_t nepoch = size / epoch_size_bytes;
|
||||
return nepoch;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool rtklib_solver_dump_reader::open_obs_file(std::string out_file)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_filename = out_file;
|
||||
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::in | std::ios::binary);
|
||||
return true;
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
std::cout << "Problem opening rtklib_solver dump Log file: " << d_dump_filename.c_str() << std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
rtklib_solver_dump_reader::~rtklib_solver_dump_reader()
|
||||
{
|
||||
if (d_dump_file.is_open() == true)
|
||||
{
|
||||
d_dump_file.close();
|
||||
}
|
||||
}
|
||||
88
src/tests/system-tests/libs/rtklib_solver_dump_reader.h
Normal file
88
src/tests/system-tests/libs/rtklib_solver_dump_reader.h
Normal file
@ -0,0 +1,88 @@
|
||||
/*!
|
||||
* \file rtklib_solver_dump_reader.h
|
||||
* \brief Helper file for unit testing
|
||||
* \author Javier Arribas, 2017. jarribas(at)cttc.es
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_RTKLIB_SOLVER_DUMP_READER_H
|
||||
#define GNSS_SDR_RTKLIB_SOLVER_DUMP_READER_H
|
||||
|
||||
#include <cstdint>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
class rtklib_solver_dump_reader
|
||||
{
|
||||
public:
|
||||
~rtklib_solver_dump_reader();
|
||||
bool read_binary_obs();
|
||||
bool restart();
|
||||
int64_t num_epochs();
|
||||
bool open_obs_file(std::string out_file);
|
||||
|
||||
//rtklib_solver dump variables
|
||||
// TOW
|
||||
uint32_t TOW_at_current_symbol_ms;
|
||||
// WEEK
|
||||
uint32_t week;
|
||||
// PVT GPS time
|
||||
double RX_time;
|
||||
// User clock offset [s]
|
||||
double clk_offset_s;
|
||||
// ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double)
|
||||
double rr[6];
|
||||
// position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double)
|
||||
float qr[6];
|
||||
|
||||
// GEO user position Latitude [deg]
|
||||
double latitude;
|
||||
// GEO user position Longitude [deg]
|
||||
double longitude;
|
||||
// GEO user position Height [m]
|
||||
double height;
|
||||
|
||||
// NUMBER OF VALID SATS
|
||||
uint8_t ns;
|
||||
// RTKLIB solution status
|
||||
uint8_t status;
|
||||
// RTKLIB solution type (0:xyz-ecef,1:enu-baseline)
|
||||
uint8_t type;
|
||||
//AR ratio factor for validation
|
||||
float AR_ratio;
|
||||
//AR ratio threshold for validation
|
||||
float AR_thres;
|
||||
|
||||
//GDOP//PDOP//HDOP//VDOP
|
||||
double dop[4];
|
||||
|
||||
private:
|
||||
std::string d_dump_filename;
|
||||
std::ifstream d_dump_file;
|
||||
};
|
||||
|
||||
#endif //GNSS_SDR_RTKLIB_SOLVER_DUMP_READER_H
|
||||
205
src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc
Normal file
205
src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc
Normal file
@ -0,0 +1,205 @@
|
||||
/*!
|
||||
* \file spirent_motion_csv_dump_reader.cc
|
||||
* \brief Helper file for unit testing
|
||||
* \author Javier Arribas, 2017. jarribas(at)cttc.es
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "spirent_motion_csv_dump_reader.h"
|
||||
#include <iostream>
|
||||
|
||||
bool spirent_motion_csv_dump_reader::read_csv_obs()
|
||||
{
|
||||
try
|
||||
{
|
||||
std::vector<double> vec;
|
||||
std::string line;
|
||||
if (getline(d_dump_file, line))
|
||||
{
|
||||
boost::tokenizer<boost::escaped_list_separator<char>> tk(
|
||||
line, boost::escaped_list_separator<char>('\\', ',', '\"'));
|
||||
for (boost::tokenizer<boost::escaped_list_separator<char>>::iterator i(
|
||||
tk.begin());
|
||||
i != tk.end(); ++i)
|
||||
{
|
||||
try
|
||||
{
|
||||
vec.push_back(std::stod(*i));
|
||||
}
|
||||
catch (const std::exception &ex)
|
||||
{
|
||||
vec.push_back(0.0);
|
||||
}
|
||||
}
|
||||
parse_vector(vec);
|
||||
}
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool spirent_motion_csv_dump_reader::parse_vector(std::vector<double> &vec)
|
||||
{
|
||||
try
|
||||
{
|
||||
int n = 0;
|
||||
TOW_ms = vec.at(n++);
|
||||
Pos_X = vec.at(n++);
|
||||
Pos_Y = vec.at(n++);
|
||||
Pos_Z = vec.at(n++);
|
||||
Vel_X = vec.at(n++);
|
||||
Vel_Y = vec.at(n++);
|
||||
Vel_Z = vec.at(n++);
|
||||
Acc_X = vec.at(n++);
|
||||
Acc_Y = vec.at(n++);
|
||||
Acc_Z = vec.at(n++);
|
||||
Jerk_X = vec.at(n++);
|
||||
Jerk_Y = vec.at(n++);
|
||||
Jerk_Z = vec.at(n++);
|
||||
Lat = vec.at(n++);
|
||||
Long = vec.at(n++);
|
||||
Height = vec.at(n++);
|
||||
Heading = vec.at(n++);
|
||||
Elevation = vec.at(n++);
|
||||
Bank = vec.at(n++);
|
||||
Ang_vel_X = vec.at(n++);
|
||||
Ang_vel_Y = vec.at(n++);
|
||||
Ang_vel_Z = vec.at(n++);
|
||||
Ang_acc_X = vec.at(n++);
|
||||
Ang_acc_Y = vec.at(n++);
|
||||
Ang_acc_Z = vec.at(n++);
|
||||
Ant1_Pos_X = vec.at(n++);
|
||||
Ant1_Pos_Y = vec.at(n++);
|
||||
Ant1_Pos_Z = vec.at(n++);
|
||||
Ant1_Vel_X = vec.at(n++);
|
||||
Ant1_Vel_Y = vec.at(n++);
|
||||
Ant1_Vel_Z = vec.at(n++);
|
||||
Ant1_Acc_X = vec.at(n++);
|
||||
Ant1_Acc_Y = vec.at(n++);
|
||||
Ant1_Acc_Z = vec.at(n++);
|
||||
Ant1_Lat = vec.at(n++);
|
||||
Ant1_Long = vec.at(n++);
|
||||
Ant1_Height = vec.at(n++);
|
||||
Ant1_DOP = vec.at(n++);
|
||||
return true;
|
||||
}
|
||||
catch (const std::exception &ex)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
bool spirent_motion_csv_dump_reader::restart()
|
||||
{
|
||||
if (d_dump_file.is_open())
|
||||
{
|
||||
d_dump_file.clear();
|
||||
d_dump_file.seekg(0, std::ios::beg);
|
||||
std::string line;
|
||||
for (int n = 0; n < header_lines; n++)
|
||||
{
|
||||
getline(d_dump_file, line);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int64_t spirent_motion_csv_dump_reader::num_epochs()
|
||||
{
|
||||
int64_t nepoch = 0;
|
||||
std::string line;
|
||||
std::ifstream tmpfile(d_dump_filename.c_str());
|
||||
if (tmpfile.is_open())
|
||||
{
|
||||
while (std::getline(tmpfile, line))
|
||||
{
|
||||
++nepoch;
|
||||
}
|
||||
return nepoch - header_lines;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool spirent_motion_csv_dump_reader::open_obs_file(std::string out_file)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_filename = out_file;
|
||||
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
|
||||
d_dump_file.open(d_dump_filename.c_str());
|
||||
std::string line;
|
||||
for (int n = 0; n < header_lines; n++)
|
||||
{
|
||||
getline(d_dump_file, line);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
std::cout << "Problem opening Spirent CSV dump Log file: " << d_dump_filename.c_str() << std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void spirent_motion_csv_dump_reader::close_obs_file()
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
d_dump_file.close();
|
||||
}
|
||||
}
|
||||
|
||||
spirent_motion_csv_dump_reader::spirent_motion_csv_dump_reader()
|
||||
{
|
||||
header_lines = 2;
|
||||
}
|
||||
|
||||
|
||||
spirent_motion_csv_dump_reader::~spirent_motion_csv_dump_reader()
|
||||
{
|
||||
if (d_dump_file.is_open() == true)
|
||||
{
|
||||
d_dump_file.close();
|
||||
}
|
||||
}
|
||||
98
src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h
Normal file
98
src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h
Normal file
@ -0,0 +1,98 @@
|
||||
/*!
|
||||
* \file spirent_motion_csv_dump_reader.h
|
||||
* \brief Helper file for unit testing
|
||||
* \author Javier Arribas, 2018. jarribas(at)cttc.es
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_spirent_motion_csv_dump_READER_H
|
||||
#define GNSS_SDR_spirent_motion_csv_dump_READER_H
|
||||
|
||||
#include <boost/tokenizer.hpp>
|
||||
#include <cstdint>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
class spirent_motion_csv_dump_reader
|
||||
{
|
||||
public:
|
||||
spirent_motion_csv_dump_reader();
|
||||
~spirent_motion_csv_dump_reader();
|
||||
bool read_csv_obs();
|
||||
bool restart();
|
||||
int64_t num_epochs();
|
||||
bool open_obs_file(std::string out_file);
|
||||
void close_obs_file();
|
||||
|
||||
int header_lines;
|
||||
//dump variables
|
||||
double TOW_ms;
|
||||
double Pos_X;
|
||||
double Pos_Y;
|
||||
double Pos_Z;
|
||||
double Vel_X;
|
||||
double Vel_Y;
|
||||
double Vel_Z;
|
||||
double Acc_X;
|
||||
double Acc_Y;
|
||||
double Acc_Z;
|
||||
double Jerk_X;
|
||||
double Jerk_Y;
|
||||
double Jerk_Z;
|
||||
double Lat;
|
||||
double Long;
|
||||
double Height;
|
||||
double Heading;
|
||||
double Elevation;
|
||||
double Bank;
|
||||
double Ang_vel_X;
|
||||
double Ang_vel_Y;
|
||||
double Ang_vel_Z;
|
||||
double Ang_acc_X;
|
||||
double Ang_acc_Y;
|
||||
double Ang_acc_Z;
|
||||
double Ant1_Pos_X;
|
||||
double Ant1_Pos_Y;
|
||||
double Ant1_Pos_Z;
|
||||
double Ant1_Vel_X;
|
||||
double Ant1_Vel_Y;
|
||||
double Ant1_Vel_Z;
|
||||
double Ant1_Acc_X;
|
||||
double Ant1_Acc_Y;
|
||||
double Ant1_Acc_Z;
|
||||
double Ant1_Lat;
|
||||
double Ant1_Long;
|
||||
double Ant1_Height;
|
||||
double Ant1_DOP;
|
||||
|
||||
private:
|
||||
std::string d_dump_filename;
|
||||
std::ifstream d_dump_file;
|
||||
bool parse_vector(std::vector<double> &vec);
|
||||
};
|
||||
|
||||
#endif //GNSS_SDR_spirent_motion_csv_dump_READER_H
|
||||
@ -1,719 +0,0 @@
|
||||
/*!
|
||||
* \file obs_gps_l1_system_test.cc
|
||||
* \brief This class implements a test for the validation of generated observables.
|
||||
* \author Carles Fernandez-Prades, 2016. cfernandez(at)cttc.es
|
||||
*
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "concurrent_map.h"
|
||||
#include "concurrent_queue.h"
|
||||
#include "control_thread.h"
|
||||
#include "in_memory_configuration.h"
|
||||
#include "signal_generator_flags.h"
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <gpstk/RinexUtilities.hpp>
|
||||
#include <gpstk/Rinex3ObsBase.hpp>
|
||||
#include <gpstk/Rinex3ObsData.hpp>
|
||||
#include <gpstk/Rinex3ObsHeader.hpp>
|
||||
#include <gpstk/Rinex3ObsStream.hpp>
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cstdlib>
|
||||
#include <exception>
|
||||
#include <iostream>
|
||||
#include <numeric>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <unistd.h>
|
||||
|
||||
|
||||
// For GPS NAVIGATION (L1)
|
||||
concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
|
||||
concurrent_map<Gps_Acq_Assist> global_gps_acq_assist_map;
|
||||
|
||||
|
||||
class ObsGpsL1SystemTest : public ::testing::Test
|
||||
{
|
||||
public:
|
||||
std::string generator_binary;
|
||||
std::string p1;
|
||||
std::string p2;
|
||||
std::string p3;
|
||||
std::string p4;
|
||||
std::string p5;
|
||||
|
||||
const double baseband_sampling_freq = 2.6e6;
|
||||
|
||||
std::string filename_rinex_obs = FLAGS_filename_rinex_obs;
|
||||
std::string filename_raw_data = FLAGS_filename_raw_data;
|
||||
std::string generated_rinex_obs;
|
||||
int configure_generator();
|
||||
int generate_signal();
|
||||
int configure_receiver();
|
||||
int run_receiver();
|
||||
void check_results();
|
||||
bool check_valid_rinex_nav(std::string filename); // return true if the file is a valid Rinex navigation file.
|
||||
bool check_valid_rinex_obs(std::string filename); // return true if the file is a valid Rinex observation file.
|
||||
double compute_stdev(const std::vector<double>& vec);
|
||||
|
||||
std::shared_ptr<InMemoryConfiguration> config;
|
||||
};
|
||||
|
||||
|
||||
bool ObsGpsL1SystemTest::check_valid_rinex_nav(std::string filename)
|
||||
{
|
||||
bool res = false;
|
||||
res = gpstk::isRinexNavFile(filename);
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
double ObsGpsL1SystemTest::compute_stdev(const std::vector<double>& vec)
|
||||
{
|
||||
double sum__ = std::accumulate(vec.begin(), vec.end(), 0.0);
|
||||
double mean__ = sum__ / vec.size();
|
||||
double accum__ = 0.0;
|
||||
std::for_each(std::begin(vec), std::end(vec), [&](const double d) {
|
||||
accum__ += (d - mean__) * (d - mean__);
|
||||
});
|
||||
double stdev__ = std::sqrt(accum__ / (vec.size() - 1));
|
||||
return stdev__;
|
||||
}
|
||||
|
||||
|
||||
bool ObsGpsL1SystemTest::check_valid_rinex_obs(std::string filename)
|
||||
{
|
||||
bool res = false;
|
||||
res = gpstk::isRinexObsFile(filename);
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
int ObsGpsL1SystemTest::configure_generator()
|
||||
{
|
||||
// Configure signal generator
|
||||
generator_binary = FLAGS_generator_binary;
|
||||
|
||||
p1 = std::string("-rinex_nav_file=") + FLAGS_rinex_nav_file;
|
||||
if (FLAGS_dynamic_position.empty())
|
||||
{
|
||||
p2 = std::string("-static_position=") + FLAGS_static_position + std::string(",") + std::to_string(std::min(FLAGS_duration * 10, 3000));
|
||||
if (FLAGS_duration > 300) std::cout << "WARNING: Duration has been set to its maximum value of 300 s" << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
p2 = std::string("-obs_pos_file=") + std::string(FLAGS_dynamic_position);
|
||||
}
|
||||
p3 = std::string("-rinex_obs_file=") + FLAGS_filename_rinex_obs; // RINEX 2.10 observation file output
|
||||
p4 = std::string("-sig_out_file=") + FLAGS_filename_raw_data; // Baseband signal output file. Will be stored in int8_t IQ multiplexed samples
|
||||
p5 = std::string("-sampling_freq=") + std::to_string(baseband_sampling_freq); //Baseband sampling frequency [MSps]
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int ObsGpsL1SystemTest::generate_signal()
|
||||
{
|
||||
pid_t wait_result;
|
||||
int child_status;
|
||||
|
||||
char* const parmList[] = {&generator_binary[0], &generator_binary[0], &p1[0], &p2[0], &p3[0], &p4[0], &p5[0], NULL};
|
||||
|
||||
int pid;
|
||||
if ((pid = fork()) == -1)
|
||||
perror("fork error");
|
||||
else if (pid == 0)
|
||||
{
|
||||
execv(&generator_binary[0], parmList);
|
||||
std::cout << "Return not expected. Must be an execv error." << std::endl;
|
||||
std::terminate();
|
||||
}
|
||||
|
||||
wait_result = waitpid(pid, &child_status, 0);
|
||||
if (wait_result == -1) perror("waitpid error");
|
||||
EXPECT_EQ(true, check_valid_rinex_obs(filename_rinex_obs));
|
||||
std::cout << "Signal and Observables RINEX files created." << std::endl;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int ObsGpsL1SystemTest::configure_receiver()
|
||||
{
|
||||
config = std::make_shared<InMemoryConfiguration>();
|
||||
|
||||
const int sampling_rate_internal = baseband_sampling_freq;
|
||||
|
||||
const int number_of_taps = 11;
|
||||
const int number_of_bands = 2;
|
||||
const float band1_begin = 0.0;
|
||||
const float band1_end = 0.48;
|
||||
const float band2_begin = 0.52;
|
||||
const float band2_end = 1.0;
|
||||
const float ampl1_begin = 1.0;
|
||||
const float ampl1_end = 1.0;
|
||||
const float ampl2_begin = 0.0;
|
||||
const float ampl2_end = 0.0;
|
||||
const float band1_error = 1.0;
|
||||
const float band2_error = 1.0;
|
||||
const int grid_density = 16;
|
||||
|
||||
const float zero = 0.0;
|
||||
const int number_of_channels = 8;
|
||||
const int in_acquisition = 1;
|
||||
|
||||
const float threshold = 0.01;
|
||||
const float doppler_max = 8000.0;
|
||||
const float doppler_step = 500.0;
|
||||
const int max_dwells = 1;
|
||||
const int tong_init_val = 2;
|
||||
const int tong_max_val = 10;
|
||||
const int tong_max_dwells = 30;
|
||||
const int coherent_integration_time_ms = 1;
|
||||
|
||||
const float pll_bw_hz = 30.0;
|
||||
const float dll_bw_hz = 4.0;
|
||||
const float early_late_space_chips = 0.5;
|
||||
const float pll_bw_narrow_hz = 20.0;
|
||||
const float dll_bw_narrow_hz = 2.0;
|
||||
const int extend_correlation_ms = 1;
|
||||
|
||||
const int display_rate_ms = 500;
|
||||
const int output_rate_ms = 10;
|
||||
|
||||
config->set_property("GNSS-SDR.internal_fs_sps", std::to_string(sampling_rate_internal));
|
||||
|
||||
// Set the assistance system parameters
|
||||
config->set_property("GNSS-SDR.SUPL_read_gps_assistance_xml", "false");
|
||||
config->set_property("GNSS-SDR.SUPL_gps_enabled", "false");
|
||||
config->set_property("GNSS-SDR.SUPL_gps_ephemeris_server", "supl.google.com");
|
||||
config->set_property("GNSS-SDR.SUPL_gps_ephemeris_port", std::to_string(7275));
|
||||
config->set_property("GNSS-SDR.SUPL_gps_acquisition_server", "supl.google.com");
|
||||
config->set_property("GNSS-SDR.SUPL_gps_acquisition_port", std::to_string(7275));
|
||||
config->set_property("GNSS-SDR.SUPL_MCC", std::to_string(244));
|
||||
config->set_property("GNSS-SDR.SUPL_MNS", std::to_string(5));
|
||||
config->set_property("GNSS-SDR.SUPL_LAC", "0x59e2");
|
||||
config->set_property("GNSS-SDR.SUPL_CI", "0x31b0");
|
||||
|
||||
// Set the Signal Source
|
||||
config->set_property("SignalSource.implementation", "File_Signal_Source");
|
||||
config->set_property("SignalSource.filename", "./" + filename_raw_data);
|
||||
config->set_property("SignalSource.sampling_frequency", std::to_string(sampling_rate_internal));
|
||||
config->set_property("SignalSource.item_type", "ibyte");
|
||||
config->set_property("SignalSource.samples", std::to_string(zero));
|
||||
|
||||
// Set the Signal Conditioner
|
||||
config->set_property("SignalConditioner.implementation", "Signal_Conditioner");
|
||||
config->set_property("DataTypeAdapter.implementation", "Ibyte_To_Complex");
|
||||
config->set_property("InputFilter.implementation", "Fir_Filter");
|
||||
config->set_property("InputFilter.dump", "false");
|
||||
config->set_property("InputFilter.input_item_type", "gr_complex");
|
||||
config->set_property("InputFilter.output_item_type", "gr_complex");
|
||||
config->set_property("InputFilter.taps_item_type", "float");
|
||||
config->set_property("InputFilter.number_of_taps", std::to_string(number_of_taps));
|
||||
config->set_property("InputFilter.number_of_bands", std::to_string(number_of_bands));
|
||||
config->set_property("InputFilter.band1_begin", std::to_string(band1_begin));
|
||||
config->set_property("InputFilter.band1_end", std::to_string(band1_end));
|
||||
config->set_property("InputFilter.band2_begin", std::to_string(band2_begin));
|
||||
config->set_property("InputFilter.band2_end", std::to_string(band2_end));
|
||||
config->set_property("InputFilter.ampl1_begin", std::to_string(ampl1_begin));
|
||||
config->set_property("InputFilter.ampl1_end", std::to_string(ampl1_end));
|
||||
config->set_property("InputFilter.ampl2_begin", std::to_string(ampl2_begin));
|
||||
config->set_property("InputFilter.ampl2_end", std::to_string(ampl2_end));
|
||||
config->set_property("InputFilter.band1_error", std::to_string(band1_error));
|
||||
config->set_property("InputFilter.band2_error", std::to_string(band2_error));
|
||||
config->set_property("InputFilter.filter_type", "bandpass");
|
||||
config->set_property("InputFilter.grid_density", std::to_string(grid_density));
|
||||
config->set_property("InputFilter.sampling_frequency", std::to_string(sampling_rate_internal));
|
||||
config->set_property("InputFilter.IF", std::to_string(zero));
|
||||
config->set_property("Resampler.implementation", "Pass_Through");
|
||||
config->set_property("Resampler.dump", "false");
|
||||
config->set_property("Resampler.item_type", "gr_complex");
|
||||
config->set_property("Resampler.sample_freq_in", std::to_string(sampling_rate_internal));
|
||||
config->set_property("Resampler.sample_freq_out", std::to_string(sampling_rate_internal));
|
||||
|
||||
// Set the number of Channels
|
||||
config->set_property("Channels_1C.count", std::to_string(number_of_channels));
|
||||
config->set_property("Channels.in_acquisition", std::to_string(in_acquisition));
|
||||
config->set_property("Channel.signal", "1C");
|
||||
|
||||
// Set Acquisition
|
||||
config->set_property("Acquisition_1C.implementation", "GPS_L1_CA_PCPS_Tong_Acquisition");
|
||||
config->set_property("Acquisition_1C.item_type", "gr_complex");
|
||||
config->set_property("Acquisition_1C.coherent_integration_time_ms", std::to_string(coherent_integration_time_ms));
|
||||
config->set_property("Acquisition_1C.threshold", std::to_string(threshold));
|
||||
config->set_property("Acquisition_1C.doppler_max", std::to_string(doppler_max));
|
||||
config->set_property("Acquisition_1C.doppler_step", std::to_string(doppler_step));
|
||||
config->set_property("Acquisition_1C.bit_transition_flag", "false");
|
||||
config->set_property("Acquisition_1C.max_dwells", std::to_string(max_dwells));
|
||||
config->set_property("Acquisition_1C.tong_init_val", std::to_string(tong_init_val));
|
||||
config->set_property("Acquisition_1C.tong_max_val", std::to_string(tong_max_val));
|
||||
config->set_property("Acquisition_1C.tong_max_dwells", std::to_string(tong_max_dwells));
|
||||
|
||||
// Set Tracking
|
||||
config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_Tracking");
|
||||
//config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_C_Aid_Tracking");
|
||||
config->set_property("Tracking_1C.item_type", "gr_complex");
|
||||
config->set_property("Tracking_1C.dump", "false");
|
||||
config->set_property("Tracking_1C.dump_filename", "./tracking_ch_");
|
||||
config->set_property("Tracking_1C.pll_bw_hz", std::to_string(pll_bw_hz));
|
||||
config->set_property("Tracking_1C.dll_bw_hz", std::to_string(dll_bw_hz));
|
||||
config->set_property("Tracking_1C.early_late_space_chips", std::to_string(early_late_space_chips));
|
||||
|
||||
config->set_property("Tracking_1C.pll_bw_narrow_hz", std::to_string(pll_bw_narrow_hz));
|
||||
config->set_property("Tracking_1C.dll_bw_narrow_hz", std::to_string(dll_bw_narrow_hz));
|
||||
config->set_property("Tracking_1C.extend_correlation_ms", std::to_string(extend_correlation_ms));
|
||||
|
||||
// Set Telemetry
|
||||
config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder");
|
||||
config->set_property("TelemetryDecoder_1C.dump", "false");
|
||||
|
||||
// Set Observables
|
||||
config->set_property("Observables.implementation", "Hybrid_Observables");
|
||||
config->set_property("Observables.dump", "false");
|
||||
config->set_property("Observables.dump_filename", "./observables.dat");
|
||||
config->set_property("Observables.averaging_depth", std::to_string(100));
|
||||
|
||||
// Set PVT
|
||||
config->set_property("PVT.implementation", "RTKLIB_PVT");
|
||||
config->set_property("PVT.positioning_mode", "Single");
|
||||
config->set_property("PVT.output_rate_ms", std::to_string(output_rate_ms));
|
||||
config->set_property("PVT.display_rate_ms", std::to_string(display_rate_ms));
|
||||
config->set_property("PVT.dump_filename", "./PVT");
|
||||
config->set_property("PVT.nmea_dump_filename", "./gnss_sdr_pvt.nmea");
|
||||
config->set_property("PVT.flag_nmea_tty_port", "false");
|
||||
config->set_property("PVT.nmea_dump_devname", "/dev/pts/4");
|
||||
config->set_property("PVT.flag_rtcm_server", "false");
|
||||
config->set_property("PVT.flag_rtcm_tty_port", "false");
|
||||
config->set_property("PVT.rtcm_dump_devname", "/dev/pts/1");
|
||||
config->set_property("PVT.dump", "false");
|
||||
config->set_property("PVT.rinex_version", std::to_string(2));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int ObsGpsL1SystemTest::run_receiver()
|
||||
{
|
||||
std::shared_ptr<ControlThread> control_thread;
|
||||
control_thread = std::make_shared<ControlThread>(config);
|
||||
// start receiver
|
||||
try
|
||||
{
|
||||
control_thread->run();
|
||||
}
|
||||
catch (const boost::exception& e)
|
||||
{
|
||||
std::cout << "Boost exception: " << boost::diagnostic_information(e);
|
||||
}
|
||||
catch (const std::exception& ex)
|
||||
{
|
||||
std::cout << "STD exception: " << ex.what();
|
||||
}
|
||||
// Get the name of the RINEX obs file generated by the receiver
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
|
||||
FILE* fp;
|
||||
std::string argum2 = std::string("/bin/ls *O | grep GSDR | tail -1");
|
||||
char buffer[1035];
|
||||
fp = popen(&argum2[0], "r");
|
||||
if (fp == NULL)
|
||||
{
|
||||
std::cout << "Failed to run command: " << argum2 << std::endl;
|
||||
return -1;
|
||||
}
|
||||
while (fgets(buffer, sizeof(buffer), fp) != NULL)
|
||||
{
|
||||
std::string aux = std::string(buffer);
|
||||
ObsGpsL1SystemTest::generated_rinex_obs = aux.erase(aux.length() - 1, 1);
|
||||
}
|
||||
pclose(fp);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void ObsGpsL1SystemTest::check_results()
|
||||
{
|
||||
std::vector<std::vector<std::pair<double, double>>> pseudorange_ref(33);
|
||||
std::vector<std::vector<std::pair<double, double>>> carrierphase_ref(33);
|
||||
std::vector<std::vector<std::pair<double, double>>> doppler_ref(33);
|
||||
|
||||
std::vector<std::vector<std::pair<double, double>>> pseudorange_meas(33);
|
||||
std::vector<std::vector<std::pair<double, double>>> carrierphase_meas(33);
|
||||
std::vector<std::vector<std::pair<double, double>>> doppler_meas(33);
|
||||
|
||||
// Open and read reference RINEX observables file
|
||||
try
|
||||
{
|
||||
gpstk::Rinex3ObsStream r_ref(FLAGS_filename_rinex_obs);
|
||||
r_ref.exceptions(std::ios::failbit);
|
||||
gpstk::Rinex3ObsData r_ref_data;
|
||||
gpstk::Rinex3ObsHeader r_ref_header;
|
||||
|
||||
gpstk::RinexDatum dataobj;
|
||||
|
||||
r_ref >> r_ref_header;
|
||||
|
||||
while (r_ref >> r_ref_data)
|
||||
{
|
||||
for (int myprn = 1; myprn < 33; myprn++)
|
||||
{
|
||||
gpstk::SatID prn(myprn, gpstk::SatID::systemGPS);
|
||||
gpstk::CommonTime time = r_ref_data.time;
|
||||
double sow(static_cast<gpstk::GPSWeekSecond>(time).sow);
|
||||
|
||||
gpstk::Rinex3ObsData::DataMap::iterator pointer = r_ref_data.obs.find(prn);
|
||||
if (pointer == r_ref_data.obs.end())
|
||||
{
|
||||
// PRN not present; do nothing
|
||||
}
|
||||
else
|
||||
{
|
||||
dataobj = r_ref_data.getObs(prn, "C1C", r_ref_header);
|
||||
double P1 = dataobj.data;
|
||||
std::pair<double, double> pseudo(sow, P1);
|
||||
pseudorange_ref.at(myprn).push_back(pseudo);
|
||||
|
||||
dataobj = r_ref_data.getObs(prn, "L1C", r_ref_header);
|
||||
double L1 = dataobj.data;
|
||||
std::pair<double, double> carrier(sow, L1);
|
||||
carrierphase_ref.at(myprn).push_back(carrier);
|
||||
|
||||
dataobj = r_ref_data.getObs(prn, "D1C", r_ref_header);
|
||||
double D1 = dataobj.data;
|
||||
std::pair<double, double> doppler(sow, D1);
|
||||
doppler_ref.at(myprn).push_back(doppler);
|
||||
} // End of 'if( pointer == roe.obs.end() )'
|
||||
} // end for
|
||||
} // end while
|
||||
} // End of 'try' block
|
||||
catch (const gpstk::FFStreamError& e)
|
||||
{
|
||||
std::cout << e;
|
||||
exit(1);
|
||||
}
|
||||
catch (const gpstk::Exception& e)
|
||||
{
|
||||
std::cout << e;
|
||||
exit(1);
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
std::cout << "unknown error. I don't feel so well..." << std::endl;
|
||||
exit(1);
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
std::string arg2_gen = std::string("./") + ObsGpsL1SystemTest::generated_rinex_obs;
|
||||
gpstk::Rinex3ObsStream r_meas(arg2_gen);
|
||||
r_meas.exceptions(std::ios::failbit);
|
||||
gpstk::Rinex3ObsData r_meas_data;
|
||||
gpstk::Rinex3ObsHeader r_meas_header;
|
||||
gpstk::RinexDatum dataobj;
|
||||
|
||||
r_meas >> r_meas_header;
|
||||
|
||||
while (r_meas >> r_meas_data)
|
||||
{
|
||||
for (int myprn = 1; myprn < 33; myprn++)
|
||||
{
|
||||
gpstk::SatID prn(myprn, gpstk::SatID::systemGPS);
|
||||
gpstk::CommonTime time = r_meas_data.time;
|
||||
double sow(static_cast<gpstk::GPSWeekSecond>(time).sow);
|
||||
|
||||
gpstk::Rinex3ObsData::DataMap::iterator pointer = r_meas_data.obs.find(prn);
|
||||
if (pointer == r_meas_data.obs.end())
|
||||
{
|
||||
// PRN not present; do nothing
|
||||
}
|
||||
else
|
||||
{
|
||||
dataobj = r_meas_data.getObs(prn, "C1C", r_meas_header);
|
||||
double P1 = dataobj.data;
|
||||
std::pair<double, double> pseudo(sow, P1);
|
||||
pseudorange_meas.at(myprn).push_back(pseudo);
|
||||
|
||||
dataobj = r_meas_data.getObs(prn, "L1C", r_meas_header);
|
||||
double L1 = dataobj.data;
|
||||
std::pair<double, double> carrier(sow, L1);
|
||||
carrierphase_meas.at(myprn).push_back(carrier);
|
||||
|
||||
dataobj = r_meas_data.getObs(prn, "D1C", r_meas_header);
|
||||
double D1 = dataobj.data;
|
||||
std::pair<double, double> doppler(sow, D1);
|
||||
doppler_meas.at(myprn).push_back(doppler);
|
||||
} // End of 'if( pointer == roe.obs.end() )'
|
||||
} // end for
|
||||
} // end while
|
||||
} // End of 'try' block
|
||||
catch (const gpstk::FFStreamError& e)
|
||||
{
|
||||
std::cout << e;
|
||||
exit(1);
|
||||
}
|
||||
catch (const gpstk::Exception& e)
|
||||
{
|
||||
std::cout << e;
|
||||
exit(1);
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
std::cout << "unknown error. I don't feel so well..." << std::endl;
|
||||
exit(1);
|
||||
}
|
||||
|
||||
// Time alignment
|
||||
std::vector<std::vector<std::pair<double, double>>> pseudorange_ref_aligned(33);
|
||||
std::vector<std::vector<std::pair<double, double>>> carrierphase_ref_aligned(33);
|
||||
std::vector<std::vector<std::pair<double, double>>> doppler_ref_aligned(33);
|
||||
|
||||
std::vector<std::vector<std::pair<double, double>>>::iterator iter;
|
||||
std::vector<std::pair<double, double>>::iterator it;
|
||||
std::vector<std::pair<double, double>>::iterator it2;
|
||||
|
||||
std::vector<std::vector<double>> pr_diff(33);
|
||||
std::vector<std::vector<double>> cp_diff(33);
|
||||
std::vector<std::vector<double>> doppler_diff(33);
|
||||
|
||||
std::vector<std::vector<double>>::iterator iter_diff;
|
||||
std::vector<double>::iterator iter_v;
|
||||
|
||||
int prn_id = 0;
|
||||
for (iter = pseudorange_ref.begin(); iter != pseudorange_ref.end(); iter++)
|
||||
{
|
||||
for (it = iter->begin(); it != iter->end(); it++)
|
||||
{
|
||||
// If a measure exists for this sow, store it
|
||||
for (it2 = pseudorange_meas.at(prn_id).begin(); it2 != pseudorange_meas.at(prn_id).end(); it2++)
|
||||
{
|
||||
if (std::abs(it->first - it2->first) < 0.1) // store measures closer than 10 ms.
|
||||
{
|
||||
pseudorange_ref_aligned.at(prn_id).push_back(*it);
|
||||
pr_diff.at(prn_id).push_back(it->second - it2->second);
|
||||
//std::cout << "Sat " << prn_id << ": " << "PR_ref=" << it->second << " PR_meas=" << it2->second << " Diff:" << it->second - it2->second << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
prn_id++;
|
||||
}
|
||||
|
||||
prn_id = 0;
|
||||
for (iter = carrierphase_ref.begin(); iter != carrierphase_ref.end(); iter++)
|
||||
{
|
||||
for (it = iter->begin(); it != iter->end(); it++)
|
||||
{
|
||||
// If a measure exists for this sow, store it
|
||||
for (it2 = carrierphase_meas.at(prn_id).begin(); it2 != carrierphase_meas.at(prn_id).end(); it2++)
|
||||
{
|
||||
if (std::abs(it->first - it2->first) < 0.1) // store measures closer than 10 ms.
|
||||
{
|
||||
carrierphase_ref_aligned.at(prn_id).push_back(*it);
|
||||
cp_diff.at(prn_id).push_back(it->second - it2->second);
|
||||
// std::cout << "Sat " << prn_id << ": " << "Carrier_ref=" << it->second << " Carrier_meas=" << it2->second << " Diff:" << it->second - it2->second << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
prn_id++;
|
||||
}
|
||||
prn_id = 0;
|
||||
for (iter = doppler_ref.begin(); iter != doppler_ref.end(); iter++)
|
||||
{
|
||||
for (it = iter->begin(); it != iter->end(); it++)
|
||||
{
|
||||
// If a measure exists for this sow, store it
|
||||
for (it2 = doppler_meas.at(prn_id).begin(); it2 != doppler_meas.at(prn_id).end(); it2++)
|
||||
{
|
||||
if (std::abs(it->first - it2->first) < 0.01) // store measures closer than 10 ms.
|
||||
{
|
||||
doppler_ref_aligned.at(prn_id).push_back(*it);
|
||||
doppler_diff.at(prn_id).push_back(it->second - it2->second);
|
||||
}
|
||||
}
|
||||
}
|
||||
prn_id++;
|
||||
}
|
||||
|
||||
// Compute pseudorange error
|
||||
prn_id = 0;
|
||||
std::vector<double> mean_pr_diff_v;
|
||||
for (iter_diff = pr_diff.begin(); iter_diff != pr_diff.end(); iter_diff++)
|
||||
{
|
||||
// For each satellite with reference and measurements aligned in time
|
||||
int number_obs = 0;
|
||||
double mean_diff = 0.0;
|
||||
for (iter_v = iter_diff->begin(); iter_v != iter_diff->end(); iter_v++)
|
||||
{
|
||||
mean_diff = mean_diff + *iter_v;
|
||||
number_obs = number_obs + 1;
|
||||
}
|
||||
if (number_obs > 0)
|
||||
{
|
||||
mean_diff = mean_diff / number_obs;
|
||||
mean_pr_diff_v.push_back(mean_diff);
|
||||
std::cout << "-- Mean pseudorange difference for sat " << prn_id << ": " << mean_diff;
|
||||
double stdev_ = compute_stdev(*iter_diff);
|
||||
std::cout << " +/- " << stdev_;
|
||||
std::cout << " [m]" << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
mean_diff = 0.0;
|
||||
}
|
||||
|
||||
prn_id++;
|
||||
}
|
||||
double stdev_pr = compute_stdev(mean_pr_diff_v);
|
||||
std::cout << "Pseudorange diff error stdev = " << stdev_pr << " [m]" << std::endl;
|
||||
ASSERT_LT(stdev_pr, 10.0);
|
||||
|
||||
// Compute carrier phase error
|
||||
prn_id = 0;
|
||||
std::vector<double> mean_cp_diff_v;
|
||||
for (iter_diff = cp_diff.begin(); iter_diff != cp_diff.end(); iter_diff++)
|
||||
{
|
||||
// For each satellite with reference and measurements aligned in time
|
||||
int number_obs = 0;
|
||||
double mean_diff = 0.0;
|
||||
for (iter_v = iter_diff->begin(); iter_v != iter_diff->end(); iter_v++)
|
||||
{
|
||||
mean_diff = mean_diff + *iter_v;
|
||||
number_obs = number_obs + 1;
|
||||
}
|
||||
if (number_obs > 0)
|
||||
{
|
||||
mean_diff = mean_diff / number_obs;
|
||||
mean_cp_diff_v.push_back(mean_diff);
|
||||
std::cout << "-- Mean carrier phase difference for sat " << prn_id << ": " << mean_diff;
|
||||
double stdev_pr_ = compute_stdev(*iter_diff);
|
||||
std::cout << " +/- " << stdev_pr_ << " whole cycles (19 cm)" << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
mean_diff = 0.0;
|
||||
}
|
||||
|
||||
prn_id++;
|
||||
}
|
||||
|
||||
// Compute Doppler error
|
||||
prn_id = 0;
|
||||
std::vector<double> mean_doppler_v;
|
||||
for (iter_diff = doppler_diff.begin(); iter_diff != doppler_diff.end(); iter_diff++)
|
||||
{
|
||||
// For each satellite with reference and measurements aligned in time
|
||||
int number_obs = 0;
|
||||
double mean_diff = 0.0;
|
||||
for (iter_v = iter_diff->begin(); iter_v != iter_diff->end(); iter_v++)
|
||||
{
|
||||
//std::cout << *iter_v << std::endl;
|
||||
mean_diff = mean_diff + *iter_v;
|
||||
number_obs = number_obs + 1;
|
||||
}
|
||||
if (number_obs > 0)
|
||||
{
|
||||
mean_diff = mean_diff / number_obs;
|
||||
mean_doppler_v.push_back(mean_diff);
|
||||
std::cout << "-- Mean Doppler difference for sat " << prn_id << ": " << mean_diff << " [Hz]" << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
mean_diff = 0.0;
|
||||
}
|
||||
|
||||
prn_id++;
|
||||
}
|
||||
|
||||
double stdev_dp = compute_stdev(mean_doppler_v);
|
||||
std::cout << "Doppler error stdev = " << stdev_dp << " [Hz]" << std::endl;
|
||||
ASSERT_LT(stdev_dp, 10.0);
|
||||
}
|
||||
|
||||
|
||||
TEST_F(ObsGpsL1SystemTest, Observables_system_test)
|
||||
{
|
||||
std::cout << "Validating input RINEX nav file: " << FLAGS_rinex_nav_file << " ..." << std::endl;
|
||||
bool is_rinex_nav_valid = check_valid_rinex_nav(FLAGS_rinex_nav_file);
|
||||
EXPECT_EQ(true, is_rinex_nav_valid) << "The RINEX navigation file " << FLAGS_rinex_nav_file << " is not well formed.";
|
||||
std::cout << "The file is valid." << std::endl;
|
||||
|
||||
// Configure the signal generator
|
||||
configure_generator();
|
||||
|
||||
// Generate signal raw signal samples and observations RINEX file
|
||||
if (!FLAGS_disable_generator)
|
||||
{
|
||||
generate_signal();
|
||||
}
|
||||
|
||||
std::cout << "Validating generated reference RINEX obs file: " << FLAGS_filename_rinex_obs << " ..." << std::endl;
|
||||
bool is_gen_rinex_obs_valid = check_valid_rinex_obs("./" + FLAGS_filename_rinex_obs);
|
||||
EXPECT_EQ(true, is_gen_rinex_obs_valid) << "The RINEX observation file " << FLAGS_filename_rinex_obs << ", generated by gnss-sim, is not well formed.";
|
||||
std::cout << "The file is valid." << std::endl;
|
||||
|
||||
// Configure receiver
|
||||
configure_receiver();
|
||||
|
||||
// Run the receiver
|
||||
EXPECT_EQ(run_receiver(), 0) << "Problem executing the software-defined signal generator";
|
||||
|
||||
std::cout << "Validating RINEX obs file obtained by GNSS-SDR: " << ObsGpsL1SystemTest::generated_rinex_obs << " ..." << std::endl;
|
||||
is_gen_rinex_obs_valid = check_valid_rinex_obs("./" + ObsGpsL1SystemTest::generated_rinex_obs);
|
||||
EXPECT_EQ(true, is_gen_rinex_obs_valid) << "The RINEX observation file " << ObsGpsL1SystemTest::generated_rinex_obs << ", generated by GNSS-SDR, is not well formed.";
|
||||
std::cout << "The file is valid." << std::endl;
|
||||
|
||||
// Check results
|
||||
check_results();
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
std::cout << "Running Observables validation test..." << std::endl;
|
||||
int res = 0;
|
||||
try
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
} // catch the "testing::internal::<unnamed>::ClassUniqueToAlwaysTrue" from gtest
|
||||
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
|
||||
// Run the Tests
|
||||
try
|
||||
{
|
||||
res = RUN_ALL_TESTS();
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
LOG(WARNING) << "Unexpected catch";
|
||||
}
|
||||
google::ShutDownCommandLineFlags();
|
||||
return res;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,7 +1,10 @@
|
||||
/*!
|
||||
* \file position_test.cc
|
||||
* \brief This class implements a test for the validation of computed position.
|
||||
* \author Carles Fernandez-Prades, 2016. cfernandez(at)cttc.es
|
||||
* \authors <ul>
|
||||
* <li> Carles Fernandez-Prades, 2016. cfernandez(at)cttc.es
|
||||
* <li> Javier Arribas, 2018. jarribas(at)cttc.es
|
||||
* </ul>
|
||||
*
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
@ -29,6 +32,9 @@
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "position_test_flags.h"
|
||||
#include "rtklib_solver_dump_reader.h"
|
||||
#include "spirent_motion_csv_dump_reader.h"
|
||||
#include "concurrent_map.h"
|
||||
#include "concurrent_queue.h"
|
||||
#include "control_thread.h"
|
||||
@ -39,6 +45,7 @@
|
||||
#include "test_flags.h"
|
||||
#include "signal_generator_flags.h"
|
||||
#include <boost/filesystem.hpp>
|
||||
#include <armadillo>
|
||||
#include <glog/logging.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <algorithm>
|
||||
@ -48,10 +55,6 @@
|
||||
#include <numeric>
|
||||
#include <thread>
|
||||
|
||||
|
||||
DEFINE_string(config_file_ptest, std::string(""), "File containing the configuration parameters for the position test.");
|
||||
DEFINE_bool(plot_position_test, false, "Plots results of FFTLengthTest with gnuplot");
|
||||
|
||||
// For GPS NAVIGATION (L1)
|
||||
concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
|
||||
concurrent_map<Gps_Acq_Assist> global_gps_acq_assist_map;
|
||||
@ -144,9 +147,9 @@ void StaticPositionSystemTest::geodetic2Enu(double latitude, double longitude, d
|
||||
|
||||
geodetic2Ecef(ref_lat * d2r, ref_long * d2r, ref_h, &ref_x, &ref_y, &ref_z);
|
||||
|
||||
double aux_x = x - ref_x;
|
||||
double aux_y = y - ref_y;
|
||||
double aux_z = z - ref_z;
|
||||
double aux_x = x; // - ref_x;
|
||||
double aux_y = y; // - ref_y;
|
||||
double aux_z = z; // - ref_z;
|
||||
|
||||
// ECEF to NED matrix
|
||||
double phiP = atan2(ref_z, sqrt(std::pow(ref_x, 2.0) + std::pow(ref_y, 2.0)));
|
||||
@ -386,7 +389,7 @@ int StaticPositionSystemTest::configure_receiver()
|
||||
config->set_property("PVT.flag_rtcm_server", "false");
|
||||
config->set_property("PVT.flag_rtcm_tty_port", "false");
|
||||
config->set_property("PVT.rtcm_dump_devname", "/dev/pts/1");
|
||||
config->set_property("PVT.dump", "false");
|
||||
config->set_property("PVT.dump", "true");
|
||||
config->set_property("PVT.rinex_version", std::to_string(2));
|
||||
config->set_property("PVT.iono_model", "OFF");
|
||||
config->set_property("PVT.trop_model", "OFF");
|
||||
@ -455,123 +458,354 @@ int StaticPositionSystemTest::run_receiver()
|
||||
|
||||
void StaticPositionSystemTest::check_results()
|
||||
{
|
||||
std::fstream myfile(StaticPositionSystemTest::generated_kml_file, std::ios_base::in);
|
||||
ASSERT_TRUE(myfile.is_open()) << "No valid kml file could be opened";
|
||||
std::string line;
|
||||
|
||||
std::vector<double> pos_e;
|
||||
std::vector<double> pos_n;
|
||||
std::vector<double> pos_u;
|
||||
|
||||
// Skip header
|
||||
std::getline(myfile, line);
|
||||
bool is_header = true;
|
||||
while (is_header)
|
||||
arma::mat R_eb_e; //ECEF position (x,y,z) estimation in the Earth frame (Nx3)
|
||||
arma::mat V_eb_e; //ECEF velocity (x,y,z) estimation in the Earth frame (Nx3)
|
||||
arma::mat LLH; //Geodetic coordinates (latitude, longitude, height) estimation in WGS84 datum
|
||||
arma::vec receiver_time_s;
|
||||
|
||||
arma::mat ref_R_eb_e; //ECEF position (x,y,z) reference in the Earth frame (Nx3)
|
||||
arma::mat ref_V_eb_e; //ECEF velocity (x,y,z) reference in the Earth frame (Nx3)
|
||||
arma::mat ref_LLH; //Geodetic coordinates (latitude, longitude, height) reference in WGS84 datum
|
||||
arma::vec ref_time_s;
|
||||
|
||||
std::istringstream iss2(FLAGS_static_position);
|
||||
std::string str_aux;
|
||||
std::getline(iss2, str_aux, ',');
|
||||
double ref_lat = std::stod(str_aux);
|
||||
std::getline(iss2, str_aux, ',');
|
||||
double ref_long = std::stod(str_aux);
|
||||
std::getline(iss2, str_aux, '\n');
|
||||
double ref_h = std::stod(str_aux);
|
||||
double ref_e, ref_n, ref_u;
|
||||
geodetic2Enu(ref_lat, ref_long, ref_h,
|
||||
&ref_e, &ref_n, &ref_u);
|
||||
|
||||
if (!FLAGS_use_pvt_solver_dump)
|
||||
{
|
||||
//fall back to read receiver KML output (position only)
|
||||
std::fstream myfile(StaticPositionSystemTest::generated_kml_file, std::ios_base::in);
|
||||
ASSERT_TRUE(myfile.is_open()) << "No valid kml file could be opened";
|
||||
std::string line;
|
||||
// Skip header
|
||||
std::getline(myfile, line);
|
||||
ASSERT_FALSE(myfile.eof()) << "No valid kml file found.";
|
||||
std::size_t found = line.find("<coordinates>");
|
||||
if (found != std::string::npos) is_header = false;
|
||||
}
|
||||
bool is_data = true;
|
||||
|
||||
//read data
|
||||
while (is_data)
|
||||
{
|
||||
if (!std::getline(myfile, line))
|
||||
bool is_header = true;
|
||||
while (is_header)
|
||||
{
|
||||
is_data = false;
|
||||
break;
|
||||
std::getline(myfile, line);
|
||||
ASSERT_FALSE(myfile.eof()) << "No valid kml file found.";
|
||||
std::size_t found = line.find("<coordinates>");
|
||||
if (found != std::string::npos) is_header = false;
|
||||
}
|
||||
std::size_t found = line.find("</coordinates>");
|
||||
if (found != std::string::npos)
|
||||
is_data = false;
|
||||
else
|
||||
{
|
||||
std::string str2;
|
||||
std::istringstream iss(line);
|
||||
double value;
|
||||
double lat = 0.0;
|
||||
double longitude = 0.0;
|
||||
double h = 0.0;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
std::getline(iss, str2, ',');
|
||||
value = std::stod(str2);
|
||||
if (i == 0) lat = value;
|
||||
if (i == 1) longitude = value;
|
||||
if (i == 2) h = value;
|
||||
}
|
||||
bool is_data = true;
|
||||
|
||||
//read data
|
||||
while (is_data)
|
||||
{
|
||||
if (!std::getline(myfile, line))
|
||||
{
|
||||
is_data = false;
|
||||
break;
|
||||
}
|
||||
std::size_t found = line.find("</coordinates>");
|
||||
if (found != std::string::npos)
|
||||
is_data = false;
|
||||
else
|
||||
{
|
||||
std::string str2;
|
||||
std::istringstream iss(line);
|
||||
double value;
|
||||
double lat = 0.0;
|
||||
double longitude = 0.0;
|
||||
double h = 0.0;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
std::getline(iss, str2, ',');
|
||||
value = std::stod(str2);
|
||||
if (i == 0) longitude = value;
|
||||
if (i == 1) lat = value;
|
||||
if (i == 2) h = value;
|
||||
}
|
||||
|
||||
double north, east, up;
|
||||
geodetic2Enu(lat, longitude, h, &east, &north, &up);
|
||||
// std::cout << "lat = " << lat << ", longitude = " << longitude << " h = " << h << std::endl;
|
||||
// std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl;
|
||||
pos_e.push_back(east);
|
||||
pos_n.push_back(north);
|
||||
pos_u.push_back(up);
|
||||
// getchar();
|
||||
}
|
||||
}
|
||||
myfile.close();
|
||||
ASSERT_FALSE(pos_e.size() == 0) << "KML file is empty";
|
||||
}
|
||||
else
|
||||
{
|
||||
//use complete binary dump from pvt solver
|
||||
rtklib_solver_dump_reader pvt_reader;
|
||||
pvt_reader.open_obs_file(FLAGS_pvt_solver_dump_filename);
|
||||
int64_t n_epochs = pvt_reader.num_epochs();
|
||||
R_eb_e = arma::zeros(n_epochs, 3);
|
||||
V_eb_e = arma::zeros(n_epochs, 3);
|
||||
LLH = arma::zeros(n_epochs, 3);
|
||||
receiver_time_s = arma::zeros(n_epochs, 1);
|
||||
int64_t current_epoch = 0;
|
||||
while (pvt_reader.read_binary_obs())
|
||||
{
|
||||
double north, east, up;
|
||||
geodetic2Enu(lat, longitude, h, &east, &north, &up);
|
||||
//std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl;
|
||||
geodetic2Enu(pvt_reader.latitude, pvt_reader.longitude, pvt_reader.height, &east, &north, &up);
|
||||
// std::cout << "lat = " << pvt_reader.latitude << ", longitude = " << pvt_reader.longitude << " h = " << pvt_reader.height << std::endl;
|
||||
// std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl;
|
||||
pos_e.push_back(east);
|
||||
pos_n.push_back(north);
|
||||
pos_u.push_back(up);
|
||||
// getchar();
|
||||
|
||||
// receiver_time_s(current_epoch) = static_cast<double>(pvt_reader.TOW_at_current_symbol_ms) / 1000.0;
|
||||
receiver_time_s(current_epoch) = pvt_reader.RX_time - pvt_reader.clk_offset_s;
|
||||
R_eb_e(current_epoch, 0) = pvt_reader.rr[0];
|
||||
R_eb_e(current_epoch, 1) = pvt_reader.rr[1];
|
||||
R_eb_e(current_epoch, 2) = pvt_reader.rr[2];
|
||||
V_eb_e(current_epoch, 0) = pvt_reader.rr[3];
|
||||
V_eb_e(current_epoch, 1) = pvt_reader.rr[4];
|
||||
V_eb_e(current_epoch, 2) = pvt_reader.rr[5];
|
||||
LLH(current_epoch, 0) = pvt_reader.latitude;
|
||||
LLH(current_epoch, 1) = pvt_reader.longitude;
|
||||
LLH(current_epoch, 2) = pvt_reader.height;
|
||||
|
||||
//debug check
|
||||
// std::cout << "t1: " << pvt_reader.RX_time << std::endl;
|
||||
// std::cout << "t2: " << pvt_reader.TOW_at_current_symbol_ms << std::endl;
|
||||
// std::cout << "offset: " << pvt_reader.clk_offset_s << std::endl;
|
||||
// getchar();
|
||||
current_epoch++;
|
||||
}
|
||||
ASSERT_FALSE(current_epoch == 0) << "PVT dump is empty";
|
||||
}
|
||||
|
||||
// compute results
|
||||
|
||||
if (FLAGS_static_scenario)
|
||||
{
|
||||
double sigma_E_2_precision = std::pow(compute_stdev_precision(pos_e), 2.0);
|
||||
double sigma_N_2_precision = std::pow(compute_stdev_precision(pos_n), 2.0);
|
||||
double sigma_U_2_precision = std::pow(compute_stdev_precision(pos_u), 2.0);
|
||||
|
||||
double sigma_E_2_accuracy = std::pow(compute_stdev_accuracy(pos_e, ref_e), 2.0);
|
||||
double sigma_N_2_accuracy = std::pow(compute_stdev_accuracy(pos_n, ref_n), 2.0);
|
||||
double sigma_U_2_accuracy = std::pow(compute_stdev_accuracy(pos_u, ref_u), 2.0);
|
||||
|
||||
double sum__e = std::accumulate(pos_e.begin(), pos_e.end(), 0.0);
|
||||
double mean__e = sum__e / pos_e.size();
|
||||
double sum__n = std::accumulate(pos_n.begin(), pos_n.end(), 0.0);
|
||||
double mean__n = sum__n / pos_n.size();
|
||||
double sum__u = std::accumulate(pos_u.begin(), pos_u.end(), 0.0);
|
||||
double mean__u = sum__u / pos_u.size();
|
||||
|
||||
std::stringstream stm;
|
||||
std::ofstream position_test_file;
|
||||
|
||||
if (FLAGS_config_file_ptest.empty())
|
||||
{
|
||||
stm << "---- 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 * compute_stdev_accuracy(pos_n, 0.0) + 0.56 * compute_stdev_accuracy(pos_e, 0.0) << " [m]" << std::endl;
|
||||
stm << "99% SAS = " << 1.122 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
|
||||
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(mean__e, 2.0) + std::pow(mean__n, 2.0)) << " [m]" << std::endl;
|
||||
stm << "Bias 3D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0) + std::pow(mean__u, 2.0)) << " [m]" << std::endl;
|
||||
stm << std::endl;
|
||||
}
|
||||
|
||||
stm << "---- 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 * compute_stdev_precision(pos_n) + 0.56 * compute_stdev_precision(pos_e) << " [m]" << std::endl;
|
||||
stm << "99% SAS = " << 1.122 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
|
||||
stm << "90% SAS = " << 0.833 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
|
||||
stm << "MRSE = " << sqrt(sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
|
||||
stm << "SEP = " << 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
|
||||
|
||||
std::cout << stm.rdbuf();
|
||||
std::string output_filename = "position_test_output_" + StaticPositionSystemTest::generated_kml_file.erase(StaticPositionSystemTest::generated_kml_file.length() - 3, 3) + "txt";
|
||||
position_test_file.open(output_filename.c_str());
|
||||
if (position_test_file.is_open())
|
||||
{
|
||||
position_test_file << stm.str();
|
||||
position_test_file.close();
|
||||
}
|
||||
|
||||
// Sanity Check
|
||||
double precision_SEP = 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision);
|
||||
ASSERT_LT(precision_SEP, 20.0);
|
||||
|
||||
if (FLAGS_plot_position_test == true)
|
||||
{
|
||||
print_results(pos_e, pos_n, pos_u);
|
||||
}
|
||||
}
|
||||
myfile.close();
|
||||
ASSERT_FALSE(pos_e.size() == 0) << "KML file is empty";
|
||||
|
||||
double sigma_E_2_precision = std::pow(compute_stdev_precision(pos_e), 2.0);
|
||||
double sigma_N_2_precision = std::pow(compute_stdev_precision(pos_n), 2.0);
|
||||
double sigma_U_2_precision = std::pow(compute_stdev_precision(pos_u), 2.0);
|
||||
|
||||
double sigma_E_2_accuracy = std::pow(compute_stdev_accuracy(pos_e, 0.0), 2.0);
|
||||
double sigma_N_2_accuracy = std::pow(compute_stdev_accuracy(pos_n, 0.0), 2.0);
|
||||
double sigma_U_2_accuracy = std::pow(compute_stdev_accuracy(pos_u, 0.0), 2.0);
|
||||
|
||||
double sum__e = std::accumulate(pos_e.begin(), pos_e.end(), 0.0);
|
||||
double mean__e = sum__e / pos_e.size();
|
||||
double sum__n = std::accumulate(pos_n.begin(), pos_n.end(), 0.0);
|
||||
double mean__n = sum__n / pos_n.size();
|
||||
double sum__u = std::accumulate(pos_u.begin(), pos_u.end(), 0.0);
|
||||
double mean__u = sum__u / pos_u.size();
|
||||
|
||||
std::stringstream stm;
|
||||
std::ofstream position_test_file;
|
||||
|
||||
if (FLAGS_config_file_ptest.empty())
|
||||
else
|
||||
{
|
||||
stm << "---- 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 * compute_stdev_accuracy(pos_n, 0.0) + 0.56 * compute_stdev_accuracy(pos_e, 0.0) << " [m]" << std::endl;
|
||||
stm << "99% SAS = " << 1.122 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
|
||||
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(mean__e, 2.0) + std::pow(mean__n, 2.0)) << " [m]" << std::endl;
|
||||
stm << "Bias 3D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0) + std::pow(mean__u, 2.0)) << " [m]" << std::endl;
|
||||
stm << std::endl;
|
||||
}
|
||||
//dynamic position
|
||||
spirent_motion_csv_dump_reader ref_reader;
|
||||
ref_reader.open_obs_file(FLAGS_ref_motion_filename);
|
||||
int64_t n_epochs = ref_reader.num_epochs();
|
||||
ref_R_eb_e = arma::zeros(n_epochs, 3);
|
||||
ref_V_eb_e = arma::zeros(n_epochs, 3);
|
||||
ref_LLH = arma::zeros(n_epochs, 3);
|
||||
ref_time_s = arma::zeros(n_epochs, 1);
|
||||
int64_t current_epoch = 0;
|
||||
while (ref_reader.read_csv_obs())
|
||||
{
|
||||
ref_time_s(current_epoch) = ref_reader.TOW_ms / 1000.0;
|
||||
ref_R_eb_e(current_epoch, 0) = ref_reader.Pos_X;
|
||||
ref_R_eb_e(current_epoch, 1) = ref_reader.Pos_Y;
|
||||
ref_R_eb_e(current_epoch, 2) = ref_reader.Pos_Z;
|
||||
ref_V_eb_e(current_epoch, 0) = ref_reader.Vel_X;
|
||||
ref_V_eb_e(current_epoch, 1) = ref_reader.Vel_Y;
|
||||
ref_V_eb_e(current_epoch, 2) = ref_reader.Vel_Z;
|
||||
ref_LLH(current_epoch, 0) = ref_reader.Lat;
|
||||
ref_LLH(current_epoch, 1) = ref_reader.Long;
|
||||
ref_LLH(current_epoch, 2) = ref_reader.Height;
|
||||
current_epoch++;
|
||||
}
|
||||
|
||||
stm << "---- 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 * compute_stdev_precision(pos_n) + 0.56 * compute_stdev_precision(pos_e) << " [m]" << std::endl;
|
||||
stm << "99% SAS = " << 1.122 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
|
||||
stm << "90% SAS = " << 0.833 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
|
||||
stm << "MRSE = " << sqrt(sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
|
||||
stm << "SEP = " << 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
|
||||
//interpolation of reference data to receiver epochs timestamps
|
||||
arma::mat ref_interp_R_eb_e = arma::zeros(R_eb_e.n_rows, 3);
|
||||
arma::mat ref_interp_V_eb_e = arma::zeros(V_eb_e.n_rows, 3);
|
||||
arma::mat ref_interp_LLH = arma::zeros(LLH.n_rows, 3);
|
||||
arma::vec tmp_vector;
|
||||
for (int n = 0; n < 3; n++)
|
||||
{
|
||||
arma::interp1(ref_time_s, ref_R_eb_e.col(n), receiver_time_s, tmp_vector);
|
||||
ref_interp_R_eb_e.col(n) = tmp_vector;
|
||||
arma::interp1(ref_time_s, ref_V_eb_e.col(n), receiver_time_s, tmp_vector);
|
||||
ref_interp_V_eb_e.col(n) = tmp_vector;
|
||||
arma::interp1(ref_time_s, ref_LLH.col(n), receiver_time_s, tmp_vector);
|
||||
ref_interp_LLH.col(n) = tmp_vector;
|
||||
}
|
||||
|
||||
std::cout << stm.rdbuf();
|
||||
std::string output_filename = "position_test_output_" + StaticPositionSystemTest::generated_kml_file.erase(StaticPositionSystemTest::generated_kml_file.length() - 3, 3) + "txt";
|
||||
position_test_file.open(output_filename.c_str());
|
||||
if (position_test_file.is_open())
|
||||
{
|
||||
position_test_file << stm.str();
|
||||
position_test_file.close();
|
||||
}
|
||||
//compute error vectors
|
||||
|
||||
// Sanity Check
|
||||
double precision_SEP = 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision);
|
||||
ASSERT_LT(precision_SEP, 20.0);
|
||||
arma::mat error_R_eb_e = arma::zeros(R_eb_e.n_rows, 3);
|
||||
arma::mat error_V_eb_e = arma::zeros(V_eb_e.n_rows, 3);
|
||||
arma::mat error_LLH = arma::zeros(LLH.n_rows, 3);
|
||||
error_R_eb_e = R_eb_e - ref_interp_R_eb_e;
|
||||
error_V_eb_e = V_eb_e - ref_interp_V_eb_e;
|
||||
error_LLH = LLH - ref_interp_LLH;
|
||||
arma::vec error_module_R_eb_e = arma::zeros(R_eb_e.n_rows, 1);
|
||||
arma::vec error_module_V_eb_e = arma::zeros(V_eb_e.n_rows, 1);
|
||||
for (uint64_t n = 0; n < R_eb_e.n_rows; n++)
|
||||
{
|
||||
error_module_R_eb_e(n) = arma::norm(error_R_eb_e.row(n));
|
||||
error_module_V_eb_e(n) = arma::norm(error_V_eb_e.row(n));
|
||||
}
|
||||
//Error statistics
|
||||
arma::vec tmp_vec;
|
||||
//RMSE, Mean, Variance and peaks
|
||||
tmp_vec = arma::square(error_module_R_eb_e);
|
||||
double rmse_R_eb_e = sqrt(arma::mean(tmp_vec));
|
||||
double error_mean_R_eb_e = arma::mean(error_module_R_eb_e);
|
||||
double error_var_R_eb_e = arma::var(error_module_R_eb_e);
|
||||
double max_error_R_eb_e = arma::max(error_module_R_eb_e);
|
||||
double min_error_R_eb_e = arma::min(error_module_R_eb_e);
|
||||
|
||||
if (FLAGS_plot_position_test == true)
|
||||
{
|
||||
print_results(pos_e, pos_n, pos_u);
|
||||
tmp_vec = arma::square(error_module_V_eb_e);
|
||||
double rmse_V_eb_e = sqrt(arma::mean(tmp_vec));
|
||||
double error_mean_V_eb_e = arma::mean(error_module_V_eb_e);
|
||||
double error_var_V_eb_e = arma::var(error_module_V_eb_e);
|
||||
double max_error_V_eb_e = arma::max(error_module_V_eb_e);
|
||||
double min_error_V_eb_e = arma::min(error_module_V_eb_e);
|
||||
|
||||
//report
|
||||
std::cout << "----- Position and Velocity 3D ECEF error statistics -----" << std::endl;
|
||||
std::streamsize ss = std::cout.precision();
|
||||
std::cout << std::setprecision(10) << "---- 3D ECEF Position RMSE = "
|
||||
<< rmse_R_eb_e << ", mean = " << error_mean_R_eb_e
|
||||
<< ", stdev = " << sqrt(error_var_R_eb_e)
|
||||
<< " (max,min) = " << max_error_R_eb_e
|
||||
<< "," << min_error_R_eb_e
|
||||
<< " [m]" << std::endl;
|
||||
std::cout << "---- 3D ECEF Velocity RMSE = "
|
||||
<< rmse_V_eb_e << ", mean = " << error_mean_V_eb_e
|
||||
<< ", stdev = " << sqrt(error_var_V_eb_e)
|
||||
<< " (max,min) = " << max_error_V_eb_e
|
||||
<< "," << min_error_V_eb_e
|
||||
<< " [m/s]" << std::endl;
|
||||
std::cout.precision(ss);
|
||||
|
||||
//plots
|
||||
Gnuplot g1("points");
|
||||
if (FLAGS_show_plots)
|
||||
{
|
||||
g1.showonscreen(); // window output
|
||||
}
|
||||
else
|
||||
{
|
||||
g1.disablescreen();
|
||||
}
|
||||
g1.set_title("3D ECEF error coordinates");
|
||||
g1.set_grid();
|
||||
//conversion between arma::vec and std:vector
|
||||
std::vector<double> X(error_R_eb_e.colptr(0), error_R_eb_e.colptr(0) + error_R_eb_e.n_rows);
|
||||
std::vector<double> Y(error_R_eb_e.colptr(1), error_R_eb_e.colptr(1) + error_R_eb_e.n_rows);
|
||||
std::vector<double> Z(error_R_eb_e.colptr(2), error_R_eb_e.colptr(2) + error_R_eb_e.n_rows);
|
||||
|
||||
g1.cmd("set key box opaque");
|
||||
g1.plot_xyz(X, Y, Z, "ECEF_3d_error");
|
||||
g1.set_legend();
|
||||
g1.savetops("ECEF_3d_error");
|
||||
|
||||
|
||||
arma::vec time_vector_from_start_s = receiver_time_s - receiver_time_s(0);
|
||||
Gnuplot g3("linespoints");
|
||||
if (FLAGS_show_plots)
|
||||
{
|
||||
g3.showonscreen(); // window output
|
||||
}
|
||||
else
|
||||
{
|
||||
g3.disablescreen();
|
||||
}
|
||||
g3.set_title("3D Position estimation error module [m]");
|
||||
g3.set_grid();
|
||||
g3.set_xlabel("Receiver epoch time from first valid PVT [s]");
|
||||
g3.set_ylabel("3D Position error [m]");
|
||||
//conversion between arma::vec and std:vector
|
||||
std::vector<double> error_vec(error_module_R_eb_e.colptr(0), error_module_R_eb_e.colptr(0) + error_module_R_eb_e.n_rows);
|
||||
g3.cmd("set key box opaque");
|
||||
g3.plot_xy(time_vector_from_start_s, error_vec,
|
||||
"Position_3d_error");
|
||||
g3.set_legend();
|
||||
g3.savetops("Position_3d_error");
|
||||
|
||||
Gnuplot g4("linespoints");
|
||||
if (FLAGS_show_plots)
|
||||
{
|
||||
g4.showonscreen(); // window output
|
||||
}
|
||||
else
|
||||
{
|
||||
g4.disablescreen();
|
||||
}
|
||||
g4.set_title("3D Velocity estimation error module [m/s]");
|
||||
g4.set_grid();
|
||||
g4.set_xlabel("Receiver epoch time from first valid PVT [s]");
|
||||
g4.set_ylabel("3D Velocity error [m/s]");
|
||||
//conversion between arma::vec and std:vector
|
||||
std::vector<double> error_vec2(error_module_V_eb_e.colptr(0), error_module_V_eb_e.colptr(0) + error_module_V_eb_e.n_rows);
|
||||
g4.cmd("set key box opaque");
|
||||
g4.plot_xy(time_vector_from_start_s, error_vec2,
|
||||
"Velocity_3d_error");
|
||||
g4.set_legend();
|
||||
g4.savetops("Velocity_3d_error");
|
||||
}
|
||||
}
|
||||
|
||||
@ -698,7 +932,7 @@ TEST_F(StaticPositionSystemTest, Position_system_test)
|
||||
configure_receiver();
|
||||
|
||||
// Run the receiver
|
||||
EXPECT_EQ(run_receiver(), 0) << "Problem executing the software-defined signal generator";
|
||||
EXPECT_EQ(run_receiver(), 0) << "Problem executing GNSS-SDR";
|
||||
|
||||
// Check results
|
||||
check_results();
|
||||
|
||||
@ -1632,7 +1632,6 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
|
||||
|
||||
//Do not compare E5a with E5 RINEX due to the Doppler frequency discrepancy caused by the different center frequencies
|
||||
//E5a_fc=1176.45e6, E5b_fc=1207.14e6, E5_fc=1191.795e6;
|
||||
std::cout << "s:" << gnss_synchro_vec.at(n).Signal << std::endl;
|
||||
if (strcmp("5X\0", gnss_synchro_vec.at(n).Signal) != 0 or FLAGS_compare_with_5X)
|
||||
{
|
||||
check_results_carrier_phase_double_diff(true_obs_vec.at(n),
|
||||
|
||||
Loading…
Reference in New Issue
Block a user