diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index a7e2d0880..8689c7230 100644 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -10,34 +10,30 @@ protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS ${CMAKE_SOURCE_DIR}/docs/protobuf/monitor_pvt.proto) set(PVT_LIB_SOURCES + pvt_conf.cc pvt_solution.cc - ls_pvt.cc - hybrid_ls_pvt.cc - kml_printer.cc + geojson_printer.cc gpx_printer.cc - rinex_printer.cc + kml_printer.cc nmea_printer.cc + rinex_printer.cc rtcm_printer.cc rtcm.cc - geojson_printer.cc rtklib_solver.cc - pvt_conf.cc monitor_pvt_udp_sink.cc ) set(PVT_LIB_HEADERS + pvt_conf.h pvt_solution.h - ls_pvt.h - hybrid_ls_pvt.h - kml_printer.h + geojson_printer.h gpx_printer.h - rinex_printer.h + kml_printer.h nmea_printer.h + rinex_printer.h rtcm_printer.h rtcm.h - geojson_printer.h rtklib_solver.h - pvt_conf.h monitor_pvt_udp_sink.h monitor_pvt.h serdes_monitor_pvt.h @@ -73,13 +69,11 @@ endif() target_link_libraries(pvt_libs PUBLIC - Armadillo::armadillo Boost::date_time protobuf::libprotobuf core_system_parameters algorithms_libs_rtklib PRIVATE - algorithms_libs Gflags::gflags Glog::glog Matio::matio @@ -91,16 +85,12 @@ target_include_directories(pvt_libs PUBLIC ${CMAKE_SOURCE_DIR}/src/core/receiver SYSTEM ${PROTO_INCLUDE_HEADERS} + PRIVATE + ${CMAKE_SOURCE_DIR}/src/algorithms/libs # for gnss_sdr_make_unique.h ) target_compile_definitions(pvt_libs PRIVATE -DGNSS_SDR_VERSION="${VERSION}") -if(ENABLE_ARMA_NO_DEBUG) - target_compile_definitions(pvt_libs - PUBLIC -DARMA_NO_BOUND_CHECKING=1 - ) -endif() - if(USE_BOOST_ASIO_IO_CONTEXT) target_compile_definitions(pvt_libs PUBLIC diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc deleted file mode 100644 index 6126c0ead..000000000 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc +++ /dev/null @@ -1,385 +0,0 @@ -/*! - * \file hybrid_ls_pvt.cc - * \brief Implementation of a Least Squares Position, Velocity, and Time - * (PVT) solver, based on K.Borre's Matlab receiver. - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2019 (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. - * - * SPDX-License-Identifier: GPL-3.0-or-later - * - * ------------------------------------------------------------------------- - */ - -#include "hybrid_ls_pvt.h" -#include "GPS_L2C.h" -#include "MATH_CONSTANTS.h" -#include -#include -#include - - -Hybrid_Ls_Pvt::Hybrid_Ls_Pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file) -{ - // init empty ephemeris for all the available GNSS channels - d_nchannels = nchannels; - d_dump_filename = std::move(dump_filename); - d_flag_dump_enabled = flag_dump_to_file; - d_galileo_current_time = 0; - this->set_averaging_flag(false); - // ############# ENABLE DATA FILE LOG ################# - if (d_flag_dump_enabled == 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 lib dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception opening PVT lib dump file " << e.what(); - } - } - } -} - - -Hybrid_Ls_Pvt::~Hybrid_Ls_Pvt() -{ - 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(); - } - } -} - - -bool Hybrid_Ls_Pvt::get_PVT(std::map gnss_observables_map, double hybrid_current_time, bool flag_averaging) -{ - std::map::iterator gnss_observables_iter; - std::map::iterator galileo_ephemeris_iter; - std::map::iterator gps_ephemeris_iter; - std::map::iterator gps_cnav_ephemeris_iter; - - arma::vec W; // channels weight vector - arma::vec obs; // pseudoranges observation vector - arma::mat satpos; // satellite positions matrix - - int Galileo_week_number = 0; - int GPS_week = 0; - double utc = 0.0; - double GST = 0.0; - double secondsperweek = 604800.0; - - // double utc_tx_corrected = 0.0; //utc computed at tx_time_corrected, added for Galileo constellation (in GPS utc is directly computed at TX_time_corrected_s) - double TX_time_corrected_s = 0.0; - double SV_clock_bias_s = 0.0; - - this->set_averaging_flag(flag_averaging); - - // ******************************************************************************** - // ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) **** - // ******************************************************************************** - int valid_obs = 0; // valid observations counter - - for (gnss_observables_iter = gnss_observables_map.begin(); - gnss_observables_iter != gnss_observables_map.end(); - gnss_observables_iter++) - { - switch (gnss_observables_iter->second.System) - { - case 'E': - { - // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key - galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != galileo_ephemeris_map.end()) - { - /*! - * \todo Place here the satellite CN0 (power level, or weight factor) - */ - W.resize(valid_obs + 1, 1); - W(valid_obs) = 1; - - // COMMON RX TIME PVT ALGORITHM - double Rx_time = hybrid_current_time; - double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / SPEED_OF_LIGHT_M_S; - - // 2- compute the clock drift using the clock model (broadcast) for this SV - SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time); - - // 3- compute the current ECEF position for this SV using corrected TX time - TX_time_corrected_s = Tx_time - SV_clock_bias_s; - galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); - - // store satellite positions in a matrix - satpos.resize(3, valid_obs + 1); - satpos(0, valid_obs) = galileo_ephemeris_iter->second.d_satpos_X; - satpos(1, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Y; - satpos(2, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Z; - - // 4- fill the observations vector with the corrected observables - obs.resize(valid_obs + 1, 1); - obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * SPEED_OF_LIGHT_M_S - this->get_time_offset_s() * SPEED_OF_LIGHT_M_S; - - Galileo_week_number = galileo_ephemeris_iter->second.WN_5; // for GST - GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time); - - // SV ECEF DEBUG OUTPUT - DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN - << " X=" << galileo_ephemeris_iter->second.d_satpos_X - << " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y - << " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z - << " [m] PR_obs=" << obs(valid_obs) << " [m]"; - - valid_obs++; - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - break; - } - case 'G': - { - // 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key - std::string sig_(gnss_observables_iter->second.Signal); - if (sig_ == "1C") - { - gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != gps_ephemeris_map.end()) - { - /*! - * \todo Place here the satellite CN0 (power level, or weight factor) - */ - W.resize(valid_obs + 1, 1); - W(valid_obs) = 1; - - // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files) - // first estimate of transmit time - double Rx_time = hybrid_current_time; - double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / SPEED_OF_LIGHT_M_S; - - // 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect - SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD; - - // 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect - TX_time_corrected_s = Tx_time - SV_clock_bias_s; - double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); - - // store satellite positions in a matrix - satpos.resize(3, valid_obs + 1); - satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X; - satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y; - satpos(2, valid_obs) = gps_ephemeris_iter->second.d_satpos_Z; - - // 4- fill the observations vector with the corrected pseudoranges - // compute code bias: TGD for single frequency - // See IS-GPS-200K section 20.3.3.3.3.2 - double sqrt_Gamma = GPS_L1_FREQ_HZ / GPS_L2_FREQ_HZ; - double Gamma = sqrt_Gamma * sqrt_Gamma; - double P1_P2 = (1.0 - Gamma) * (gps_ephemeris_iter->second.d_TGD * SPEED_OF_LIGHT_M_S); - double Code_bias_m = P1_P2 / (1.0 - Gamma); - obs.resize(valid_obs + 1, 1); - obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * SPEED_OF_LIGHT_M_S - Code_bias_m - this->get_time_offset_s() * SPEED_OF_LIGHT_M_S; - - // SV ECEF DEBUG OUTPUT - LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN - << " TX Time corrected=" << TX_time_corrected_s << " X=" << gps_ephemeris_iter->second.d_satpos_X - << " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y - << " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z - << " [m] PR_obs=" << obs(valid_obs) << " [m]"; - - valid_obs++; - // compute the UTC time for this SV (just to print the associated UTC timestamp) - GPS_week = gps_ephemeris_iter->second.i_GPS_week; - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first; - } - } - if (sig_ == "2S") - { - gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end()) - { - /*! - * \todo Place here the satellite CN0 (power level, or weight factor) - */ - W.resize(valid_obs + 1, 1); - W(valid_obs) = 1; - - // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files) - // first estimate of transmit time - double Rx_time = hybrid_current_time; - double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / SPEED_OF_LIGHT_M_S; - - // 2- compute the clock drift using the clock model (broadcast) for this SV - SV_clock_bias_s = gps_cnav_ephemeris_iter->second.sv_clock_drift(Tx_time); - - // 3- compute the current ECEF position for this SV using corrected TX time - TX_time_corrected_s = Tx_time - SV_clock_bias_s; - // std::cout<<"TX time["<second.i_satellite_PRN<<"]="<second.satellitePosition(TX_time_corrected_s); - - // store satellite positions in a matrix - satpos.resize(3, valid_obs + 1); - satpos(0, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_X; - satpos(1, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Y; - satpos(2, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Z; - - // 4- fill the observations vector with the corrected observables - obs.resize(valid_obs + 1, 1); - obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * SPEED_OF_LIGHT_M_S + SV_clock_bias_s * SPEED_OF_LIGHT_M_S; - - GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week; - GPS_week = GPS_week % 1024; // Necessary due to the increase of WN bits in CNAV message (10 in GPS NAV and 13 in CNAV) - - // SV ECEF DEBUG OUTPUT - LOG(INFO) << "(new)ECEF GPS L2M satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN - << " TX Time corrected=" << TX_time_corrected_s - << " X=" << gps_cnav_ephemeris_iter->second.d_satpos_X - << " [m] Y=" << gps_cnav_ephemeris_iter->second.d_satpos_Y - << " [m] Z=" << gps_cnav_ephemeris_iter->second.d_satpos_Z - << " [m] PR_obs=" << obs(valid_obs) << " [m]"; - - valid_obs++; - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - } - break; - } - default: - DLOG(INFO) << "Hybrid observables: Unknown GNSS"; - break; - } - } - - // ******************************************************************************** - // ****** SOLVE LEAST SQUARES****************************************************** - // ******************************************************************************** - this->set_num_valid_observations(valid_obs); - - LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs; - - if (valid_obs >= 4) - { - arma::vec rx_position_and_time; - DLOG(INFO) << "satpos=" << satpos; - DLOG(INFO) << "obs=" << obs; - DLOG(INFO) << "W=" << W; - try - { - // check if this is the initial position computation - if (this->get_time_offset_s() == 0) - { - // execute Bancroft's algorithm to estimate initial receiver position and time - DLOG(INFO) << " Executing Bancroft algorithm..."; - rx_position_and_time = bancroftPos(satpos.t(), obs); - this->set_rx_pos({rx_position_and_time(0), rx_position_and_time(1), rx_position_and_time(2)}); // save ECEF position for the next iteration - this->set_time_offset_s(rx_position_and_time(3) / SPEED_OF_LIGHT_M_S); // save time for the next iteration [meters]->[seconds] - } - - // Execute WLS using previous position as the initialization point - rx_position_and_time = leastSquarePos(satpos, obs, W); - - this->set_rx_pos({rx_position_and_time(0), rx_position_and_time(1), rx_position_and_time(2)}); // save ECEF position for the next iteration - this->set_time_offset_s(this->get_time_offset_s() + rx_position_and_time(3) / SPEED_OF_LIGHT_M_S); // accumulate the rx time error for the next iteration [meters]->[seconds] - - DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time[0] << ", " << rx_position_and_time[1] << ", " << rx_position_and_time[2] << ", " << rx_position_and_time[4]; - DLOG(INFO) << "Accumulated rx clock error=" << this->get_time_offset_s() << " clock error for this iteration=" << rx_position_and_time(3) / SPEED_OF_LIGHT_M_S << " [s]"; - - // Compute GST and Gregorian time - if (GST != 0.0) - { - utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number); - } - else - { - utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week) + secondsperweek * static_cast(GPS_week); - } - - // get time string Gregorian calendar - boost::posix_time::time_duration t = boost::posix_time::milliseconds(static_cast(utc * 1000.0)); // NOLINT(google-runtime-int) - // 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2) - boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); - this->set_position_UTC_time(p_time); - - DLOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time) - << " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude() - << " [deg], Height= " << this->get_height() << " [m]" - << " RX time offset= " << this->get_time_offset_s() << " [s]"; - - // ######## LOG FILE ######### - if (d_flag_dump_enabled == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - // PVT GPS time - tmp_double = hybrid_current_time; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // ECEF User Position East [m] - tmp_double = rx_position_and_time(0); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // ECEF User Position North [m] - tmp_double = rx_position_and_time(1); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // ECEF User Position Up [m] - tmp_double = rx_position_and_time(2); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // User clock offset [s] - tmp_double = rx_position_and_time(3); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // GEO user position Latitude [deg] - tmp_double = this->get_latitude(); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // GEO user position Longitude [deg] - tmp_double = this->get_longitude(); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // GEO user position Height [m] - tmp_double = this->get_height(); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing PVT LS dump file " << e.what(); - } - } - - // MOVING AVERAGE PVT - this->perform_pos_averaging(); - } - catch (const std::exception& e) - { - this->set_time_offset_s(0.0); // reset rx time estimation - LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what(); - this->set_valid_position(false); - } - } - else - { - this->set_valid_position(false); - } - return this->is_valid_position(); -} diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.h b/src/algorithms/PVT/libs/hybrid_ls_pvt.h deleted file mode 100644 index f4c502b42..000000000 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.h +++ /dev/null @@ -1,68 +0,0 @@ -/*! - * \file hybrid_ls_pvt.h - * \brief Interface of a Least Squares Position, Velocity, and Time (PVT) - * solver, based on K.Borre's Matlab receiver. - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2019 (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. - * - * SPDX-License-Identifier: GPL-3.0-or-later - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_HYBRID_LS_PVT_H -#define GNSS_SDR_HYBRID_LS_PVT_H - -#include "galileo_almanac.h" -#include "galileo_navigation_message.h" -#include "gnss_synchro.h" -#include "gps_cnav_navigation_message.h" -#include "gps_navigation_message.h" -#include "ls_pvt.h" -#include "rtklib_rtkcmn.h" -#include -#include -#include - -/*! - * \brief This class implements a simple PVT Least Squares solution - */ -class Hybrid_Ls_Pvt : public Ls_Pvt -{ -public: - Hybrid_Ls_Pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file); - ~Hybrid_Ls_Pvt(); - - bool get_PVT(std::map gnss_observables_map, double hybrid_current_time, bool flag_averaging); - - std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris - std::map gps_ephemeris_map; //!< Map storing new GPS_Ephemeris - std::map gps_cnav_ephemeris_map; - - Galileo_Utc_Model galileo_utc_model; - Galileo_Iono galileo_iono; - Galileo_Almanac galileo_almanac; - - Gps_Utc_Model gps_utc_model; - Gps_Iono gps_iono; - - Gps_CNAV_Iono gps_cnav_iono; - Gps_CNAV_Utc_Model gps_cnav_utc_model; - -private: - bool d_flag_dump_enabled; - std::string d_dump_filename; - std::ofstream d_dump_file; - int d_nchannels; // Number of available channels for positioning - double d_galileo_current_time; -}; - -#endif diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc deleted file mode 100644 index 59fed2006..000000000 --- a/src/algorithms/PVT/libs/ls_pvt.cc +++ /dev/null @@ -1,428 +0,0 @@ -/*! - * \file ls_pvt.cc - * \brief Implementation of a base class for Least Squares PVT solutions - * \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es - * - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2019 (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. - * - * SPDX-License-Identifier: GPL-3.0-or-later - * - * ------------------------------------------------------------------------- - */ - -#include "ls_pvt.h" -#include "MATH_CONSTANTS.h" -#include "geofunctions.h" -#include -#include - -arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs) -{ - // BANCROFT Calculation of preliminary coordinates for a GPS receiver based on pseudoranges - // to 4 or more satellites. The ECEF coordinates are stored in satpos. - // The observed pseudoranges are stored in obs - // Reference: Bancroft, S. (1985) An Algebraic Solution of the GPS Equations, - // IEEE Trans. Aerosp. and Elec. Systems, AES-21, Issue 1, pp. 56--59 - // Based on code by: - // Kai Borre 04-30-95, improved by C.C. Goad 11-24-96 - // - // Test values to use in debugging - // B_pass =[ -11716227.778 -10118754.628 21741083.973 22163882.029; - // -12082643.974 -20428242.179 11741374.154 21492579.823; - // 14373286.650 -10448439.349 19596404.858 21492492.771; - // 10278432.244 -21116508.618 -12689101.970 25284588.982 ]; - // Solution: 595025.053 -4856501.221 4078329.981 - // - // Test values to use in debugging - // B_pass = [14177509.188 -18814750.650 12243944.449 21119263.116; - // 15097198.146 -4636098.555 21326705.426 22527063.486; - // 23460341.997 -9433577.991 8174873.599 23674159.579; - // -8206498.071 -18217989.839 17605227.065 20951643.862; - // 1399135.830 -17563786.820 19705534.862 20155386.649; - // 6995655.459 -23537808.269 -9927906.485 24222112.972 ]; - // Solution: 596902.683 -4847843.316 4088216.740 - - arma::vec pos = arma::zeros(4, 1); - arma::mat B_pass = arma::zeros(obs.size(), 4); - B_pass.submat(0, 0, obs.size() - 1, 2) = satpos; - B_pass.col(3) = obs; - - arma::mat B; - arma::mat BBB; - double traveltime = 0; - for (int iter = 0; iter < 2; iter++) - { - B = B_pass; - int m = arma::size(B, 0); - for (int i = 0; i < m; i++) - { - int x = B(i, 0); - int y = B(i, 1); - if (iter == 0) - { - traveltime = 0.072; - } - else - { - int z = B(i, 2); - double rho = (x - pos(0)) * (x - pos(0)) + (y - pos(1)) * (y - pos(1)) + (z - pos(2)) * (z - pos(2)); - traveltime = sqrt(rho) / SPEED_OF_LIGHT_M_S; - } - double angle = traveltime * 7.292115147e-5; - double cosa = cos(angle); - double sina = sin(angle); - B(i, 0) = cosa * x + sina * y; - B(i, 1) = -sina * x + cosa * y; - } // % i-loop - - if (m > 3) - { - BBB = arma::inv(B.t() * B) * B.t(); - } - else - { - BBB = arma::inv(B); - } - arma::vec e = arma::ones(m, 1); - arma::vec alpha = arma::zeros(m, 1); - for (int i = 0; i < m; i++) - { - alpha(i) = lorentz(B.row(i).t(), B.row(i).t()) / 2.0; - } - arma::mat BBBe = BBB * e; - arma::mat BBBalpha = BBB * alpha; - double a = lorentz(BBBe, BBBe); - double b = lorentz(BBBe, BBBalpha) - 1; - double c = lorentz(BBBalpha, BBBalpha); - double root = sqrt(b * b - a * c); - arma::vec r = {(-b - root) / a, (-b + root) / a}; - arma::mat possible_pos = arma::zeros(4, 2); - for (int i = 0; i < 2; i++) - { - possible_pos.col(i) = r(i) * BBBe + BBBalpha; - possible_pos(3, i) = -possible_pos(3, i); - } - - arma::vec abs_omc = arma::zeros(2, 1); - for (int j = 0; j < m; j++) - { - for (int i = 0; i < 2; i++) - { - double c_dt = possible_pos(3, i); - double calc = arma::norm(satpos.row(i).t() - possible_pos.col(i).rows(0, 2)) + c_dt; - double omc = obs(j) - calc; - abs_omc(i) = std::abs(omc); - } - } // % j-loop - - // discrimination between roots - if (abs_omc(0) > abs_omc(1)) - { - pos = possible_pos.col(1); - } - else - { - pos = possible_pos.col(0); - } - } // % iter loop - return pos; -} - - -double Ls_Pvt::lorentz(const arma::vec& x, const arma::vec& y) -{ - // LORENTZ Calculates the Lorentz inner product of the two - // 4 by 1 vectors x and y - // Based on code by: - // Kai Borre 04-22-95 - // - // M = diag([1 1 1 -1]); - // p = x'*M*y; - - return (x(0) * y(0) + x(1) * y(1) + x(2) * y(2) - x(3) * y(3)); -} - - -arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs, const arma::vec& w_vec) -{ - /* Computes the Least Squares Solution. - * Inputs: - * satpos - Satellites positions in ECEF system: [X; Y; Z;] - * obs - Observations - the pseudorange measurements to each satellite - * w - weight vector - * - * Returns: - * pos - receiver position and receiver clock error - * (in ECEF system: [X, Y, Z, dt]) - */ - - //=== Initialization ======================================================= - constexpr double GPS_STARTOFFSET_MS = 68.802; // [ms] Initial signal travel time - int nmbOfIterations = 10; // TODO: include in config - int nmbOfSatellites; - nmbOfSatellites = satpos.n_cols; // Armadillo - arma::mat w = arma::zeros(nmbOfSatellites, nmbOfSatellites); - w.diag() = w_vec; // diagonal weight matrix - - std::array rx_pos = this->get_rx_pos(); - arma::vec pos = {rx_pos[0], rx_pos[1], rx_pos[2], 0}; // time error in METERS (time x speed) - arma::mat A; - arma::mat omc; - A = arma::zeros(nmbOfSatellites, 4); - omc = arma::zeros(nmbOfSatellites, 1); - arma::mat X = satpos; - arma::vec Rot_X; - double rho2; - double traveltime; - double trop = 0.0; - double dlambda; - double dphi; - double h; - arma::vec x; - - //=== Iteratively find receiver position =================================== - for (int iter = 0; iter < nmbOfIterations; iter++) - { - for (int i = 0; i < nmbOfSatellites; i++) - { - if (iter == 0) - { - // --- Initialize variables at the first iteration ------------- - Rot_X = X.col(i); // Armadillo - trop = 0.0; - } - else - { - // --- Update equations ---------------------------------------- - rho2 = (X(0, i) - pos(0)) * - (X(0, i) - pos(0)) + - (X(1, i) - pos(1)) * - (X(1, i) - pos(1)) + - (X(2, i) - pos(2)) * - (X(2, i) - pos(2)); - traveltime = sqrt(rho2) / SPEED_OF_LIGHT_M_S; - - // --- Correct satellite position (do to earth rotation) ------- - std::array rot_x = Ls_Pvt::rotateSatellite(traveltime, {X(0, i), X(1, i), X(2, i)}); - Rot_X = {rot_x[0], rot_x[1], rot_x[2]}; - - // -- Find DOA and range of satellites - double* azim = nullptr; - double* elev = nullptr; - double* dist = nullptr; - topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2)); - - if (traveltime < 0.1 && nmbOfSatellites > 3) - { - // --- Find receiver's height - togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2)); - // Add troposphere correction if the receiver is below the troposphere - if (h > 15000) - { - // receiver is above the troposphere - trop = 0.0; - } - else - { - // --- Find delay due to troposphere (in meters) - Ls_Pvt::tropo(&trop, sin(elev[0] * GNSS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); - if (trop > 5.0) - { - trop = 0.0; // check for erratic values - } - } - } - } - // --- Apply the corrections ---------------------------------------- - omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo - - // -- Construct the A matrix --------------------------------------- - // Armadillo - A(i, 0) = (-(Rot_X(0) - pos(0))) / obs(i); - A(i, 1) = (-(Rot_X(1) - pos(1))) / obs(i); - A(i, 2) = (-(Rot_X(2) - pos(2))) / obs(i); - A(i, 3) = 1.0; - } - - // -- Find position update --------------------------------------------- - x = arma::solve(w * A, w * omc); // Armadillo - - // -- Apply position update -------------------------------------------- - pos = pos + x; - if (arma::norm(x, 2) < 1e-4) - { - break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm) - } - } - - // check the consistency of the PVT solution - if (((fabs(pos(3)) * 1000.0) / SPEED_OF_LIGHT_M_S) > GPS_STARTOFFSET_MS * 2) - { - LOG(WARNING) << "Receiver time offset out of range! Estimated RX Time error [s]:" << pos(3) / SPEED_OF_LIGHT_M_S; - throw std::runtime_error("Receiver time offset out of range!"); - } - return pos; -} - - -int Ls_Pvt::tropo(double* ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km) -{ - /* Inputs: - sinel - sin of elevation angle of satellite - hsta_km - height of station in km - p_mb - atmospheric pressure in mb at height hp_km - t_kel - surface temperature in degrees Kelvin at height htkel_km - hum - humidity in % at height hhum_km - hp_km - height of pressure measurement in km - htkel_km - height of temperature measurement in km - hhum_km - height of humidity measurement in km - - Outputs: - ddr_m - range correction (meters) - - Reference - Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric - Refraction Correction Model. Paper presented at the - American Geophysical Union Annual Fall Meeting, San - Francisco, December 12-17 - - Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre - */ - - const double a_e = 6378.137; // semi-major axis of earth ellipsoid - const double b0 = 7.839257e-5; - const double tlapse = -6.5; - const double em = -978.77 / (2.8704e6 * tlapse * 1.0e-5); - const double tkhum = t_kel + tlapse * (hhum_km - htkel_km); - const double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15); - const double e0 = 0.0611 * hum * pow(10, atkel); - const double tksea = t_kel - tlapse * htkel_km; - const double tkelh = tksea + tlapse * hhum_km; - const double e0sea = e0 * pow((tksea / tkelh), (4 * em)); - const double tkelp = tksea + tlapse * hp_km; - const double psea = p_mb * pow((tksea / tkelp), em); - - if (sinel < 0) - { - sinel = 0.0; - } - - double tropo_delay = 0.0; - bool done = false; - double refsea = 77.624e-6 / tksea; - double htop = 1.1385e-5 / refsea; - refsea = refsea * psea; - double ref = refsea * pow(((htop - hsta_km) / htop), 4); - - double a; - double b; - double rtop; - - while (true) - { - rtop = pow((a_e + htop), 2) - pow((a_e + hsta_km), 2) * (1 - pow(sinel, 2)); - - // check to see if geometry is crazy - if (rtop < 0) - { - rtop = 0; - } - - rtop = sqrt(rtop) - (a_e + hsta_km) * sinel; - - a = -sinel / (htop - hsta_km); - b = -b0 * (1 - pow(sinel, 2)) / (htop - hsta_km); - - arma::vec rn = arma::vec(8); - rn.zeros(); - - for (int i = 0; i < 8; i++) - { - rn(i) = pow(rtop, (i + 1 + 1)); - } - - arma::rowvec alpha = {2 * a, 2 * pow(a, 2) + 4 * b / 3, a * (pow(a, 2) + 3 * b), - pow(a, 4) / 5 + 2.4 * pow(a, 2) * b + 1.2 * pow(b, 2), 2 * a * b * (pow(a, 2) + 3 * b) / 3, - pow(b, 2) * (6 * pow(a, 2) + 4 * b) * 1.428571e-1, 0, 0}; - - if (pow(b, 2) > 1.0e-35) - { - alpha(6) = a * pow(b, 3) / 2; - alpha(7) = pow(b, 4) / 9; - } - - double dr = rtop; - arma::mat aux_ = alpha * rn; - dr = dr + aux_(0, 0); - tropo_delay = tropo_delay + dr * ref * 1000; - - if (done == true) - { - *ddr_m = tropo_delay; - break; - } - - done = true; - refsea = (371900.0e-6 / tksea - 12.92e-6) / tksea; - htop = 1.1385e-5 * (1255 / tksea + 0.05) / refsea; - ref = refsea * e0sea * pow(((htop - hsta_km) / htop), 4); - } - return 0; -} - - -std::array Ls_Pvt::rotateSatellite(double traveltime, const std::array& X_sat) -{ - /* - * Returns rotated satellite ECEF coordinates due to Earth - * rotation during signal travel time - * - * Inputs: - * travelTime - signal travel time - * X_sat - satellite's ECEF coordinates - * - * Returns: - * X_sat_rot - rotated satellite's coordinates (ECEF) - */ - - const double omegatau = GNSS_OMEGA_EARTH_DOT * traveltime; - const double cosomg = cos(omegatau); - const double sinomg = sin(omegatau); - const double x = cosomg * X_sat[0] + sinomg * X_sat[1]; - const double y = -sinomg * X_sat[0] + cosomg * X_sat[1]; - - std::array X_sat_rot = {x, y, X_sat[2]}; - return X_sat_rot; -} - - -double Ls_Pvt::get_gdop() const -{ - return 0.0; // not implemented -} - - -double Ls_Pvt::get_pdop() const -{ - return 0.0; // not implemented -} - - -double Ls_Pvt::get_hdop() const -{ - return 0.0; // not implemented -} - - -double Ls_Pvt::get_vdop() const -{ - return 0.0; // not implemented -} diff --git a/src/algorithms/PVT/libs/ls_pvt.h b/src/algorithms/PVT/libs/ls_pvt.h deleted file mode 100644 index 8cc77e6fd..000000000 --- a/src/algorithms/PVT/libs/ls_pvt.h +++ /dev/null @@ -1,90 +0,0 @@ -/*! - * \file ls_pvt.h - * \brief Interface of a base class for Least Squares PVT solutions - * \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es - * - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2019 (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. - * - * SPDX-License-Identifier: GPL-3.0-or-later - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_LS_PVT_H -#define GNSS_SDR_LS_PVT_H - -#if ARMA_NO_BOUND_CHECKING -#define ARMA_NO_DEBUG 1 -#endif - -#include "pvt_solution.h" -#include -#include - -/*! - * \brief Base class for the Least Squares PVT solution - * - */ -class Ls_Pvt : public Pvt_Solution -{ -public: - Ls_Pvt() = default; - - /*! - * \brief Computes the initial position solution based on the Bancroft algorithm - */ - arma::vec bancroftPos(const arma::mat& satpos, const arma::vec& obs); - - /*! - * \brief Computes the Weighted Least Squares position solution - */ - arma::vec leastSquarePos(const arma::mat& satpos, const arma::vec& obs, const arma::vec& w_vec); - - double get_hdop() const override; - double get_vdop() const override; - double get_pdop() const override; - double get_gdop() const override; - -private: - /* - * Computes the Lorentz inner product between two vectors - */ - double lorentz(const arma::vec& x, const arma::vec& y); - - /* - * Tropospheric correction - * - * \param[in] sinel - sin of elevation angle of satellite - * \param[in] hsta_km - height of station in km - * \param[in] p_mb - atmospheric pressure in mb at height hp_km - * \param[in] t_kel - surface temperature in degrees Kelvin at height htkel_km - * \param[in] hum - humidity in % at height hhum_km - * \param[in] hp_km - height of pressure measurement in km - * \param[in] htkel_km - height of temperature measurement in km - * \param[in] hhum_km - height of humidity measurement in km - * - * \param[out] ddr_m - range correction (meters) - * - * - * Reference: - * Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric - * Refraction Correction Model. Paper presented at the - * American Geophysical Union Annual Fall Meeting, San - * Francisco, December 12-17 - * - * Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre - */ - int tropo(double* ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km); - - std::array rotateSatellite(double traveltime, const std::array& X_sat); -}; - -#endif