From c178d9a8a6761b83a1b492f7ef6b387027a5b2c4 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sun, 12 Jul 2020 12:42:06 +0200 Subject: [PATCH] Remove Armadillo from Pvt_Solution API Some API cleaning. The user does not need to call cart2geo anymore. Armadillo stuff moved to old ls_pvt solution --- src/algorithms/PVT/libs/hybrid_ls_pvt.cc | 11 +- src/algorithms/PVT/libs/ls_pvt.cc | 170 ++++++++++++++++++- src/algorithms/PVT/libs/ls_pvt.h | 41 ++++- src/algorithms/PVT/libs/pvt_solution.cc | 177 +++----------------- src/algorithms/PVT/libs/pvt_solution.h | 124 +++++--------- src/algorithms/PVT/libs/rtklib_solver.cc | 43 ++--- src/core/system_parameters/GPS_L1_CA.h | 2 - src/core/system_parameters/MATH_CONSTANTS.h | 8 +- 8 files changed, 291 insertions(+), 285 deletions(-) diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc index cdab47472..68fa8a652 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc @@ -19,9 +19,8 @@ */ #include "hybrid_ls_pvt.h" -#include "GPS_L1_CA.h" #include "GPS_L2C.h" -#include "Galileo_E1.h" +#include "MATH_CONSTANTS.h" #include #include #include @@ -296,14 +295,14 @@ bool Hybrid_Ls_Pvt::get_PVT(std::map gnss_observables_map, do // 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.rows(0, 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] + 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.rows(0, 2)); // save ECEF position for the next iteration + 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; @@ -325,8 +324,6 @@ bool Hybrid_Ls_Pvt::get_PVT(std::map gnss_observables_map, do boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); this->set_position_UTC_time(p_time); - cart2geo(static_cast(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(rx_position_and_time(2)), 4); - 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]" diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc index a0ca8d42e..59fed2006 100644 --- a/src/algorithms/PVT/libs/ls_pvt.cc +++ b/src/algorithms/PVT/libs/ls_pvt.cc @@ -19,12 +19,11 @@ */ #include "ls_pvt.h" -#include "GPS_L1_CA.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 @@ -166,14 +165,15 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs, */ //=== Initialization ======================================================= - int nmbOfIterations = 10; // TODO: include in config + 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 - arma::vec 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) + 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); @@ -211,7 +211,8 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs, traveltime = sqrt(rho2) / SPEED_OF_LIGHT_M_S; // --- Correct satellite position (do to earth rotation) ------- - Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); // armadillo + 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; @@ -232,7 +233,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs, else { // --- Find delay due to troposphere (in meters) - Ls_Pvt::tropo(&trop, sin(*elev * GNSS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); + 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 @@ -270,3 +271,158 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs, } 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 index 610d6a605..8cc77e6fd 100644 --- a/src/algorithms/PVT/libs/ls_pvt.h +++ b/src/algorithms/PVT/libs/ls_pvt.h @@ -21,8 +21,13 @@ #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 @@ -43,11 +48,43 @@ public: */ 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: - /*! - * \brief Computes the Lorentz inner product between two vectors + /* + * 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 diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index b77c88ccb..bf8274a24 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -6,7 +6,7 @@ * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -19,10 +19,9 @@ */ #include "pvt_solution.h" -#include "GPS_L1_CA.h" -#include "geofunctions.h" +#include "MATH_CONSTANTS.h" #include -#include +#include #include @@ -40,43 +39,12 @@ Pvt_Solution::Pvt_Solution() b_valid_position = false; d_averaging_depth = 0; d_valid_observations = 0; - d_rx_pos = arma::zeros(3, 1); d_rx_dt_s = 0.0; d_rx_clock_drift_ppm = 0.0; d_pre_2009_file = false; // disabled by default } -arma::vec Pvt_Solution::rotateSatellite(double const traveltime, const arma::vec &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) - */ - - // -- Find rotation angle -------------------------------------------------- - double omegatau; - omegatau = GNSS_OMEGA_EARTH_DOT * traveltime; - - // -- Build a rotation matrix ---------------------------------------------- - arma::mat R3 = {{cos(omegatau), sin(omegatau), 0.0}, - {-sin(omegatau), cos(omegatau), 0.0}, - {0.0, 0.0, 1.0}}; - - // -- Do the rotation ------------------------------------------------------ - arma::vec X_sat_rot; - X_sat_rot = R3 * X_sat; - return X_sat_rot; -} - - int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection) { /* Conversion of Cartesian coordinates (X,Y,Z) to geographical @@ -93,9 +61,9 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection) const std::array a = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0}; const std::array f = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563}; - double lambda = atan2(Y, X); - 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); + const double lambda = atan2(Y, X); + const double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection])); + const double c = a[elipsoid_selection] * sqrt(1.0 + ex2); double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection]))); double h = 0.1; @@ -116,121 +84,15 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection) } } while (std::abs(h - oldh) > 1.0e-12); - d_latitude_d = phi * 180.0 / GNSS_PI; - d_longitude_d = lambda * 180.0 / GNSS_PI; + + d_latitude_d = phi * R2D; + d_longitude_d = lambda * R2D; d_height_m = h; // todo: refactor this class. Mix of duplicated functions, use either RTKLIB geodetic functions or geofunctions.h return 0; } -int Pvt_Solution::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); - - double tkhum = t_kel + tlapse * (hhum_km - htkel_km); - double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15); - double e0 = 0.0611 * hum * pow(10, atkel); - double tksea = t_kel - tlapse * htkel_km; - double tkelh = tksea + tlapse * hhum_km; - double e0sea = e0 * pow((tksea / tkelh), (4 * em)); - double tkelp = tksea + tlapse * hp_km; - 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; -} - - void Pvt_Solution::set_averaging_depth(int depth) { d_averaging_depth = depth; @@ -306,6 +168,7 @@ void Pvt_Solution::set_time_offset_s(double offset) d_rx_dt_s = offset; } + double Pvt_Solution::get_clock_drift_ppm() const { return d_rx_clock_drift_ppm; @@ -317,6 +180,7 @@ void Pvt_Solution::set_clock_drift_ppm(double clock_drift_ppm) d_rx_clock_drift_ppm = clock_drift_ppm; } + double Pvt_Solution::get_latitude() const { return d_latitude_d; @@ -395,27 +259,26 @@ void Pvt_Solution::set_valid_position(bool is_valid) } -void Pvt_Solution::set_rx_pos(const arma::vec &pos) +void Pvt_Solution::set_rx_pos(const std::array &pos) { d_rx_pos = pos; - d_latitude_d = d_rx_pos(0); - d_longitude_d = d_rx_pos(1); - d_height_m = d_rx_pos(2); + Pvt_Solution::cart2geo(d_rx_pos[0], d_rx_pos[1], d_rx_pos[2], 4); } -arma::vec Pvt_Solution::get_rx_pos() const +std::array Pvt_Solution::get_rx_pos() const { return d_rx_pos; } -void Pvt_Solution::set_rx_vel(const arma::vec &vel) + +void Pvt_Solution::set_rx_vel(const std::array &vel) { d_rx_vel = vel; } -arma::vec Pvt_Solution::get_rx_vel() const +std::array Pvt_Solution::get_rx_vel() const { return d_rx_vel; } @@ -449,3 +312,9 @@ void Pvt_Solution::set_pre_2009_file(bool pre_2009_file) { d_pre_2009_file = pre_2009_file; } + + +bool Pvt_Solution::is_pre_2009() const +{ + return d_pre_2009_file; +} diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h index cb1eb5289..e5a6587e0 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/src/algorithms/PVT/libs/pvt_solution.h @@ -6,7 +6,7 @@ * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -21,12 +21,8 @@ #ifndef GNSS_SDR_PVT_SOLUTION_H #define GNSS_SDR_PVT_SOLUTION_H -#if ARMA_NO_BOUND_CHECKING -#define ARMA_NO_DEBUG 1 -#endif - -#include #include +#include #include @@ -39,51 +35,48 @@ class Pvt_Solution public: Pvt_Solution(); virtual ~Pvt_Solution() = default; - void set_pre_2009_file(bool pre_2009_file); //!< Flag for the week rollover computation in post processing mode for signals older than 2009 - double get_time_offset_s() const; //!< Get RX time offset [s] - void set_time_offset_s(double offset); //!< Set RX time offset [s] - double get_clock_drift_ppm() const; //!< Get the Rx clock drift [ppm] - void set_clock_drift_ppm(double clock_drift_ppm); //!< Set the Rx clock drift [ppm] - double get_latitude() const; //!< Get RX position Latitude WGS84 [deg] - double get_longitude() const; //!< Get RX position Longitude WGS84 [deg] - double get_height() const; //!< Get RX position height WGS84 [m] - - double get_speed_over_ground() const; //!< Get RX speed over ground [m/s] - void set_speed_over_ground(double speed_m_s); //!< Set RX speed over ground [m/s] - - double get_course_over_ground() const; //!< Get RX course over ground [deg] - void set_course_over_ground(double cog_deg); //!< Set RX course over ground [deg] - - double get_avg_latitude() const; //!< Get RX position averaged Latitude WGS84 [deg] - double get_avg_longitude() const; //!< Get RX position averaged Longitude WGS84 [deg] - double get_avg_height() const; //!< Get RX position averaged height WGS84 [m] - - void set_rx_pos(const arma::vec &pos); //!< Set position: Latitude [deg], longitude [deg], height [m] - arma::vec get_rx_pos() const; - - void set_rx_vel(const arma::vec &vel); //!< Set velocity: East [m/s], North [m/s], Up [m/s] - arma::vec get_rx_vel() const; - - bool is_valid_position() const; - void set_valid_position(bool is_valid); - - boost::posix_time::ptime get_position_UTC_time() const; + void set_rx_pos(const std::array &pos); //!< Set position: X, Y, Z in Cartesian ECEF coordinates [m] + void set_rx_vel(const std::array &vel); //!< Set velocity: East [m/s], North [m/s], Up [m/s] void set_position_UTC_time(const boost::posix_time::ptime &pt); - - int get_num_valid_observations() const; //!< Get the number of valid pseudorange observations (valid satellites) - void set_num_valid_observations(int num); //!< Set the number of valid pseudorange observations (valid satellites) - + void set_time_offset_s(double offset); //!< Set RX time offset [s] + void set_clock_drift_ppm(double clock_drift_ppm); //!< Set the Rx clock drift [ppm] + void set_speed_over_ground(double speed_m_s); //!< Set RX speed over ground [m/s] + void set_course_over_ground(double cog_deg); //!< Set RX course over ground [deg] + void set_valid_position(bool is_valid); + void set_num_valid_observations(int num); //!< Set the number of valid pseudorange observations (valid satellites) + void set_pre_2009_file(bool pre_2009_file); //!< Flag for the week rollover computation in post processing mode for signals older than 2009 // averaging - void perform_pos_averaging(); void set_averaging_depth(int depth); //!< Set length of averaging window - bool is_averaging() const; void set_averaging_flag(bool flag); + void perform_pos_averaging(); - arma::vec rotateSatellite(double traveltime, const arma::vec &X_sat); + std::array get_rx_pos() const; + std::array get_rx_vel() const; + boost::posix_time::ptime get_position_UTC_time() const; + double get_latitude() const; //!< Get RX position Latitude WGS84 [deg] + double get_longitude() const; //!< Get RX position Longitude WGS84 [deg] + double get_height() const; //!< Get RX position height WGS84 [m] + double get_time_offset_s() const; //!< Get RX time offset [s] + double get_clock_drift_ppm() const; //!< Get the Rx clock drift [ppm] + double get_speed_over_ground() const; //!< Get RX speed over ground [m/s] + double get_course_over_ground() const; //!< Get RX course over ground [deg] + double get_avg_latitude() const; //!< Get RX position averaged Latitude WGS84 [deg] + double get_avg_longitude() const; //!< Get RX position averaged Longitude WGS84 [deg] + double get_avg_height() const; //!< Get RX position averaged height WGS84 [m] + int get_num_valid_observations() const; //!< Get the number of valid pseudorange observations (valid satellites) + bool is_pre_2009() const; + bool is_valid_position() const; + bool is_averaging() const; - /*! - * \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical + virtual double get_hdop() const = 0; + virtual double get_vdop() const = 0; + virtual double get_pdop() const = 0; + virtual double get_gdop() const = 0; + +private: + /* + * Conversion of Cartesian coordinates (X,Y,Z) to geographical * coordinates (d_latitude_d, d_longitude_d, d_height_m) on a selected reference ellipsoid. * * \param[in] X [m] Cartesian coordinate @@ -99,53 +92,19 @@ public: */ int cart2geo(double X, double Y, double Z, int elipsoid_selection); - /*! - * \brief 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); - - virtual double get_hdop() const = 0; - virtual double get_vdop() const = 0; - virtual double get_pdop() const = 0; - virtual double get_gdop() const = 0; - -protected: - bool d_pre_2009_file; // Flag to correct week rollover in post processing mode for signals older than 2009 -private: - arma::vec d_rx_pos; - arma::vec d_rx_vel; + std::array d_rx_pos{}; + std::array d_rx_vel{}; boost::posix_time::ptime d_position_UTC_time; std::deque d_hist_latitude_d; std::deque d_hist_longitude_d; std::deque d_hist_height_m; - double d_rx_dt_s; // RX time offset [s] - double d_rx_clock_drift_ppm; // RX clock drift [ppm] - double d_latitude_d; // RX position Latitude WGS84 [deg] double d_longitude_d; // RX position Longitude WGS84 [deg] double d_height_m; // RX position height WGS84 [m] + double d_rx_dt_s; // RX time offset [s] + double d_rx_clock_drift_ppm; // RX clock drift [ppm] double d_speed_over_ground_m_s; // RX speed over ground [m/s] double d_course_over_ground_d; // RX course over ground [deg] @@ -156,6 +115,7 @@ private: int d_averaging_depth; // Length of averaging window int d_valid_observations; + bool d_pre_2009_file; // Flag to correct week rollover in post processing mode for signals older than 2009 bool b_valid_position; bool d_flag_averaging; }; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index e5123d145..e0f43a6a6 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -31,12 +31,7 @@ * -----------------------------------------------------------------------*/ #include "rtklib_solver.h" -#include "Beidou_B1I.h" -#include "Beidou_B3I.h" #include "Beidou_DNAV.h" -#include "GLONASS_L1_L2_CA.h" -#include "GPS_L1_CA.h" -#include "Galileo_E1.h" #include "rtklib_conversions.h" #include "rtklib_rtkpos.h" #include "rtklib_solution.h" @@ -546,14 +541,14 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ if (gps_ephemeris_iter != gps_ephemeris_map.cend()) { // convert ephemeris from GNSS-SDR class to RTKLIB structure - eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second, d_pre_2009_file); + eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second, this->is_pre_2009()); // convert observation from GNSS-SDR class to RTKLIB structure obsd_t newobs = {{0, 0}, '0', '0', {}, {}, {}, {}, {}, {}}; obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, gps_ephemeris_iter->second.i_GPS_week, 0, - d_pre_2009_file); + this->is_pre_2009()); valid_obs++; } else // the ephemeris are not available for this SV @@ -944,24 +939,24 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ dops(index_aux, azel.data(), 0.0, dop_.data()); } this->set_valid_position(true); - arma::vec rx_position_and_time(4); - rx_position_and_time(0) = pvt_sol.rr[0]; // [m] - rx_position_and_time(1) = pvt_sol.rr[1]; // [m] - rx_position_and_time(2) = pvt_sol.rr[2]; // [m] + std::array rx_position_and_time{}; + rx_position_and_time[0] = pvt_sol.rr[0]; // [m] + rx_position_and_time[1] = pvt_sol.rr[1]; // [m] + rx_position_and_time[2] = pvt_sol.rr[2]; // [m] // todo: fix this ambiguity in the RTKLIB units in receiver clock offset! if (rtk_.opt.mode == PMODE_SINGLE) { // if the RTKLIB solver is set to SINGLE, the dtr is already expressed in [s] // add also the clock offset from gps to galileo (pvt_sol.dtr[2]) - rx_position_and_time(3) = pvt_sol.dtr[0] + pvt_sol.dtr[2]; + rx_position_and_time[3] = pvt_sol.dtr[0] + pvt_sol.dtr[2]; } else { // the receiver clock offset is expressed in [meters], so we convert it into [s] // add also the clock offset from gps to galileo (pvt_sol.dtr[2]) - rx_position_and_time(3) = pvt_sol.dtr[2] + pvt_sol.dtr[0] / SPEED_OF_LIGHT_M_S; + rx_position_and_time[3] = pvt_sol.dtr[2] + pvt_sol.dtr[0] / SPEED_OF_LIGHT_M_S; } - this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration + 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 // compute Ground speed and COG double ground_speed_ms = 0.0; @@ -981,7 +976,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ this->set_course_over_ground(new_cog); } - this->set_time_offset_s(rx_position_and_time(3)); + this->set_time_offset_s(rx_position_and_time[3]); DLOG(INFO) << "RTKLIB Position at RX TOW = " << gnss_observables_map.begin()->second.RX_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; @@ -989,13 +984,12 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ boost::posix_time::ptime p_time; // gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time); // Corrected RX Time (Non integer multiply of 1 ms of granularity) // Uncorrected RX Time (integer multiply of 1 ms and the same observables time reported in RTCM and RINEX) - gtime_t rtklib_time = timeadd(pvt_sol.time, rx_position_and_time(3)); // uncorrected rx time + gtime_t rtklib_time = timeadd(pvt_sol.time, rx_position_and_time[3]); // uncorrected rx time gtime_t rtklib_utc_time = gpst2utc(rtklib_time); p_time = boost::posix_time::from_time_t(rtklib_utc_time.time); p_time += boost::posix_time::microseconds(static_cast(round(rtklib_utc_time.sec * 1e6))); // NOLINT(google-runtime-int) this->set_position_UTC_time(p_time); - cart2geo(static_cast(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(rx_position_and_time(2)), 4); DLOG(INFO) << "RTKLIB Position at " << boost::posix_time::to_simple_string(p_time) << " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude() @@ -1006,11 +1000,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ // TOW monitor_pvt.TOW_at_current_symbol_ms = gnss_observables_map.begin()->second.TOW_at_current_symbol_ms; // WEEK - monitor_pvt.week = adjgpsweek(nav_data.eph[0].week, d_pre_2009_file); + monitor_pvt.week = adjgpsweek(nav_data.eph[0].week, this->is_pre_2009()); // PVT GPS time monitor_pvt.RX_time = gnss_observables_map.begin()->second.RX_time; // User clock offset [s] - monitor_pvt.user_clk_offset = rx_position_and_time(3); + monitor_pvt.user_clk_offset = rx_position_and_time[3]; // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double) monitor_pvt.pos_x = pvt_sol.rr[0]; @@ -1052,12 +1046,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ monitor_pvt.hdop = dop_[2]; monitor_pvt.vdop = dop_[3]; - arma::vec rx_vel_enu(3); - rx_vel_enu(0) = enuv[0]; - rx_vel_enu(1) = enuv[1]; - rx_vel_enu(2) = enuv[2]; - - this->set_rx_vel(rx_vel_enu); + this->set_rx_vel({enuv[0], enuv[1], enuv[2]}); double clock_drift_ppm = pvt_sol.dtr[5] / SPEED_OF_LIGHT_M_S * 1e6; @@ -1077,13 +1066,13 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_uint32 = gnss_observables_map.begin()->second.TOW_at_current_symbol_ms; d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); // WEEK - tmp_uint32 = adjgpsweek(nav_data.eph[0].week, d_pre_2009_file); + tmp_uint32 = adjgpsweek(nav_data.eph[0].week, this->is_pre_2009()); d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); // PVT GPS time tmp_double = gnss_observables_map.begin()->second.RX_time; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // User clock offset [s] - tmp_double = rx_position_and_time(3); + tmp_double = rx_position_and_time[3]; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double) diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index 9fa9442a4..c014b7c76 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -47,8 +47,6 @@ constexpr uint32_t GPS_L1_CA_BIT_PERIOD_MS = 20U; //!< GPS L1 C/A bit peri */ constexpr double MAX_TOA_DELAY_MS = 20.0; -constexpr double GPS_STARTOFFSET_MS = 68.802; // [ms] Initial signal travel time (only for old ls_pvt implementation) - // optimum parameters constexpr uint32_t GPS_L1_CA_OPT_ACQ_FS_SPS = 2000000; //!< Sampling frequency that maximizes the acquisition SNR while using a non-multiple of chip rate diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index cbfd99ea9..835a60edb 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -114,10 +114,10 @@ constexpr double PI_TWO_N31 = 1.462918079267160e-009; //!< Pi*2^-31 constexpr double PI_TWO_N38 = 1.142904749427469e-011; //!< Pi*2^-38 constexpr double PI_TWO_N23 = 3.745070282923929e-007; //!< Pi*2^-23 -constexpr double D2R = (GNSS_PI / 180.0); //!< deg to rad -constexpr double R2D = (180.0 / GNSS_PI); //!< rad to deg -constexpr double SC2RAD = GNSS_PI; //!< semi-circle to radian (IS-GPS) -constexpr double AS2R = (D2R / 3600.0); //!< arc sec to radian +constexpr double D2R = GNSS_PI / 180.0; //!< deg to rad +constexpr double R2D = 180.0 / GNSS_PI; //!< rad to deg +constexpr double SC2RAD = GNSS_PI; //!< semi-circle to radian (IS-GPS) +constexpr double AS2R = D2R / 3600.0; //!< arc sec to radian constexpr double AU = 149597870691.0; //!< 1 Astronomical Unit AU (m) distance from Earth to the Sun.