From 0c2bafffe5c8d523a9866fbc441053b12fceda31 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 11 Oct 2018 16:48:20 +0200 Subject: [PATCH 01/10] Silence annoying warnings from gpstk --- src/tests/CMakeLists.txt | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 29489e66b..dafa5047d 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -228,7 +228,9 @@ if(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA) find_package(GPSTK) if(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK) message(STATUS "GPSTk v${GNSSSDR_GPSTK_LOCAL_VERSION} will be automatically downloaded and built when doing 'make'.") - + if ("${TOOLCHAIN_ARG}" STREQUAL "") + set(${TOOLCHAIN_ARG} "-DCMAKE_CXX_FLAGS=\"-Wno-deprecated\"") + endif("${TOOLCHAIN_ARG}" STREQUAL "") # if(NOT ENABLE_FPGA) if(CMAKE_VERSION VERSION_LESS 3.2) ExternalProject_Add( @@ -322,7 +324,7 @@ else(ENABLE_INSTALL_TESTS) file(COPY ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/GPS_L1_CA_ID_1_Fs_4Msps_2ms.dat DESTINATION ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples) file(COPY ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/NT1065_GLONASS_L1_20160831_fs6625e6_if0e3_4ms.bin DESTINATION ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples) file(COPY ${CMAKE_SOURCE_DIR}/src/tests/data/rtklib_test/obs_test1.xml DESTINATION ${CMAKE_SOURCE_DIR}/thirdparty/data/rtklib_test) - file(COPY ${CMAKE_SOURCE_DIR}/src/tests/data/rtklib_test/eph_GPS_L1CA_test1.xml DESTINATION ${CMAKE_SOURCE_DIR}/thirdparty/data/rtklib_test) + file(COPY ${CMAKE_SOURCE_DIR}/src/tests/data/rtklib_test/eph_GPS_L1CA_test1.xml DESTINATION ${CMAKE_SOURCE_DIR}/thirdparty/data/rtklib_test) add_definitions(-DTEST_PATH="${CMAKE_SOURCE_DIR}/thirdparty/") endif(ENABLE_INSTALL_TESTS) @@ -512,7 +514,7 @@ 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) From 2bafcc2bf34fd38c5faf70f08a7d1c7effd01662 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 11 Oct 2018 17:22:27 +0200 Subject: [PATCH 02/10] Fix compiler flag passing to GPSTk --- src/tests/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index dafa5047d..99a375beb 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -229,7 +229,7 @@ if(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA) if(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK) message(STATUS "GPSTk v${GNSSSDR_GPSTK_LOCAL_VERSION} will be automatically downloaded and built when doing 'make'.") if ("${TOOLCHAIN_ARG}" STREQUAL "") - set(${TOOLCHAIN_ARG} "-DCMAKE_CXX_FLAGS=\"-Wno-deprecated\"") + set(${TOOLCHAIN_ARG} "-DCMAKE_CXX_FLAGS=-Wno-deprecated") endif("${TOOLCHAIN_ARG}" STREQUAL "") # if(NOT ENABLE_FPGA) if(CMAKE_VERSION VERSION_LESS 3.2) From 9c19a269ed8d79291eae2e10ff7bc153135fbcba Mon Sep 17 00:00:00 2001 From: Javier Date: Thu, 11 Oct 2018 17:47:23 +0200 Subject: [PATCH 03/10] Improving position system test for static scenario using kml receiver output (default scenario) --- src/tests/system-tests/libs/geofunctions.cc | 340 +++++++++++++++++++- src/tests/system-tests/libs/geofunctions.h | 25 ++ src/tests/system-tests/position_test.cc | 264 +++++---------- 3 files changed, 448 insertions(+), 181 deletions(-) diff --git a/src/tests/system-tests/libs/geofunctions.cc b/src/tests/system-tests/libs/geofunctions.cc index e06654ac9..1f372e8c2 100644 --- a/src/tests/system-tests/libs/geofunctions.cc +++ b/src/tests/system-tests/libs/geofunctions.cc @@ -29,6 +29,7 @@ * ------------------------------------------------------------------------- */ #include "geofunctions.h" +#include const double STRP_G_SI = 9.80665; const double STRP_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E @@ -363,7 +364,7 @@ arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection) break; } } - while (std::abs(h - oldh) > 1.0e-12); + while (std::fabs(h - oldh) > 1.0e-12); arma::vec LLH = {{phi, lambda, h}}; //radians return LLH; @@ -473,3 +474,340 @@ double great_circle_distance(double lat1, double lon1, double lat2, double lon2) double d = R * c; return d * 1000.0; // meters } + +void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu) +{ + //%CART2UTM Transformation of (X,Y,Z) to (E,N,U) in UTM, zone 'zone'. + //% + //%[E, N, U] = cart2utm(X, Y, Z, zone); + //% + //% Inputs: + //% X,Y,Z - Cartesian coordinates. Coordinates are referenced + //% with respect to the International Terrestrial Reference + //% Frame 1996 (ITRF96) + //% zone - UTM zone of the given position + //% + //% Outputs: + //% E, N, U - UTM coordinates (Easting, Northing, Uping) + // + //%Kai Borre -11-1994 + //%Copyright (c) by Kai Borre + //% + //% CVS record: + //% $Id: cart2utm.m,v 1.1.1.1.2.6 2007/01/30 09:45:12 dpl Exp $ + // + //%This implementation is based upon + //%O. Andersson & K. Poder (1981) Koordinattransformationer + //% ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren + //% Vol. 30: 552--571 and Vol. 31: 76 + //% + //%An excellent, general reference (KW) is + //%R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der + //% h\"oheren Geod\"asie und Kartographie. + //% Erster Band, Springer Verlag + // + //% Explanation of variables used: + //% f flattening of ellipsoid + //% a semi major axis in m + //% m0 1 - scale at central meridian; for UTM 0.0004 + //% Q_n normalized meridian quadrant + //% E0 Easting of central meridian + //% L0 Longitude of central meridian + //% bg constants for ellipsoidal geogr. to spherical geogr. + //% gb constants for spherical geogr. to ellipsoidal geogr. + //% gtu constants for ellipsoidal N, E to spherical N, E + //% utg constants for spherical N, E to ellipoidal N, E + //% tolutm tolerance for utm, 1.2E-10*meridian quadrant + //% tolgeo tolerance for geographical, 0.00040 second of arc + // + //% B, L refer to latitude and longitude. Southern latitude is negative + //% International ellipsoid of 1924, valid for ED50 + + double a = 6378388.0; + double f = 1.0 / 297.0; + double ex2 = (2.0 - f) * f / ((1.0 - f) * (1.0 - f)); + double c = a * sqrt(1.0 + ex2); + arma::vec vec = r_eb_e; + vec(2) = vec(2) - 4.5; + double alpha = 0.756e-6; + arma::mat R = {{1.0, -alpha, 0.0}, {alpha, 1.0, 0.0}, {0.0, 0.0, 1.0}}; + arma::vec trans = {89.5, 93.8, 127.6}; + double scale = 0.9999988; + arma::vec v = scale * R * vec + trans; // coordinate vector in ED50 + double L = atan2(v(1), v(0)); + double N1 = 6395000; // preliminary value + double B = atan2(v(2) / ((1 - f) * (1 - f) * N1), arma::norm(v.subvec(0, 1)) / N1); // preliminary value + double U = 0.1; + double oldU = 0; + int iterations = 0; + while (fabs(U - oldU) > 1.0E-4) + { + oldU = U; + N1 = c / sqrt(1.0 + ex2 * (cos(B) * cos(B))); + B = atan2(v(2) / ((1.0 - f) * (1.0 - f) * N1 + U), arma::norm(v.subvec(0, 1)) / (N1 + U)); + U = arma::norm(v.subvec(0, 1)) / cos(B) - N1; + iterations = iterations + 1; + if (iterations > 100) + { + std::cout << "Failed to approximate U with desired precision. U-oldU:" << U - oldU << std::endl; + break; + } + } + //%Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21) + double m0 = 0.0004; + double n = f / (2.0 - f); + double m = n * n * (1 / 4 + n * n / 64); + double w = (a * (-n - m0 + m * (1 - m0))) / (1 + n); + double Q_n = a + w; + + //%Easting and longitude of central meridian + double E0 = 500000; + double L0 = (zone - 30.0) * 6.0 - 3.0; + + //%Check tolerance for reverse transformation + //double tolutm = STRP_PI / 2.0 * 1.2e-10 * Q_n; + //double tolgeo = 0.000040; + // % Coefficients of trigonometric series + + // % ellipsoidal to spherical geographical, KW p .186 --187, (51) - (52) + // % bg[1] = n * (-2 + n * (2 / 3 + n * (4 / 3 + n * (-82 / 45)))); + // % bg[2] = n ^ 2 * (5 / 3 + n * (-16 / 15 + n * (-13 / 9))); + // % bg[3] = n ^ 3 * (-26 / 15 + n * 34 / 21); + // % bg[4] = n ^ 4 * 1237 / 630; + // + // % spherical to ellipsoidal geographical, KW p.190 --191, (61) - (62) % gb[1] = n * (2 + n * (-2 / 3 + n * (-2 + n * 116 / 45))); + // % gb[2] = n ^ 2 * (7 / 3 + n * (-8 / 5 + n * (-227 / 45))); + // % gb[3] = n ^ 3 * (56 / 15 + n * (-136 / 35)); + // % gb[4] = n ^ 4 * 4279 / 630; + // + // % spherical to ellipsoidal N, E, KW p.196, (69) % gtu[1] = n * (1 / 2 + n * (-2 / 3 + n * (5 / 16 + n * 41 / 180))); + // % gtu[2] = n ^ 2 * (13 / 48 + n * (-3 / 5 + n * 557 / 1440)); + // % gtu[3] = n ^ 3 * (61 / 240 + n * (-103 / 140)); + // % gtu[4] = n ^ 4 * 49561 / 161280; + // + // % ellipsoidal to spherical N, E, KW p.194, (65) % utg[1] = n * (-1 / 2 + n * (2 / 3 + n * (-37 / 96 + n * 1 / 360))); + // % utg[2] = n ^ 2 * (-1 / 48 + n * (-1 / 15 + n * 437 / 1440)); + // % utg[3] = n ^ 3 * (-17 / 480 + n * 37 / 840); + // % utg[4] = n ^ 4 * (-4397 / 161280); + + // % With f = 1 / 297 we get + + arma::colvec bg = {-3.37077907e-3, + 4.73444769e-6, + -8.29914570e-9, + 1.58785330e-11}; + + arma::colvec gb = {3.37077588e-3, + 6.62769080e-6, + 1.78718601e-8, + 5.49266312e-11}; + + arma::colvec gtu = {8.41275991e-4, + 7.67306686e-7, + 1.21291230e-9, + 2.48508228e-12}; + + arma::colvec utg = {-8.41276339e-4, + -5.95619298e-8, + -1.69485209e-10, + -2.20473896e-13}; + + // % Ellipsoidal latitude, longitude to spherical latitude, longitude + bool neg_geo = false; + + if (B < 0) neg_geo = true; + + double Bg_r = fabs(B); + double res_clensin = clsin(bg, 4, 2 * Bg_r); + Bg_r = Bg_r + res_clensin; + L0 = L0 * STRP_PI / 180.0; + double Lg_r = L - L0; + + // % Spherical latitude, longitude to complementary spherical latitude % i.e.spherical N, E + double cos_BN = cos(Bg_r); + double Np = atan2(sin(Bg_r), cos(Lg_r) * cos_BN); + double Ep = atanh(sin(Lg_r) * cos_BN); + + // % Spherical normalized N, E to ellipsoidal N, E + Np = 2.0 * Np; + Ep = 2.0 * Ep; + + double dN; + double dE; + clksin(gtu, 4, Np, Ep, &dN, &dE); + Np = Np / 2.0; + Ep = Ep / 2.0; + Np = Np + dN; + Ep = Ep + dE; + double N = Q_n * Np; + double E = Q_n * Ep + E0; + if (neg_geo) + { + N = -N + 20000000.0; + } + r_enu(0) = E; + r_enu(1) = N; + r_enu(2) = U; +} + + +double clsin(arma::colvec ar, int degree, double argument) +{ + //%Clenshaw summation of sinus of argument. + //% + //%result = clsin(ar, degree, argument); + // + //% Written by Kai Borre + //% December 20, 1995 + //% + //% See also WGS2UTM or CART2UTM + //% + //% CVS record: + //% $Id: clsin.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ + //%========================================================================== + + double cos_arg = 2.0 * cos(argument); + double hr1 = 0; + double hr = 0; + double hr2; + for (int t = degree; t > 0; t--) + { + hr2 = hr1; + hr1 = hr; + hr = ar(t - 1) + cos_arg * hr1 - hr2; + } + + return (hr * sin(argument)); +} + + +void clksin(arma::colvec ar, int degree, double arg_real, double arg_imag, double *re, double *im) +{ + //%Clenshaw summation of sinus with complex argument + //%[re, im] = clksin(ar, degree, arg_real, arg_imag); + // + //% Written by Kai Borre + //% December 20, 1995 + //% + //% See also WGS2UTM or CART2UTM + //% + //% CVS record: + //% $Id: clksin.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ + //%========================================================================== + + double sin_arg_r = sin(arg_real); + double cos_arg_r = cos(arg_real); + double sinh_arg_i = sinh(arg_imag); + double cosh_arg_i = cosh(arg_imag); + + double r = 2 * cos_arg_r * cosh_arg_i; + double i = -2 * sin_arg_r * sinh_arg_i; + + double hr1 = 0; + double hr = 0; + double hi1 = 0; + double hi = 0; + double hi2; + double hr2; + for (int t = degree; t > 0; t--) + { + hr2 = hr1; + hr1 = hr; + hi2 = hi1; + hi1 = hi; + double z = ar(t - 1) + r * hr1 - i * hi - hr2; + hi = i * hr1 + r * hi1 - hi2; + hr = z; + } + + r = sin_arg_r * cosh_arg_i; + i = cos_arg_r * sinh_arg_i; + + *re = r * hr - i * hi; + *im = r * hi + i * hr; +} + +int findUtmZone(double latitude_deg, double longitude_deg) +{ + //%Function finds the UTM zone number for given longitude and latitude. + //%The longitude value must be between -180 (180 degree West) and 180 (180 + //%degree East) degree. The latitude must be within -80 (80 degree South) and + //%84 (84 degree North). + //% + //%utmZone = findUtmZone(latitude, longitude); + //% + //%Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not + //%15 deg 30 min). + // + //%-------------------------------------------------------------------------- + //% SoftGNSS v3.0 + //% + //% Copyright (C) Darius Plausinaitis + //% Written by Darius Plausinaitis + //%-------------------------------------------------------------------------- + //%This program 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 2 + //%of the License, or (at your option) any later version. + //% + //%This program 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 this program; if not, write to the Free Software + //%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, + //%USA. + //%========================================================================== + // + //%CVS record: + //%$Id: findUtmZone.m,v 1.1.2.2 2006/08/22 13:45:59 dpl Exp $ + // + // % % Check value bounds == == == == == == == == == == == == == == == == == == == == == == == == == == = + + if ((longitude_deg > 180) || (longitude_deg < -180)) + std::cout << "Longitude value exceeds limits (-180:180).\n"; + + + if ((latitude_deg > 84) || (latitude_deg < -80)) + std::cout << "Latitude value exceeds limits (-80:84).\n"; + + // + // % % Find zone == + // == == == == == == == == == == == == == == == == == == == == == == == == == == == == == == + + // % Start at 180 deg west = -180 deg + + int utmZone = floor((180 + longitude_deg) / 6) + 1; + + //%% Correct zone numbers for particular areas ============================== + + if (latitude_deg > 72) + { + // % Corrections for zones 31 33 35 37 + if ((longitude_deg >= 0) && (longitude_deg < 9)) + { + utmZone = 31; + } + else if ((longitude_deg >= 9) && (longitude_deg < 21)) + { + utmZone = 33; + } + else if ((longitude_deg >= 21) && (longitude_deg < 33)) + { + utmZone = 35; + } + else if ((longitude_deg >= 33) && (longitude_deg < 42)) + { + utmZone = 37; + } + } + else if ((latitude_deg >= 56) && (latitude_deg < 64)) + { + // % Correction for zone 32 + if ((longitude_deg >= 3) && (longitude_deg < 12)) + utmZone = 32; + } + return utmZone; +} diff --git a/src/tests/system-tests/libs/geofunctions.h b/src/tests/system-tests/libs/geofunctions.h index bc111cfb1..e31a8a2cb 100644 --- a/src/tests/system-tests/libs/geofunctions.h +++ b/src/tests/system-tests/libs/geofunctions.h @@ -156,5 +156,30 @@ void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_ */ double great_circle_distance(double lat1, double lon1, double lat2, double lon2); +/*! + * \brief Transformation of ECEF (X,Y,Z) to (E,N,U) in UTM, zone 'zone'. + */ + +void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu); + +/*! + * \brief Function finds the UTM zone number for given longitude and latitude. + */ + +int findUtmZone(double latitude_deg, double longitude_deg); + +/*! + * \brief Clenshaw summation of sinus of argument. + */ + +double clsin(arma::colvec ar, int degree, double argument); + + +/*! + * \brief Clenshaw summation of sinus with complex argument. + */ + + +void clksin(arma::colvec ar, int degree, double arg_real, double arg_imag, double *re, double *im); #endif diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 3ba1fc70a..351b95f1f 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -32,6 +32,7 @@ * ------------------------------------------------------------------------- */ +#include "geofunctions.h" #include "position_test_flags.h" #include "rtklib_solver_dump_reader.h" #include "spirent_motion_csv_dump_reader.h" @@ -54,6 +55,7 @@ #include #include #include +#include // For GPS NAVIGATION (L1) concurrent_queue global_gps_acq_assist_queue; @@ -82,118 +84,13 @@ private: std::string filename_rinex_obs = FLAGS_filename_rinex_obs; std::string filename_raw_data = FLAGS_filename_raw_data; - void print_results(const std::vector& east, - const std::vector& north, - const std::vector& up); - - double compute_stdev_precision(const std::vector& vec); - double compute_stdev_accuracy(const std::vector& vec, double ref); - - void geodetic2Enu(const double latitude, const double longitude, const double altitude, - double* east, double* north, double* up); - - void geodetic2Ecef(const double latitude, const double longitude, const double altitude, - double* x, double* y, double* z); - + void print_results(arma::mat R_eb_enu); std::shared_ptr config; std::shared_ptr config_f; std::string generated_kml_file; }; -void PositionSystemTest::geodetic2Ecef(const double latitude, const double longitude, const double altitude, - double* x, double* y, double* z) -{ - const double a = 6378137.0; // WGS84 - const double b = 6356752.314245; // WGS84 - - double aux_x, aux_y, aux_z; - - // Convert to ECEF (See https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates ) - const double cLat = cos(latitude); - const double cLon = cos(longitude); - const double sLon = sin(longitude); - const double sLat = sin(latitude); - double N = std::pow(a, 2.0) / sqrt(std::pow(a, 2.0) * std::pow(cLat, 2.0) + std::pow(b, 2.0) * std::pow(sLat, 2.0)); - - aux_x = (N + altitude) * cLat * cLon; - aux_y = (N + altitude) * cLat * sLon; - aux_z = ((std::pow(b, 2.0) / std::pow(a, 2.0)) * N + altitude) * sLat; - - *x = aux_x; - *y = aux_y; - *z = aux_z; -} - - -void PositionSystemTest::geodetic2Enu(double latitude, double longitude, double altitude, - double* east, double* north, double* up) -{ - double x, y, z; - const double d2r = PI / 180.0; - - geodetic2Ecef(latitude * d2r, longitude * d2r, altitude, &x, &y, &z); - - double aux_north, aux_east, aux_down; - - std::istringstream iss2(FLAGS_static_position); - std::string str_aux; - std::getline(iss2, str_aux, ','); - double ref_long = std::stod(str_aux); - std::getline(iss2, str_aux, ','); - double ref_lat = std::stod(str_aux); - std::getline(iss2, str_aux, '\n'); - double ref_h = std::stod(str_aux); - double ref_x, ref_y, ref_z; - - 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; - - // ECEF to NED matrix - double phiP = atan2(ref_z, sqrt(std::pow(ref_x, 2.0) + std::pow(ref_y, 2.0))); - const double sLat = sin(phiP); - const double sLon = sin(ref_long * d2r); - const double cLat = cos(phiP); - const double cLon = cos(ref_long * d2r); - - aux_north = -aux_x * sLat * cLon - aux_y * sLon + aux_z * cLat * cLon; - aux_east = -aux_x * sLat * sLon + aux_y * cLon + aux_z * cLat * sLon; - aux_down = aux_x * cLat + aux_z * sLat; - - *east = aux_east; - *north = aux_north; - *up = -aux_down; -} - - -double PositionSystemTest::compute_stdev_precision(const std::vector& 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__; -} - - -double PositionSystemTest::compute_stdev_accuracy(const std::vector& vec, const double ref) -{ - const double mean__ = ref; - 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__; -} - - int PositionSystemTest::configure_generator() { // Configure signal generator @@ -261,23 +158,23 @@ int PositionSystemTest::configure_receiver() const int grid_density = 16; const float zero = 0.0; - const int number_of_channels = 12; + const int number_of_channels = 11; 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 float threshold = 2.5; + const float doppler_max = 5000.0; + const float doppler_step = 250.0; + const int max_dwells = 10; 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 pll_bw_hz = 35.0; + const float dll_bw_hz = 1.5; 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 float pll_bw_narrow_hz = 1.0; + const float dll_bw_narrow_hz = 0.1; const int extend_correlation_ms = 1; const int display_rate_ms = 500; @@ -307,7 +204,7 @@ int PositionSystemTest::configure_receiver() // 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.implementation", "Freq_Xlating_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"); @@ -324,7 +221,7 @@ int PositionSystemTest::configure_receiver() 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.filter_type", "lowpass"); 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)); @@ -358,7 +255,6 @@ int PositionSystemTest::configure_receiver() // 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_"); @@ -460,13 +356,10 @@ int PositionSystemTest::run_receiver() void PositionSystemTest::check_results() { - std::vector pos_e; - std::vector pos_n; - std::vector pos_u; - - 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::mat R_eb_e; //ECEF position (x,y,z) estimation in the Earth frame (Nx3) + arma::mat R_eb_enu; //ENU position (N,E,U) estimation in UTM (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) @@ -482,10 +375,15 @@ void PositionSystemTest::check_results() 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); + int utm_zone = findUtmZone(ref_lat, ref_long); + arma::vec v_eb_n = {0.0, 0.0, 0.0}; + arma::vec true_r_eb_e = {0.0, 0.0, 0.0}; + arma::vec true_v_eb_e = {0.0, 0.0, 0.0}; + pv_Geo_to_ECEF(degtorad(ref_lat), degtorad(ref_long), ref_h, v_eb_n, true_r_eb_e, true_v_eb_e); + ref_R_eb_e.insert_cols(0, true_r_eb_e); + arma::vec ref_r_enu = {0, 0, 0}; + cart2utm(true_r_eb_e, utm_zone, ref_r_enu); if (!FLAGS_use_pvt_solver_dump) { //fall back to read receiver KML output (position only) @@ -503,8 +401,8 @@ void PositionSystemTest::check_results() if (found != std::string::npos) is_header = false; } bool is_data = true; - //read data + int64_t current_epoch = 0; while (is_data) { if (!std::getline(myfile, line)) @@ -532,18 +430,20 @@ void PositionSystemTest::check_results() if (i == 2) h = value; } - double north, east, up; - geodetic2Enu(lat, longitude, h, &east, &north, &up); + arma::vec tmp_v_ecef; + arma::vec tmp_r_ecef; + pv_Geo_to_ECEF(degtorad(lat), degtorad(longitude), h, arma::vec{0, 0, 0}, tmp_r_ecef, tmp_v_ecef); + R_eb_e.insert_cols(current_epoch, tmp_r_ecef); + arma::vec tmp_r_enu = {0, 0, 0}; + cart2utm(tmp_r_ecef, utm_zone, tmp_r_enu); + R_eb_enu.insert_cols(current_epoch, tmp_r_enu); // 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"; + ASSERT_FALSE(R_eb_e.n_cols == 0) << "KML file is empty"; } else { @@ -558,16 +458,6 @@ void PositionSystemTest::check_results() int64_t current_epoch = 0; while (pvt_reader.read_binary_obs()) { - double north, east, up; - 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(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]; @@ -578,6 +468,9 @@ void PositionSystemTest::check_results() LLH(current_epoch, 0) = pvt_reader.latitude; LLH(current_epoch, 1) = pvt_reader.longitude; LLH(current_epoch, 2) = pvt_reader.height; + arma::vec tmp_r_enu; + cart2utm(R_eb_e.col(current_epoch), utm_zone, tmp_r_enu); + R_eb_enu.insert_cols(current_epoch, tmp_r_enu); //debug check // std::cout << "t1: " << pvt_reader.RX_time << std::endl; @@ -593,20 +486,21 @@ void PositionSystemTest::check_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_precision = arma::var(R_eb_enu.row(0)); + double sigma_N_2_precision = arma::var(R_eb_enu.row(1)); + double sigma_U_2_precision = arma::var(R_eb_enu.row(2)); - 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); + arma::rowvec tmp_vec; + tmp_vec = R_eb_enu.row(0) - ref_r_enu(0); + double sigma_E_2_accuracy = sqrt(arma::sum(arma::square(tmp_vec)) / tmp_vec.n_cols); + tmp_vec = R_eb_enu.row(1) - ref_r_enu(1); + double sigma_N_2_accuracy = sqrt(arma::sum(arma::square(tmp_vec)) / tmp_vec.n_cols); + tmp_vec = R_eb_enu.row(2) - ref_r_enu(2); + double sigma_U_2_accuracy = sqrt(arma::sum(arma::square(tmp_vec)) / tmp_vec.n_cols); - 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(); + double mean__e = arma::mean(R_eb_enu.row(0)); + double mean__n = arma::mean(R_eb_enu.row(1)); + double mean__u = arma::mean(R_eb_enu.row(2)); std::stringstream stm; std::ofstream position_test_file; @@ -619,20 +513,20 @@ void PositionSystemTest::check_results() 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, ref_n) + 0.56 * compute_stdev_accuracy(pos_e, ref_e) << " [m]" << std::endl; + stm << "CEP = " << 0.62 * sqrt(sigma_N_2_accuracy) + 0.56 * sqrt(sigma_E_2_accuracy) << " [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(abs(mean__e - ref_e), 2.0) + std::pow(abs(mean__n - ref_n), 2.0)) << " [m]" << std::endl; - stm << "Bias 3D = " << sqrt(std::pow(abs(mean__e - ref_e), 2.0) + std::pow(abs(mean__n - ref_n), 2.0) + std::pow(abs(mean__u - ref_u), 2.0)) << " [m]" << std::endl; + stm << "Bias 2D = " << sqrt(std::pow(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0)) << " [m]" << std::endl; + stm << "Bias 3D = " << sqrt(std::pow(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0) + std::pow(fabs(mean__u - ref_r_enu(2)), 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 << "CEP = " << 0.62 * sqrt(sigma_N_2_precision) + 0.56 * sqrt(sigma_E_2_precision) << " [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; @@ -649,11 +543,11 @@ void PositionSystemTest::check_results() // 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); + ASSERT_LT(precision_SEP, 1.0); if (FLAGS_plot_position_test == true) { - print_results(pos_e, pos_n, pos_u); + print_results(R_eb_enu); } } else @@ -844,9 +738,7 @@ void PositionSystemTest::check_results() } -void PositionSystemTest::print_results(const std::vector& east, - const std::vector& north, - const std::vector& up) +void PositionSystemTest::print_results(arma::mat R_eb_enu) { const std::string gnuplot_executable(FLAGS_gnuplot_executable); if (gnuplot_executable.empty()) @@ -857,29 +749,40 @@ void PositionSystemTest::print_results(const std::vector& east, } else { - double sigma_E_2_precision = std::pow(compute_stdev_precision(east), 2.0); - double sigma_N_2_precision = std::pow(compute_stdev_precision(north), 2.0); - double sigma_U_2_precision = std::pow(compute_stdev_precision(up), 2.0); + double sigma_E_2_precision = arma::var(R_eb_enu.row(0)); + double sigma_N_2_precision = arma::var(R_eb_enu.row(1)); + double sigma_U_2_precision = arma::var(R_eb_enu.row(2)); - double mean_east = std::accumulate(east.begin(), east.end(), 0.0) / east.size(); - double mean_north = std::accumulate(north.begin(), north.end(), 0.0) / north.size(); + double mean_east = arma::mean(R_eb_enu.row(0)); + double mean_north = arma::mean(R_eb_enu.row(1)); + double mean_up = arma::mean(R_eb_enu.row(2)); - auto it_max_east = std::max_element(std::begin(east), std::end(east)); - auto it_min_east = std::min_element(std::begin(east), std::end(east)); - auto it_max_north = std::max_element(std::begin(north), std::end(north)); - auto it_min_north = std::min_element(std::begin(north), std::end(north)); - auto it_max_up = std::max_element(std::begin(up), std::end(up)); - auto it_min_up = std::min_element(std::begin(up), std::end(up)); + double it_max_east = arma::max(R_eb_enu.row(0) - mean_east); + double it_min_east = arma::min(R_eb_enu.row(0) - mean_east); - auto east_range = std::max(*it_max_east, std::abs(*it_min_east)); - auto north_range = std::max(*it_max_north, std::abs(*it_min_north)); - auto up_range = std::max(*it_max_up, std::abs(*it_min_up)); + double it_max_north = arma::max(R_eb_enu.row(1) - mean_north); + double it_min_north = arma::min(R_eb_enu.row(1) - mean_north); + + double it_max_up = arma::max(R_eb_enu.row(2) - mean_up); + double it_min_up = arma::min(R_eb_enu.row(2) - mean_up); + + double east_range = std::max(it_max_east, std::abs(it_min_east)); + double north_range = std::max(it_max_north, std::abs(it_min_north)); + double up_range = std::max(it_max_up, std::abs(it_min_up)); double range = std::max(east_range, north_range) * 1.1; double range_3d = std::max(std::max(east_range, north_range), up_range) * 1.1; double two_drms = 2 * sqrt(sigma_E_2_precision + sigma_N_2_precision); double ninty_sas = 0.833 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision); + arma::rowvec arma_east = R_eb_enu.row(0) - mean_east; + arma::rowvec arma_north = R_eb_enu.row(1) - mean_north; + arma::rowvec arma_up = R_eb_enu.row(2) - mean_up; + + std::vector east(arma_east.colptr(0), arma_east.row(0).colptr(0) + arma_east.row(0).n_cols); + std::vector north(arma_north.colptr(0), arma_north.colptr(0) + arma_north.n_cols); + std::vector up(arma_up.colptr(0), arma_up.colptr(0) + arma_up.n_cols); + try { boost::filesystem::path p(gnuplot_executable); @@ -903,6 +806,7 @@ void PositionSystemTest::print_results(const std::vector& east, g1.cmd("set xrange [-" + std::to_string(range) + ":" + std::to_string(range) + "]"); g1.cmd("set yrange [-" + std::to_string(range) + ":" + std::to_string(range) + "]"); + g1.plot_xy(east, north, "2D Position Fixes"); g1.set_style("lines").plot_circle(mean_east, mean_north, two_drms, "2DRMS"); g1.set_style("lines").plot_circle(mean_east, mean_north, two_drms / 2.0, "DRMS"); From 10185708679e08ef363ec7bc6ab97c0a7b1ba79b Mon Sep 17 00:00:00 2001 From: Javier Date: Thu, 11 Oct 2018 17:49:42 +0200 Subject: [PATCH 04/10] Set a more restrictive threshold for the RTKLIB solver test --- .../signal-processing-blocks/pvt/rtklib_solver_test.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc index bc073d587..22615cc2a 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc @@ -452,7 +452,7 @@ TEST(RTKLibSolverTest, test1) std::cout << "3D positioning error: " << error_3d_m << " [meters]" << std::endl; //check results against the test tolerance - ASSERT_LT(error_3d_m, 1.0); + ASSERT_LT(error_3d_m, 0.2); pvt_valid = true; } } From b61bbbb346188e653ce5e861c2c65d1022653601 Mon Sep 17 00:00:00 2001 From: Javier Date: Thu, 11 Oct 2018 22:28:07 +0200 Subject: [PATCH 05/10] Fix matrix dimensions in position_test --- src/tests/system-tests/position_test.cc | 124 ++++++++++++------------ 1 file changed, 63 insertions(+), 61 deletions(-) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 351b95f1f..0e917476d 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -451,24 +451,25 @@ void PositionSystemTest::check_results() 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); + R_eb_e = arma::zeros(3, n_epochs); + V_eb_e = arma::zeros(3, n_epochs); + LLH = arma::zeros(3, n_epochs); receiver_time_s = arma::zeros(n_epochs, 1); int64_t current_epoch = 0; while (pvt_reader.read_binary_obs()) { 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; - arma::vec tmp_r_enu; + R_eb_e(0, current_epoch) = pvt_reader.rr[0]; + R_eb_e(1, current_epoch) = pvt_reader.rr[1]; + R_eb_e(2, current_epoch) = pvt_reader.rr[2]; + V_eb_e(0, current_epoch) = pvt_reader.rr[3]; + V_eb_e(1, current_epoch) = pvt_reader.rr[4]; + V_eb_e(2, current_epoch) = pvt_reader.rr[5]; + LLH(0, current_epoch) = pvt_reader.latitude; + LLH(1, current_epoch) = pvt_reader.longitude; + LLH(2, current_epoch) = pvt_reader.height; + + arma::vec tmp_r_enu = {0, 0, 0}; cart2utm(R_eb_e.col(current_epoch), utm_zone, tmp_r_enu); R_eb_enu.insert_cols(current_epoch, tmp_r_enu); @@ -508,20 +509,18 @@ void PositionSystemTest::check_results() { stm << "Configuration file: " << FLAGS_config_file_ptest << std::endl; } - if (FLAGS_static_scenario) - { - 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 * sqrt(sigma_N_2_accuracy) + 0.56 * sqrt(sigma_E_2_accuracy) << " [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(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0)) << " [m]" << std::endl; - stm << "Bias 3D = " << sqrt(std::pow(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0) + std::pow(fabs(mean__u - ref_r_enu(2)), 2.0)) << " [m]" << std::endl; - stm << std::endl; - } + + 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 * sqrt(sigma_N_2_accuracy) + 0.56 * sqrt(sigma_E_2_accuracy) << " [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(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0)) << " [m]" << std::endl; + stm << "Bias 3D = " << sqrt(std::pow(fabs(mean__e - ref_r_enu(0)), 2.0) + std::pow(fabs(mean__n - ref_r_enu(1)), 2.0) + std::pow(fabs(mean__u - ref_r_enu(2)), 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; @@ -556,56 +555,55 @@ void PositionSystemTest::check_results() 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_R_eb_e = arma::zeros(3, n_epochs); + ref_V_eb_e = arma::zeros(3, n_epochs); + ref_LLH = arma::zeros(3, n_epochs); 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; + ref_R_eb_e(0, current_epoch) = ref_reader.Pos_X; + ref_R_eb_e(1, current_epoch) = ref_reader.Pos_Y; + ref_R_eb_e(2, current_epoch) = ref_reader.Pos_Z; + ref_V_eb_e(0, current_epoch) = ref_reader.Vel_X; + ref_V_eb_e(1, current_epoch) = ref_reader.Vel_Y; + ref_V_eb_e(2, current_epoch) = ref_reader.Vel_Z; + ref_LLH(0, current_epoch) = ref_reader.Lat; + ref_LLH(1, current_epoch) = ref_reader.Long; + ref_LLH(2, current_epoch) = ref_reader.Height; current_epoch++; } - //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::mat ref_interp_R_eb_e = arma::zeros(3, R_eb_e.n_cols); + arma::mat ref_interp_V_eb_e = arma::zeros(3, V_eb_e.n_cols); + arma::mat ref_interp_LLH = arma::zeros(3, LLH.n_cols); 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; + arma::interp1(ref_time_s, ref_R_eb_e.row(n), receiver_time_s, tmp_vector); + ref_interp_R_eb_e.row(n) = tmp_vector.t(); + arma::interp1(ref_time_s, ref_V_eb_e.row(n), receiver_time_s, tmp_vector); + ref_interp_V_eb_e.row(n) = tmp_vector.t(); + arma::interp1(ref_time_s, ref_LLH.row(n), receiver_time_s, tmp_vector); + ref_interp_LLH.row(n) = tmp_vector.t(); } //compute error vectors - - 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); + arma::mat error_R_eb_e = arma::zeros(3, R_eb_e.n_cols); + arma::mat error_V_eb_e = arma::zeros(3, V_eb_e.n_cols); + arma::mat error_LLH = arma::zeros(3, LLH.n_cols); 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++) + arma::vec error_module_R_eb_e = arma::zeros(R_eb_e.n_cols, 1); + arma::vec error_module_V_eb_e = arma::zeros(V_eb_e.n_cols, 1); + for (uint64_t n = 0; n < R_eb_e.n_cols; 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_module_R_eb_e(n) = arma::norm(error_R_eb_e.col(n)); + error_module_V_eb_e(n) = arma::norm(error_V_eb_e.col(n)); } + //Error statistics arma::vec tmp_vec; //RMSE, Mean, Variance and peaks @@ -657,9 +655,13 @@ void PositionSystemTest::check_results() g1.set_title("3D ECEF error coordinates"); g1.set_grid(); //conversion between arma::vec and std:vector - std::vector X(error_R_eb_e.colptr(0), error_R_eb_e.colptr(0) + error_R_eb_e.n_rows); - std::vector Y(error_R_eb_e.colptr(1), error_R_eb_e.colptr(1) + error_R_eb_e.n_rows); - std::vector Z(error_R_eb_e.colptr(2), error_R_eb_e.colptr(2) + error_R_eb_e.n_rows); + arma::rowvec arma_vec_error_x = error_R_eb_e.row(0); + arma::rowvec arma_vec_error_y = error_R_eb_e.row(1); + arma::rowvec arma_vec_error_z = error_R_eb_e.row(2); + + std::vector X(arma_vec_error_x.colptr(0), arma_vec_error_x.colptr(0) + arma_vec_error_x.n_rows); + std::vector Y(arma_vec_error_y.colptr(0), arma_vec_error_y.colptr(0) + arma_vec_error_y.n_rows); + std::vector Z(arma_vec_error_z.colptr(0), arma_vec_error_z.colptr(0) + arma_vec_error_z.n_rows); g1.cmd("set key box opaque"); g1.plot_xyz(X, Y, Z, "ECEF 3D error"); From 9b91404ecbc970efbdf964e07e67781a03acb70d Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 13 Oct 2018 11:05:01 +0200 Subject: [PATCH 06/10] Fix test if gnuplot is not present --- src/tests/system-tests/position_test.cc | 190 ++++++++++++------------ 1 file changed, 99 insertions(+), 91 deletions(-) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 0e917476d..9cdd00740 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -265,7 +265,8 @@ int PositionSystemTest::configure_receiver() 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_symbols", std::to_string(extend_correlation_ms)); - + config->set_property("Tracking_1C.high_dyn", "true"); + config->set_property("Tracking_1C.smoother_length", "200"); // Set Telemetry config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder"); config->set_property("TelemetryDecoder_1C.dump", "false"); @@ -277,7 +278,7 @@ int PositionSystemTest::configure_receiver() // Set PVT config->set_property("PVT.implementation", "RTKLIB_PVT"); - config->set_property("PVT.positioning_mode", "Single"); + config->set_property("PVT.positioning_mode", "PPP_Static"); 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"); @@ -642,99 +643,106 @@ void PositionSystemTest::check_results() << " [m/s]" << std::endl; std::cout.precision(ss); - //plots - Gnuplot g1("points"); - if (FLAGS_show_plots) + // plots + if (FLAGS_plot_position_test == true) { - g1.showonscreen(); // window output - } - else - { - g1.disablescreen(); - } - g1.set_title("3D ECEF error coordinates"); - g1.set_grid(); - //conversion between arma::vec and std:vector - arma::rowvec arma_vec_error_x = error_R_eb_e.row(0); - arma::rowvec arma_vec_error_y = error_R_eb_e.row(1); - arma::rowvec arma_vec_error_z = error_R_eb_e.row(2); + const std::string gnuplot_executable(FLAGS_gnuplot_executable); + if (!gnuplot_executable.empty()) + { + 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 + arma::rowvec arma_vec_error_x = error_R_eb_e.row(0); + arma::rowvec arma_vec_error_y = error_R_eb_e.row(1); + arma::rowvec arma_vec_error_z = error_R_eb_e.row(2); - std::vector X(arma_vec_error_x.colptr(0), arma_vec_error_x.colptr(0) + arma_vec_error_x.n_rows); - std::vector Y(arma_vec_error_y.colptr(0), arma_vec_error_y.colptr(0) + arma_vec_error_y.n_rows); - std::vector Z(arma_vec_error_z.colptr(0), arma_vec_error_z.colptr(0) + arma_vec_error_z.n_rows); + std::vector X(arma_vec_error_x.colptr(0), arma_vec_error_x.colptr(0) + arma_vec_error_x.n_rows); + std::vector Y(arma_vec_error_y.colptr(0), arma_vec_error_y.colptr(0) + arma_vec_error_y.n_rows); + std::vector Z(arma_vec_error_z.colptr(0), arma_vec_error_z.colptr(0) + arma_vec_error_z.n_rows); - g1.cmd("set key box opaque"); - g1.plot_xyz(X, Y, Z, "ECEF 3D error"); - g1.set_legend(); - if (FLAGS_config_file_ptest.empty()) - { - g1.savetops("ECEF_3d_error"); - } - else - { - g1.savetops("ECEF_3d_error_" + config_filename_no_extension); - } - 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 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"); - double mean3d = std::accumulate(error_vec.begin(), error_vec.end(), 0.0) / error_vec.size(); - std::vector error_mean(error_module_R_eb_e.n_rows, mean3d); - g3.set_style("lines"); - g3.plot_xy(time_vector_from_start_s, error_mean, "Mean"); - g3.set_legend(); - if (FLAGS_config_file_ptest.empty()) - { - g3.savetops("Position_3d_error"); - } - else - { - g3.savetops("Position_3d_error_" + config_filename_no_extension); - } + g1.cmd("set key box opaque"); + g1.plot_xyz(X, Y, Z, "ECEF 3D error"); + g1.set_legend(); + if (FLAGS_config_file_ptest.empty()) + { + g1.savetops("ECEF_3d_error"); + } + else + { + g1.savetops("ECEF_3d_error_" + config_filename_no_extension); + } + 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 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"); + double mean3d = std::accumulate(error_vec.begin(), error_vec.end(), 0.0) / error_vec.size(); + std::vector error_mean(error_module_R_eb_e.n_rows, mean3d); + g3.set_style("lines"); + g3.plot_xy(time_vector_from_start_s, error_mean, "Mean"); + g3.set_legend(); + if (FLAGS_config_file_ptest.empty()) + { + g3.savetops("Position_3d_error"); + } + else + { + g3.savetops("Position_3d_error_" + config_filename_no_extension); + } - 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 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"); - double mean3dv = std::accumulate(error_vec2.begin(), error_vec2.end(), 0.0) / error_vec2.size(); - std::vector error_mean_v(error_module_V_eb_e.n_rows, mean3dv); - g4.set_style("lines"); - g4.plot_xy(time_vector_from_start_s, error_mean_v, "Mean"); - g4.set_legend(); - if (FLAGS_config_file_ptest.empty()) - { - g4.savetops("Velocity_3d_error"); - } - else - { - g4.savetops("Velocity_3d_error_" + config_filename_no_extension); + 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 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"); + double mean3dv = std::accumulate(error_vec2.begin(), error_vec2.end(), 0.0) / error_vec2.size(); + std::vector error_mean_v(error_module_V_eb_e.n_rows, mean3dv); + g4.set_style("lines"); + g4.plot_xy(time_vector_from_start_s, error_mean_v, "Mean"); + g4.set_legend(); + if (FLAGS_config_file_ptest.empty()) + { + g4.savetops("Velocity_3d_error"); + } + else + { + g4.savetops("Velocity_3d_error_" + config_filename_no_extension); + } + } } } } From afb7d71c09b4232c86ca7a86a708e9a4600ffd54 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 13 Oct 2018 11:05:54 +0200 Subject: [PATCH 07/10] Move comment to log --- .../observables/gnuradio_blocks/hybrid_observables_cc.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index cb5468af0..81d385cf5 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -484,7 +484,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused) if (T_rx_clock_step_samples == 0) { T_rx_clock_step_samples = std::round(static_cast(in[d_nchannels_in - 1][0].fs) * 1e-3); // 1 ms - std::cout << "Observables clock step samples set to " << T_rx_clock_step_samples << std::endl; + LOG(INFO) << "Observables clock step samples set to " << T_rx_clock_step_samples; usleep(1000000); } From 2f4c24e94ef134a70a84000d6db7bf1595560acf Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 13 Oct 2018 11:15:04 +0200 Subject: [PATCH 08/10] Remove trial parameters --- src/tests/system-tests/position_test.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 9cdd00740..a306b30f1 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -265,8 +265,9 @@ int PositionSystemTest::configure_receiver() 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_symbols", std::to_string(extend_correlation_ms)); - config->set_property("Tracking_1C.high_dyn", "true"); - config->set_property("Tracking_1C.smoother_length", "200"); + //config->set_property("Tracking_1C.high_dyn", "true"); + //config->set_property("Tracking_1C.smoother_length", "200"); + // Set Telemetry config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder"); config->set_property("TelemetryDecoder_1C.dump", "false"); From 9710fa9430896477e2bee160db1c338472100e60 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 13 Oct 2018 14:31:00 +0200 Subject: [PATCH 09/10] Fix CMake script, avoid GPSTK warnings --- src/tests/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 99a375beb..9e8d519b6 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -229,7 +229,8 @@ if(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA) if(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK) message(STATUS "GPSTk v${GNSSSDR_GPSTK_LOCAL_VERSION} will be automatically downloaded and built when doing 'make'.") if ("${TOOLCHAIN_ARG}" STREQUAL "") - set(${TOOLCHAIN_ARG} "-DCMAKE_CXX_FLAGS=-Wno-deprecated") + set(TOOLCHAIN_ARG "-DCMAKE_CXX_FLAGS=\"-Wno-deprecated\"") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated") endif("${TOOLCHAIN_ARG}" STREQUAL "") # if(NOT ENABLE_FPGA) if(CMAKE_VERSION VERSION_LESS 3.2) From 21eb9517fe5ce1a52d60a20a244ead1dfbf31164 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Mon, 15 Oct 2018 02:04:15 +0200 Subject: [PATCH 10/10] Avoid passing Armadillo vectors by value, code cleaning --- src/tests/system-tests/libs/geofunctions.cc | 338 +++++++++----------- src/tests/system-tests/libs/geofunctions.h | 17 +- 2 files changed, 158 insertions(+), 197 deletions(-) diff --git a/src/tests/system-tests/libs/geofunctions.cc b/src/tests/system-tests/libs/geofunctions.cc index 1f372e8c2..a99e2572e 100644 --- a/src/tests/system-tests/libs/geofunctions.cc +++ b/src/tests/system-tests/libs/geofunctions.cc @@ -28,11 +28,10 @@ * * ------------------------------------------------------------------------- */ -#include "geofunctions.h" -#include -const double STRP_G_SI = 9.80665; -const double STRP_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E +#include "geofunctions.h" + +const double STRP_PI = 3.1415926535898; // Pi as defined in IS-GPS-200E arma::mat Skew_symmetric(const arma::vec &a) { @@ -206,17 +205,17 @@ int togeod(double *dphi, double *dlambda, double *h, double a, double finv, doub cosphi = cos(*dphi); // compute radius of curvature in prime vertical direction - N_phi = a / sqrt(1 - esq * sinphi * sinphi); + N_phi = a / sqrt(1.0 - esq * sinphi * sinphi); - // compute residuals in P and Z + // compute residuals in P and Z dP = P - (N_phi + (*h)) * cosphi; dZ = Z - (N_phi * oneesq + (*h)) * sinphi; - // update height and latitude + // update height and latitude *h = *h + (sinphi * dZ + cosphi * dP); *dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h)); - // test for convergence + // test for convergence if ((dP * dP + dZ * dZ) < tolsq) { break; @@ -234,7 +233,7 @@ int togeod(double *dphi, double *dlambda, double *h, double a, double finv, doub arma::mat Gravity_ECEF(const arma::vec &r_eb_e) { // Parameters - const double R_0 = 6378137; // WGS84 Equatorial radius in meters + const double R_0 = 6378137.0; // WGS84 Equatorial radius in meters const double mu = 3.986004418E14; // WGS84 Earth gravitational constant (m^3 s^-2) const double J_2 = 1.082627E-3; // WGS84 Earth's second gravitational constant const double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s) @@ -260,12 +259,14 @@ arma::mat Gravity_ECEF(const arma::vec &r_eb_e) } -arma::vec LLH_to_deg(arma::vec &LLH) +arma::vec LLH_to_deg(const arma::vec &LLH) { const double rtd = 180.0 / STRP_PI; - LLH(0) = LLH(0) * rtd; - LLH(1) = LLH(1) * rtd; - return LLH; + arma::vec deg = arma::zeros(3, 1); + deg(0) = LLH(0) * rtd; + deg(1) = LLH(1) * rtd; + deg(2) = LLH(2); + return deg; } @@ -297,15 +298,16 @@ double mstokph(double MetersPerSeconds) } -arma::vec CTM_to_Euler(arma::mat &C) +arma::vec CTM_to_Euler(const arma::mat &C) { // Calculate Euler angles using (2.23) + arma::mat CTM = C; 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 + eul(0) = atan2(CTM(1, 2), CTM(2, 2)); // roll + if (CTM(0, 2) < -1.0) CTM(0, 2) = -1.0; + if (CTM(0, 2) > 1.0) CTM(0, 2) = 1.0; + eul(1) = -asin(CTM(0, 2)); // pitch + eul(2) = atan2(CTM(0, 1), CTM(0, 0)); // yaw return eul; } @@ -354,19 +356,19 @@ arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection) do { oldh = h; - N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi))); + N = c / sqrt(1.0 + 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; + // std::cout << "Failed to approximate h with desired precision. h-oldh= " << h - oldh; break; } } while (std::fabs(h - oldh) > 1.0e-12); - arma::vec LLH = {{phi, lambda, h}}; //radians + arma::vec LLH = {{phi, lambda, h}}; // radians return LLH; } @@ -399,11 +401,11 @@ void ECEF_to_Geo(const arma::vec &r_eb_e, const arma::vec &v_eb_e, const arma::m void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const 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 R_0 = 6378137.0; // 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)))); + double R_E = R_0 / sqrt(1.0 - (e * sin(LLH(0))) * (e * sin(LLH(0)))); // Convert position using (2.112) double cos_lat = cos(LLH(0)); @@ -435,7 +437,7 @@ void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e) { // Parameters - const double R_0 = 6378137; // WGS84 Equatorial radius in meters + const double R_0 = 6378137.0; // WGS84 Equatorial radius in meters const double e = 0.0818191908425; // WGS84 eccentricity // Calculate transverse radius of curvature using (2.105) @@ -460,9 +462,10 @@ void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_ v_eb_e = C_e_n.t() * v_eb_n; } + double great_circle_distance(double lat1, double lon1, double lat2, double lon2) { - //The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes. + // The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes. // generally used geo measurement function double R = 6378.137; // Radius of earth in KM double dLat = lat2 * STRP_PI / 180.0 - lat1 * STRP_PI / 180.0; @@ -475,53 +478,49 @@ double great_circle_distance(double lat1, double lon1, double lat2, double lon2) return d * 1000.0; // meters } -void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu) + +void cart2utm(const arma::vec &r_eb_e, int zone, arma::vec &r_enu) { - //%CART2UTM Transformation of (X,Y,Z) to (E,N,U) in UTM, zone 'zone'. - //% - //%[E, N, U] = cart2utm(X, Y, Z, zone); - //% - //% Inputs: - //% X,Y,Z - Cartesian coordinates. Coordinates are referenced - //% with respect to the International Terrestrial Reference - //% Frame 1996 (ITRF96) - //% zone - UTM zone of the given position - //% - //% Outputs: - //% E, N, U - UTM coordinates (Easting, Northing, Uping) + // Transformation of (X,Y,Z) to (E,N,U) in UTM, zone 'zone' // - //%Kai Borre -11-1994 - //%Copyright (c) by Kai Borre - //% - //% CVS record: - //% $Id: cart2utm.m,v 1.1.1.1.2.6 2007/01/30 09:45:12 dpl Exp $ + // Inputs: + // r_eb_e - Cartesian coordinates. Coordinates are referenced + // with respect to the International Terrestrial Reference + // Frame 1996 (ITRF96) + // zone - UTM zone of the given position // - //%This implementation is based upon - //%O. Andersson & K. Poder (1981) Koordinattransformationer - //% ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren - //% Vol. 30: 552--571 and Vol. 31: 76 - //% - //%An excellent, general reference (KW) is - //%R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der - //% h\"oheren Geod\"asie und Kartographie. - //% Erster Band, Springer Verlag + // Outputs: + // r_enu - UTM coordinates (Easting, Northing, Uping) // - //% Explanation of variables used: - //% f flattening of ellipsoid - //% a semi major axis in m - //% m0 1 - scale at central meridian; for UTM 0.0004 - //% Q_n normalized meridian quadrant - //% E0 Easting of central meridian - //% L0 Longitude of central meridian - //% bg constants for ellipsoidal geogr. to spherical geogr. - //% gb constants for spherical geogr. to ellipsoidal geogr. - //% gtu constants for ellipsoidal N, E to spherical N, E - //% utg constants for spherical N, E to ellipoidal N, E - //% tolutm tolerance for utm, 1.2E-10*meridian quadrant - //% tolgeo tolerance for geographical, 0.00040 second of arc + // Originally written in Matlab by Kai Borre, Nov. 1994 + // Implemented in C++ by J.Arribas // - //% B, L refer to latitude and longitude. Southern latitude is negative - //% International ellipsoid of 1924, valid for ED50 + // This implementation is based upon + // O. Andersson & K. Poder (1981) Koordinattransformationer + // ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren + // Vol. 30: 552--571 and Vol. 31: 76 + // + // An excellent, general reference (KW) is + // R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der + // h\"oheren Geod\"asie und Kartographie. + // Erster Band, Springer Verlag + // + // Explanation of variables used: + // f flattening of ellipsoid + // a semi major axis in m + // m0 1 - scale at central meridian; for UTM 0.0004 + // Q_n normalized meridian quadrant + // E0 Easting of central meridian + // L0 Longitude of central meridian + // bg constants for ellipsoidal geogr. to spherical geogr. + // gb constants for spherical geogr. to ellipsoidal geogr. + // gtu constants for ellipsoidal N, E to spherical N, E + // utg constants for spherical N, E to ellipoidal N, E + // tolutm tolerance for utm, 1.2E-10*meridian quadrant + // tolgeo tolerance for geographical, 0.00040 second of arc + // + // B, L refer to latitude and longitude. Southern latitude is negative + // International ellipsoid of 1924, valid for ED50 double a = 6378388.0; double f = 1.0 / 297.0; @@ -535,10 +534,10 @@ void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu) double scale = 0.9999988; arma::vec v = scale * R * vec + trans; // coordinate vector in ED50 double L = atan2(v(1), v(0)); - double N1 = 6395000; // preliminary value - double B = atan2(v(2) / ((1 - f) * (1 - f) * N1), arma::norm(v.subvec(0, 1)) / N1); // preliminary value + double N1 = 6395000.0; // preliminary value + double B = atan2(v(2) / ((1.0 - f) * (1.0 - f) * N1), arma::norm(v.subvec(0, 1)) / N1); // preliminary value double U = 0.1; - double oldU = 0; + double oldU = 0.0; int iterations = 0; while (fabs(U - oldU) > 1.0E-4) { @@ -553,44 +552,44 @@ void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu) break; } } - //%Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21) + // Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21) double m0 = 0.0004; double n = f / (2.0 - f); - double m = n * n * (1 / 4 + n * n / 64); - double w = (a * (-n - m0 + m * (1 - m0))) / (1 + n); + double m = n * n * (1.0 / 4.0 + n * n / 64.0); + double w = (a * (-n - m0 + m * (1.0 - m0))) / (1.0 + n); double Q_n = a + w; - //%Easting and longitude of central meridian - double E0 = 500000; - double L0 = (zone - 30.0) * 6.0 - 3.0; + // Easting and longitude of central meridian + double E0 = 500000.0; + double L0 = (zone - 30) * 6.0 - 3.0; - //%Check tolerance for reverse transformation - //double tolutm = STRP_PI / 2.0 * 1.2e-10 * Q_n; - //double tolgeo = 0.000040; - // % Coefficients of trigonometric series - - // % ellipsoidal to spherical geographical, KW p .186 --187, (51) - (52) - // % bg[1] = n * (-2 + n * (2 / 3 + n * (4 / 3 + n * (-82 / 45)))); - // % bg[2] = n ^ 2 * (5 / 3 + n * (-16 / 15 + n * (-13 / 9))); - // % bg[3] = n ^ 3 * (-26 / 15 + n * 34 / 21); - // % bg[4] = n ^ 4 * 1237 / 630; + // Check tolerance for reverse transformation + // double tolutm = STRP_PI / 2.0 * 1.2e-10 * Q_n; + // double tolgeo = 0.000040; + // Coefficients of trigonometric series // - // % spherical to ellipsoidal geographical, KW p.190 --191, (61) - (62) % gb[1] = n * (2 + n * (-2 / 3 + n * (-2 + n * 116 / 45))); - // % gb[2] = n ^ 2 * (7 / 3 + n * (-8 / 5 + n * (-227 / 45))); - // % gb[3] = n ^ 3 * (56 / 15 + n * (-136 / 35)); - // % gb[4] = n ^ 4 * 4279 / 630; + // ellipsoidal to spherical geographical, KW p .186 --187, (51) - (52) + // bg[1] = n * (-2 + n * (2 / 3 + n * (4 / 3 + n * (-82 / 45)))); + // bg[2] = n ^ 2 * (5 / 3 + n * (-16 / 15 + n * (-13 / 9))); + // bg[3] = n ^ 3 * (-26 / 15 + n * 34 / 21); + // bg[4] = n ^ 4 * 1237 / 630; // - // % spherical to ellipsoidal N, E, KW p.196, (69) % gtu[1] = n * (1 / 2 + n * (-2 / 3 + n * (5 / 16 + n * 41 / 180))); - // % gtu[2] = n ^ 2 * (13 / 48 + n * (-3 / 5 + n * 557 / 1440)); - // % gtu[3] = n ^ 3 * (61 / 240 + n * (-103 / 140)); - // % gtu[4] = n ^ 4 * 49561 / 161280; + // spherical to ellipsoidal geographical, KW p.190 --191, (61) - (62) % gb[1] = n * (2 + n * (-2 / 3 + n * (-2 + n * 116 / 45))); + // gb[2] = n ^ 2 * (7 / 3 + n * (-8 / 5 + n * (-227 / 45))); + // gb[3] = n ^ 3 * (56 / 15 + n * (-136 / 35)); + // gb[4] = n ^ 4 * 4279 / 630; // - // % ellipsoidal to spherical N, E, KW p.194, (65) % utg[1] = n * (-1 / 2 + n * (2 / 3 + n * (-37 / 96 + n * 1 / 360))); - // % utg[2] = n ^ 2 * (-1 / 48 + n * (-1 / 15 + n * 437 / 1440)); - // % utg[3] = n ^ 3 * (-17 / 480 + n * 37 / 840); - // % utg[4] = n ^ 4 * (-4397 / 161280); - - // % With f = 1 / 297 we get + // spherical to ellipsoidal N, E, KW p.196, (69) % gtu[1] = n * (1 / 2 + n * (-2 / 3 + n * (5 / 16 + n * 41 / 180))); + // gtu[2] = n ^ 2 * (13 / 48 + n * (-3 / 5 + n * 557 / 1440)); + // gtu[3] = n ^ 3 * (61 / 240 + n * (-103 / 140)); + // gtu[4] = n ^ 4 * 49561 / 161280; + // + // ellipsoidal to spherical N, E, KW p.194, (65) % utg[1] = n * (-1 / 2 + n * (2 / 3 + n * (-37 / 96 + n * 1 / 360))); + // utg[2] = n ^ 2 * (-1 / 48 + n * (-1 / 15 + n * 437 / 1440)); + // utg[3] = n ^ 3 * (-17 / 480 + n * 37 / 840); + // utg[4] = n ^ 4 * (-4397 / 161280); + // + // With f = 1 / 297 we get arma::colvec bg = {-3.37077907e-3, 4.73444769e-6, @@ -612,23 +611,23 @@ void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu) -1.69485209e-10, -2.20473896e-13}; - // % Ellipsoidal latitude, longitude to spherical latitude, longitude + // Ellipsoidal latitude, longitude to spherical latitude, longitude bool neg_geo = false; - if (B < 0) neg_geo = true; + if (B < 0.0) neg_geo = true; double Bg_r = fabs(B); - double res_clensin = clsin(bg, 4, 2 * Bg_r); + double res_clensin = clsin(bg, 4, 2.0 * Bg_r); Bg_r = Bg_r + res_clensin; L0 = L0 * STRP_PI / 180.0; double Lg_r = L - L0; - // % Spherical latitude, longitude to complementary spherical latitude % i.e.spherical N, E + // Spherical latitude, longitude to complementary spherical latitude % i.e.spherical N, E double cos_BN = cos(Bg_r); double Np = atan2(sin(Bg_r), cos(Lg_r) * cos_BN); double Ep = atanh(sin(Lg_r) * cos_BN); - // % Spherical normalized N, E to ellipsoidal N, E + // Spherical normalized N, E to ellipsoidal N, E Np = 2.0 * Np; Ep = 2.0 * Ep; @@ -651,24 +650,18 @@ void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu) } -double clsin(arma::colvec ar, int degree, double argument) +double clsin(const arma::colvec &ar, int degree, double argument) { - //%Clenshaw summation of sinus of argument. - //% - //%result = clsin(ar, degree, argument); + // Clenshaw summation of sinus of argument. // - //% Written by Kai Borre - //% December 20, 1995 - //% - //% See also WGS2UTM or CART2UTM - //% - //% CVS record: - //% $Id: clsin.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ - //%========================================================================== + // result = clsin(ar, degree, argument); + // + // Originally written in Matlab by Kai Borre + // Implemented in C++ by J.Arribas double cos_arg = 2.0 * cos(argument); - double hr1 = 0; - double hr = 0; + double hr1 = 0.0; + double hr = 0.0; double hr2; for (int t = degree; t > 0; t--) { @@ -681,32 +674,26 @@ double clsin(arma::colvec ar, int degree, double argument) } -void clksin(arma::colvec ar, int degree, double arg_real, double arg_imag, double *re, double *im) +void clksin(const arma::colvec &ar, int degree, double arg_real, double arg_imag, double *re, double *im) { - //%Clenshaw summation of sinus with complex argument - //%[re, im] = clksin(ar, degree, arg_real, arg_imag); + // Clenshaw summation of sinus with complex argument + // [re, im] = clksin(ar, degree, arg_real, arg_imag); // - //% Written by Kai Borre - //% December 20, 1995 - //% - //% See also WGS2UTM or CART2UTM - //% - //% CVS record: - //% $Id: clksin.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $ - //%========================================================================== + // Originally written in Matlab by Kai Borre + // Implemented in C++ by J.Arribas double sin_arg_r = sin(arg_real); double cos_arg_r = cos(arg_real); double sinh_arg_i = sinh(arg_imag); double cosh_arg_i = cosh(arg_imag); - double r = 2 * cos_arg_r * cosh_arg_i; - double i = -2 * sin_arg_r * sinh_arg_i; + double r = 2.0 * cos_arg_r * cosh_arg_i; + double i = -2.0 * sin_arg_r * sinh_arg_i; - double hr1 = 0; - double hr = 0; - double hi1 = 0; - double hi = 0; + double hr1 = 0.0; + double hr = 0.0; + double hi1 = 0.0; + double hi = 0.0; double hi2; double hr2; for (int t = degree; t > 0; t--) @@ -727,86 +714,61 @@ void clksin(arma::colvec ar, int degree, double arg_real, double arg_imag, doubl *im = r * hi + i * hr; } + int findUtmZone(double latitude_deg, double longitude_deg) { - //%Function finds the UTM zone number for given longitude and latitude. - //%The longitude value must be between -180 (180 degree West) and 180 (180 - //%degree East) degree. The latitude must be within -80 (80 degree South) and - //%84 (84 degree North). - //% - //%utmZone = findUtmZone(latitude, longitude); - //% - //%Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not - //%15 deg 30 min). + // Function finds the UTM zone number for given longitude and latitude. + // The longitude value must be between -180 (180 degree West) and 180 (180 + // degree East) degree. The latitude must be within -80 (80 degree South) and + // 84 (84 degree North). // - //%-------------------------------------------------------------------------- - //% SoftGNSS v3.0 - //% - //% Copyright (C) Darius Plausinaitis - //% Written by Darius Plausinaitis - //%-------------------------------------------------------------------------- - //%This program 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 2 - //%of the License, or (at your option) any later version. - //% - //%This program 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 this program; if not, write to the Free Software - //%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, - //%USA. - //%========================================================================== + // utmZone = findUtmZone(latitude, longitude); // - //%CVS record: - //%$Id: findUtmZone.m,v 1.1.2.2 2006/08/22 13:45:59 dpl Exp $ + // Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not + // 15 deg 30 min). // - // % % Check value bounds == == == == == == == == == == == == == == == == == == == == == == == == == == = + // Originally written in Matlab by Darius Plausinaitis + // Implemented in C++ by J.Arribas - if ((longitude_deg > 180) || (longitude_deg < -180)) + // Check value bounds + if ((longitude_deg > 180.0) || (longitude_deg < -180.0)) std::cout << "Longitude value exceeds limits (-180:180).\n"; - - if ((latitude_deg > 84) || (latitude_deg < -80)) + if ((latitude_deg > 84.0) || (latitude_deg < -80.0)) std::cout << "Latitude value exceeds limits (-80:84).\n"; // - // % % Find zone == - // == == == == == == == == == == == == == == == == == == == == == == == == == == == == == == - - // % Start at 180 deg west = -180 deg + // Find zone + // + // Start at 180 deg west = -180 deg int utmZone = floor((180 + longitude_deg) / 6) + 1; - //%% Correct zone numbers for particular areas ============================== - - if (latitude_deg > 72) + // Correct zone numbers for particular areas + if (latitude_deg > 72.0) { - // % Corrections for zones 31 33 35 37 - if ((longitude_deg >= 0) && (longitude_deg < 9)) + // Corrections for zones 31 33 35 37 + if ((longitude_deg >= 0.0) && (longitude_deg < 9.0)) { utmZone = 31; } - else if ((longitude_deg >= 9) && (longitude_deg < 21)) + else if ((longitude_deg >= 9.0) && (longitude_deg < 21.0)) { utmZone = 33; } - else if ((longitude_deg >= 21) && (longitude_deg < 33)) + else if ((longitude_deg >= 21.0) && (longitude_deg < 33.0)) { utmZone = 35; } - else if ((longitude_deg >= 33) && (longitude_deg < 42)) + else if ((longitude_deg >= 33.0) && (longitude_deg < 42.0)) { utmZone = 37; } } - else if ((latitude_deg >= 56) && (latitude_deg < 64)) + else if ((latitude_deg >= 56.0) && (latitude_deg < 64.0)) { - // % Correction for zone 32 - if ((longitude_deg >= 3) && (longitude_deg < 12)) + // Correction for zone 32 + if ((longitude_deg >= 3.0) && (longitude_deg < 12.0)) utmZone = 32; } return utmZone; diff --git a/src/tests/system-tests/libs/geofunctions.h b/src/tests/system-tests/libs/geofunctions.h index e31a8a2cb..677dc4a1d 100644 --- a/src/tests/system-tests/libs/geofunctions.h +++ b/src/tests/system-tests/libs/geofunctions.h @@ -94,7 +94,7 @@ arma::mat Gravity_ECEF(const arma::vec &r_eb_e); //!< Calculates acceleration d */ arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection); -arma::vec LLH_to_deg(arma::vec &LLH); +arma::vec LLH_to_deg(const arma::vec &LLH); double degtorad(double angleInDegrees); @@ -104,7 +104,7 @@ double mstoknotsh(double MetersPerSeconds); double mstokph(double Kph); -arma::vec CTM_to_Euler(arma::mat &C); +arma::vec CTM_to_Euler(const arma::mat &C); arma::mat Euler_to_CTM(const arma::vec &eul); @@ -151,35 +151,34 @@ void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat */ void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e); + /*! * \brief The Haversine formula determines the great-circle distance between two points on a sphere given their longitudes and latitudes. */ double great_circle_distance(double lat1, double lon1, double lat2, double lon2); + /*! * \brief Transformation of ECEF (X,Y,Z) to (E,N,U) in UTM, zone 'zone'. */ +void cart2utm(const arma::vec &r_eb_e, int zone, arma::vec &r_enu); -void cart2utm(arma::vec r_eb_e, int zone, arma::vec &r_enu); /*! * \brief Function finds the UTM zone number for given longitude and latitude. */ - int findUtmZone(double latitude_deg, double longitude_deg); + /*! * \brief Clenshaw summation of sinus of argument. */ - -double clsin(arma::colvec ar, int degree, double argument); +double clsin(const arma::colvec &ar, int degree, double argument); /*! * \brief Clenshaw summation of sinus with complex argument. */ - - -void clksin(arma::colvec ar, int degree, double arg_real, double arg_imag, double *re, double *im); +void clksin(const arma::colvec &ar, int degree, double arg_real, double arg_imag, double *re, double *im); #endif