mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-16 05:00:35 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

Browse Source
This commit is contained in:
Carles Fernandez 2018-10-15 10:06:08 +02:00
commit 44d829f84d
6 changed files with 595 additions and 353 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
View File

@ -484,7 +484,7 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
if (T_rx_clock_step_samples == 0) if (T_rx_clock_step_samples == 0)
{ {
T_rx_clock_step_samples = std::round(static_cast<double>(in[d_nchannels_in - 1][0].fs) * 1e-3); // 1 ms T_rx_clock_step_samples = std::round(static_cast<double>(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); usleep(1000000);
} }

5
src/tests/CMakeLists.txt
View File

@ -228,7 +228,10 @@ if(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA)
find_package(GPSTK) find_package(GPSTK)
if(NOT GPSTK_FOUND OR ENABLE_OWN_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'.") 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(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated")
endif("${TOOLCHAIN_ARG}" STREQUAL "")
# if(NOT ENABLE_FPGA) # if(NOT ENABLE_FPGA)
if(CMAKE_VERSION VERSION_LESS 3.2) if(CMAKE_VERSION VERSION_LESS 3.2)
ExternalProject_Add( ExternalProject_Add(

344
src/tests/system-tests/libs/geofunctions.cc
View File

@ -28,10 +28,10 @@
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
*/ */
#include "geofunctions.h" #include "geofunctions.h"
const double STRP_G_SI = 9.80665; const double STRP_PI = 3.1415926535898; // Pi as defined in IS-GPS-200E
const double STRP_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E
arma::mat Skew_symmetric(const arma::vec &a) arma::mat Skew_symmetric(const arma::vec &a)
{ {
@ -205,7 +205,7 @@ int togeod(double *dphi, double *dlambda, double *h, double a, double finv, doub
cosphi = cos(*dphi); cosphi = cos(*dphi);
// compute radius of curvature in prime vertical direction // 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; dP = P - (N_phi + (*h)) * cosphi;
@ -233,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) arma::mat Gravity_ECEF(const arma::vec &r_eb_e)
{ {
// Parameters // 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 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 J_2 = 1.082627E-3; // WGS84 Earth's second gravitational constant
const double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s) const double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s)
@ -259,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; const double rtd = 180.0 / STRP_PI;
LLH(0) = LLH(0) * rtd; arma::vec deg = arma::zeros(3, 1);
LLH(1) = LLH(1) * rtd; deg(0) = LLH(0) * rtd;
return LLH; deg(1) = LLH(1) * rtd;
deg(2) = LLH(2);
return deg;
} }
@ -296,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) // Calculate Euler angles using (2.23)
arma::mat CTM = C;
arma::vec eul = arma::zeros(3, 1); arma::vec eul = arma::zeros(3, 1);
eul(0) = atan2(C(1, 2), C(2, 2)); // roll eul(0) = atan2(CTM(1, 2), CTM(2, 2)); // roll
if (C(0, 2) < -1.0) C(0, 2) = -1.0; if (CTM(0, 2) < -1.0) CTM(0, 2) = -1.0;
if (C(0, 2) > 1.0) C(0, 2) = 1.0; if (CTM(0, 2) > 1.0) CTM(0, 2) = 1.0;
eul(1) = -asin(C(0, 2)); // pitch eul(1) = -asin(CTM(0, 2)); // pitch
eul(2) = atan2(C(0, 1), C(0, 0)); // yaw eul(2) = atan2(CTM(0, 1), CTM(0, 0)); // yaw
return eul; return eul;
} }
@ -353,19 +356,19 @@ arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection)
do do
{ {
oldh = h; 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))))); 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; h = sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) / cos(phi) - N;
iterations = iterations + 1; iterations = iterations + 1;
if (iterations > 100) 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; break;
} }
} }
while (std::abs(h - oldh) > 1.0e-12); while (std::fabs(h - oldh) > 1.0e-12);
arma::vec LLH = {{phi, lambda, h}}; //radians arma::vec LLH = {{phi, lambda, h}}; // radians
return LLH; return LLH;
} }
@ -398,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) 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 // 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 double e = 0.0818191908425; // WGS84 eccentricity
// Calculate transverse radius of curvature using (2.105) // 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) // Convert position using (2.112)
double cos_lat = cos(LLH(0)); double cos_lat = cos(LLH(0));
@ -434,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) 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 // 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 const double e = 0.0818191908425; // WGS84 eccentricity
// Calculate transverse radius of curvature using (2.105) // Calculate transverse radius of curvature using (2.105)
@ -459,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; v_eb_e = C_e_n.t() * v_eb_n;
} }
double great_circle_distance(double lat1, double lon1, double lat2, double lon2) 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 // generally used geo measurement function
double R = 6378.137; // Radius of earth in KM double R = 6378.137; // Radius of earth in KM
double dLat = lat2 * STRP_PI / 180.0 - lat1 * STRP_PI / 180.0; double dLat = lat2 * STRP_PI / 180.0 - lat1 * STRP_PI / 180.0;
@ -473,3 +477,299 @@ double great_circle_distance(double lat1, double lon1, double lat2, double lon2)
double d = R * c; double d = R * c;
return d * 1000.0; // meters return d * 1000.0; // meters
} }
void cart2utm(const arma::vec &r_eb_e, int zone, arma::vec &r_enu)
{
// Transformation of (X,Y,Z) to (E,N,U) in UTM, zone 'zone'
//
// 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
//
// Outputs:
// r_enu - UTM coordinates (Easting, Northing, Uping)
//
// Originally written in Matlab by Kai Borre, Nov. 1994
// Implemented in C++ by J.Arribas
//
// 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.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.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.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.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;
//
// 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.0) neg_geo = true;
double Bg_r = fabs(B);
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
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(const arma::colvec &ar, int degree, double argument)
{
// Clenshaw summation of sinus of argument.
//
// 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.0;
double hr = 0.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(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);
//
// 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.0 * cos_arg_r * cosh_arg_i;
double i = -2.0 * sin_arg_r * sinh_arg_i;
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--)
{
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).
//
// Originally written in Matlab by Darius Plausinaitis
// Implemented in C++ by J.Arribas
// 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.0) || (latitude_deg < -80.0))
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.0)
{
// Corrections for zones 31 33 35 37
if ((longitude_deg >= 0.0) && (longitude_deg < 9.0))
{
utmZone = 31;
}
else if ((longitude_deg >= 9.0) && (longitude_deg < 21.0))
{
utmZone = 33;
}
else if ((longitude_deg >= 21.0) && (longitude_deg < 33.0))
{
utmZone = 35;
}
else if ((longitude_deg >= 33.0) && (longitude_deg < 42.0))
{
utmZone = 37;
}
}
else if ((latitude_deg >= 56.0) && (latitude_deg < 64.0))
{
// Correction for zone 32
if ((longitude_deg >= 3.0) && (longitude_deg < 12.0))
utmZone = 32;
}
return utmZone;
}

28
src/tests/system-tests/libs/geofunctions.h
View File

@ -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 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); double degtorad(double angleInDegrees);
@ -104,7 +104,7 @@ double mstoknotsh(double MetersPerSeconds);
double mstokph(double Kph); 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); arma::mat Euler_to_CTM(const arma::vec &eul);
@ -151,10 +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); 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. * \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); 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);
/*!
* \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(const arma::colvec &ar, int degree, double argument);
/*!
* \brief Clenshaw summation of sinus with complex argument.
*/
void clksin(const arma::colvec &ar, int degree, double arg_real, double arg_imag, double *re, double *im);
#endif #endif

371
src/tests/system-tests/position_test.cc
View File

@ -32,6 +32,7 @@
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
*/ */
#include "geofunctions.h"
#include "position_test_flags.h" #include "position_test_flags.h"
#include "rtklib_solver_dump_reader.h" #include "rtklib_solver_dump_reader.h"
#include "spirent_motion_csv_dump_reader.h" #include "spirent_motion_csv_dump_reader.h"
@ -54,6 +55,7 @@
#include <fstream> #include <fstream>
#include <numeric> #include <numeric>
#include <thread> #include <thread>
#include <armadillo>
// For GPS NAVIGATION (L1) // For GPS NAVIGATION (L1)
concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue; concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
@ -82,118 +84,13 @@ private:
std::string filename_rinex_obs = FLAGS_filename_rinex_obs; std::string filename_rinex_obs = FLAGS_filename_rinex_obs;
std::string filename_raw_data = FLAGS_filename_raw_data; std::string filename_raw_data = FLAGS_filename_raw_data;
void print_results(const std::vector<double>& east, void print_results(arma::mat R_eb_enu);
const std::vector<double>& north,
const std::vector<double>& up);
double compute_stdev_precision(const std::vector<double>& vec);
double compute_stdev_accuracy(const std::vector<double>& 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);
std::shared_ptr<InMemoryConfiguration> config; std::shared_ptr<InMemoryConfiguration> config;
std::shared_ptr<FileConfiguration> config_f; std::shared_ptr<FileConfiguration> config_f;
std::string generated_kml_file; 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<double>& vec)
{
double sum__ = std::accumulate(vec.begin(), vec.end(), 0.0);
double mean__ = sum__ / vec.size();
double accum__ = 0.0;
std::for_each(std::begin(vec), std::end(vec), [&](const double d) {
accum__ += (d - mean__) * (d - mean__);
});
double stdev__ = std::sqrt(accum__ / (vec.size() - 1));
return stdev__;
}
double PositionSystemTest::compute_stdev_accuracy(const std::vector<double>& 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() int PositionSystemTest::configure_generator()
{ {
// Configure signal generator // Configure signal generator
@ -261,23 +158,23 @@ int PositionSystemTest::configure_receiver()
const int grid_density = 16; const int grid_density = 16;
const float zero = 0.0; const float zero = 0.0;
const int number_of_channels = 12; const int number_of_channels = 11;
const int in_acquisition = 1; const int in_acquisition = 1;
const float threshold = 0.01; const float threshold = 2.5;
const float doppler_max = 8000.0; const float doppler_max = 5000.0;
const float doppler_step = 500.0; const float doppler_step = 250.0;
const int max_dwells = 1; const int max_dwells = 10;
const int tong_init_val = 2; const int tong_init_val = 2;
const int tong_max_val = 10; const int tong_max_val = 10;
const int tong_max_dwells = 30; const int tong_max_dwells = 30;
const int coherent_integration_time_ms = 1; const int coherent_integration_time_ms = 1;
const float pll_bw_hz = 30.0; const float pll_bw_hz = 35.0;
const float dll_bw_hz = 4.0; const float dll_bw_hz = 1.5;
const float early_late_space_chips = 0.5; const float early_late_space_chips = 0.5;
const float pll_bw_narrow_hz = 20.0; const float pll_bw_narrow_hz = 1.0;
const float dll_bw_narrow_hz = 2.0; const float dll_bw_narrow_hz = 0.1;
const int extend_correlation_ms = 1; const int extend_correlation_ms = 1;
const int display_rate_ms = 500; const int display_rate_ms = 500;
@ -307,7 +204,7 @@ int PositionSystemTest::configure_receiver()
// Set the Signal Conditioner // Set the Signal Conditioner
config->set_property("SignalConditioner.implementation", "Signal_Conditioner"); config->set_property("SignalConditioner.implementation", "Signal_Conditioner");
config->set_property("DataTypeAdapter.implementation", "Ibyte_To_Complex"); 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.dump", "false");
config->set_property("InputFilter.input_item_type", "gr_complex"); config->set_property("InputFilter.input_item_type", "gr_complex");
config->set_property("InputFilter.output_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.ampl2_end", std::to_string(ampl2_end));
config->set_property("InputFilter.band1_error", std::to_string(band1_error)); 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.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.grid_density", std::to_string(grid_density));
config->set_property("InputFilter.sampling_frequency", std::to_string(sampling_rate_internal)); config->set_property("InputFilter.sampling_frequency", std::to_string(sampling_rate_internal));
config->set_property("InputFilter.IF", std::to_string(zero)); config->set_property("InputFilter.IF", std::to_string(zero));
@ -358,7 +255,6 @@ int PositionSystemTest::configure_receiver()
// Set Tracking // 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_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.item_type", "gr_complex");
config->set_property("Tracking_1C.dump", "false"); config->set_property("Tracking_1C.dump", "false");
config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); config->set_property("Tracking_1C.dump_filename", "./tracking_ch_");
@ -369,6 +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.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.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.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 // Set Telemetry
config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder"); config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder");
@ -381,7 +279,7 @@ int PositionSystemTest::configure_receiver()
// Set PVT // Set PVT
config->set_property("PVT.implementation", "RTKLIB_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.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.display_rate_ms", std::to_string(display_rate_ms));
config->set_property("PVT.dump_filename", "./PVT"); config->set_property("PVT.dump_filename", "./PVT");
@ -460,11 +358,8 @@ int PositionSystemTest::run_receiver()
void PositionSystemTest::check_results() void PositionSystemTest::check_results()
{ {
std::vector<double> pos_e;
std::vector<double> pos_n;
std::vector<double> pos_u;
arma::mat R_eb_e; //ECEF position (x,y,z) estimation in the Earth frame (Nx3) 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 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 LLH; //Geodetic coordinates (latitude, longitude, height) estimation in WGS84 datum
arma::vec receiver_time_s; arma::vec receiver_time_s;
@ -482,10 +377,15 @@ void PositionSystemTest::check_results()
double ref_long = std::stod(str_aux); double ref_long = std::stod(str_aux);
std::getline(iss2, str_aux, '\n'); std::getline(iss2, str_aux, '\n');
double ref_h = std::stod(str_aux); double ref_h = std::stod(str_aux);
double ref_e, ref_n, ref_u; int utm_zone = findUtmZone(ref_lat, ref_long);
geodetic2Enu(ref_lat, ref_long, ref_h,
&ref_e, &ref_n, &ref_u);
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) if (!FLAGS_use_pvt_solver_dump)
{ {
//fall back to read receiver KML output (position only) //fall back to read receiver KML output (position only)
@ -503,8 +403,8 @@ void PositionSystemTest::check_results()
if (found != std::string::npos) is_header = false; if (found != std::string::npos) is_header = false;
} }
bool is_data = true; bool is_data = true;
//read data //read data
int64_t current_epoch = 0;
while (is_data) while (is_data)
{ {
if (!std::getline(myfile, line)) if (!std::getline(myfile, line))
@ -532,18 +432,20 @@ void PositionSystemTest::check_results()
if (i == 2) h = value; if (i == 2) h = value;
} }
double north, east, up; arma::vec tmp_v_ecef;
geodetic2Enu(lat, longitude, h, &east, &north, &up); 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 << "lat = " << lat << ", longitude = " << longitude << " h = " << h << std::endl;
// std::cout << "E = " << east << ", N = " << north << " U = " << up << 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(); // getchar();
} }
} }
myfile.close(); 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 else
{ {
@ -551,33 +453,27 @@ void PositionSystemTest::check_results()
rtklib_solver_dump_reader pvt_reader; rtklib_solver_dump_reader pvt_reader;
pvt_reader.open_obs_file(FLAGS_pvt_solver_dump_filename); pvt_reader.open_obs_file(FLAGS_pvt_solver_dump_filename);
int64_t n_epochs = pvt_reader.num_epochs(); int64_t n_epochs = pvt_reader.num_epochs();
R_eb_e = arma::zeros(n_epochs, 3); R_eb_e = arma::zeros(3, n_epochs);
V_eb_e = arma::zeros(n_epochs, 3); V_eb_e = arma::zeros(3, n_epochs);
LLH = arma::zeros(n_epochs, 3); LLH = arma::zeros(3, n_epochs);
receiver_time_s = arma::zeros(n_epochs, 1); receiver_time_s = arma::zeros(n_epochs, 1);
int64_t current_epoch = 0; int64_t current_epoch = 0;
while (pvt_reader.read_binary_obs()) 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<double>(pvt_reader.TOW_at_current_symbol_ms) / 1000.0;
receiver_time_s(current_epoch) = pvt_reader.RX_time - pvt_reader.clk_offset_s; 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(0, current_epoch) = pvt_reader.rr[0];
R_eb_e(current_epoch, 1) = pvt_reader.rr[1]; R_eb_e(1, current_epoch) = pvt_reader.rr[1];
R_eb_e(current_epoch, 2) = pvt_reader.rr[2]; R_eb_e(2, current_epoch) = pvt_reader.rr[2];
V_eb_e(current_epoch, 0) = pvt_reader.rr[3]; V_eb_e(0, current_epoch) = pvt_reader.rr[3];
V_eb_e(current_epoch, 1) = pvt_reader.rr[4]; V_eb_e(1, current_epoch) = pvt_reader.rr[4];
V_eb_e(current_epoch, 2) = pvt_reader.rr[5]; V_eb_e(2, current_epoch) = pvt_reader.rr[5];
LLH(current_epoch, 0) = pvt_reader.latitude; LLH(0, current_epoch) = pvt_reader.latitude;
LLH(current_epoch, 1) = pvt_reader.longitude; LLH(1, current_epoch) = pvt_reader.longitude;
LLH(current_epoch, 2) = pvt_reader.height; 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);
//debug check //debug check
// std::cout << "t1: " << pvt_reader.RX_time << std::endl; // std::cout << "t1: " << pvt_reader.RX_time << std::endl;
@ -593,20 +489,21 @@ void PositionSystemTest::check_results()
if (FLAGS_static_scenario) if (FLAGS_static_scenario)
{ {
double sigma_E_2_precision = std::pow(compute_stdev_precision(pos_e), 2.0); double sigma_E_2_precision = arma::var(R_eb_enu.row(0));
double sigma_N_2_precision = std::pow(compute_stdev_precision(pos_n), 2.0); double sigma_N_2_precision = arma::var(R_eb_enu.row(1));
double sigma_U_2_precision = std::pow(compute_stdev_precision(pos_u), 2.0); 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); arma::rowvec tmp_vec;
double sigma_N_2_accuracy = std::pow(compute_stdev_accuracy(pos_n, ref_n), 2.0); tmp_vec = R_eb_enu.row(0) - ref_r_enu(0);
double sigma_U_2_accuracy = std::pow(compute_stdev_accuracy(pos_u, ref_u), 2.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 = arma::mean(R_eb_enu.row(0));
double mean__e = sum__e / pos_e.size(); double mean__n = arma::mean(R_eb_enu.row(1));
double sum__n = std::accumulate(pos_n.begin(), pos_n.end(), 0.0); double mean__u = arma::mean(R_eb_enu.row(2));
double mean__n = sum__n / pos_n.size();
double sum__u = std::accumulate(pos_u.begin(), pos_u.end(), 0.0);
double mean__u = sum__u / pos_u.size();
std::stringstream stm; std::stringstream stm;
std::ofstream position_test_file; std::ofstream position_test_file;
@ -614,25 +511,23 @@ void PositionSystemTest::check_results()
{ {
stm << "Configuration file: " << FLAGS_config_file_ptest << std::endl; stm << "Configuration file: " << FLAGS_config_file_ptest << std::endl;
} }
if (FLAGS_static_scenario)
{
stm << "---- ACCURACY ----" << std::endl; stm << "---- ACCURACY ----" << std::endl;
stm << "2DRMS = " << 2 * sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << 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 << "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 << "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 << "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 << "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 << "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 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(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 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 << std::endl;
}
stm << "---- PRECISION ----" << std::endl; stm << "---- PRECISION ----" << std::endl;
stm << "2DRMS = " << 2 * sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << 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 << "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 << "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 << "90% SAS = " << 0.833 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
stm << "MRSE = " << sqrt(sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; stm << "MRSE = " << sqrt(sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
@ -649,11 +544,11 @@ void PositionSystemTest::check_results()
// Sanity Check // Sanity Check
double precision_SEP = 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision); 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) if (FLAGS_plot_position_test == true)
{ {
print_results(pos_e, pos_n, pos_u); print_results(R_eb_enu);
} }
} }
else else
@ -662,56 +557,55 @@ void PositionSystemTest::check_results()
spirent_motion_csv_dump_reader ref_reader; spirent_motion_csv_dump_reader ref_reader;
ref_reader.open_obs_file(FLAGS_ref_motion_filename); ref_reader.open_obs_file(FLAGS_ref_motion_filename);
int64_t n_epochs = ref_reader.num_epochs(); int64_t n_epochs = ref_reader.num_epochs();
ref_R_eb_e = arma::zeros(n_epochs, 3); ref_R_eb_e = arma::zeros(3, n_epochs);
ref_V_eb_e = arma::zeros(n_epochs, 3); ref_V_eb_e = arma::zeros(3, n_epochs);
ref_LLH = arma::zeros(n_epochs, 3); ref_LLH = arma::zeros(3, n_epochs);
ref_time_s = arma::zeros(n_epochs, 1); ref_time_s = arma::zeros(n_epochs, 1);
int64_t current_epoch = 0; int64_t current_epoch = 0;
while (ref_reader.read_csv_obs()) while (ref_reader.read_csv_obs())
{ {
ref_time_s(current_epoch) = ref_reader.TOW_ms / 1000.0; 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(0, current_epoch) = ref_reader.Pos_X;
ref_R_eb_e(current_epoch, 1) = ref_reader.Pos_Y; ref_R_eb_e(1, current_epoch) = ref_reader.Pos_Y;
ref_R_eb_e(current_epoch, 2) = ref_reader.Pos_Z; ref_R_eb_e(2, current_epoch) = ref_reader.Pos_Z;
ref_V_eb_e(current_epoch, 0) = ref_reader.Vel_X; ref_V_eb_e(0, current_epoch) = ref_reader.Vel_X;
ref_V_eb_e(current_epoch, 1) = ref_reader.Vel_Y; ref_V_eb_e(1, current_epoch) = ref_reader.Vel_Y;
ref_V_eb_e(current_epoch, 2) = ref_reader.Vel_Z; ref_V_eb_e(2, current_epoch) = ref_reader.Vel_Z;
ref_LLH(current_epoch, 0) = ref_reader.Lat; ref_LLH(0, current_epoch) = ref_reader.Lat;
ref_LLH(current_epoch, 1) = ref_reader.Long; ref_LLH(1, current_epoch) = ref_reader.Long;
ref_LLH(current_epoch, 2) = ref_reader.Height; ref_LLH(2, current_epoch) = ref_reader.Height;
current_epoch++; current_epoch++;
} }
//interpolation of reference data to receiver epochs timestamps //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_R_eb_e = arma::zeros(3, R_eb_e.n_cols);
arma::mat ref_interp_V_eb_e = arma::zeros(V_eb_e.n_rows, 3); arma::mat ref_interp_V_eb_e = arma::zeros(3, V_eb_e.n_cols);
arma::mat ref_interp_LLH = arma::zeros(LLH.n_rows, 3); arma::mat ref_interp_LLH = arma::zeros(3, LLH.n_cols);
arma::vec tmp_vector; arma::vec tmp_vector;
for (int n = 0; n < 3; n++) for (int n = 0; n < 3; n++)
{ {
arma::interp1(ref_time_s, ref_R_eb_e.col(n), receiver_time_s, tmp_vector); arma::interp1(ref_time_s, ref_R_eb_e.row(n), receiver_time_s, tmp_vector);
ref_interp_R_eb_e.col(n) = tmp_vector; ref_interp_R_eb_e.row(n) = tmp_vector.t();
arma::interp1(ref_time_s, ref_V_eb_e.col(n), receiver_time_s, tmp_vector); arma::interp1(ref_time_s, ref_V_eb_e.row(n), receiver_time_s, tmp_vector);
ref_interp_V_eb_e.col(n) = tmp_vector; ref_interp_V_eb_e.row(n) = tmp_vector.t();
arma::interp1(ref_time_s, ref_LLH.col(n), receiver_time_s, tmp_vector); arma::interp1(ref_time_s, ref_LLH.row(n), receiver_time_s, tmp_vector);
ref_interp_LLH.col(n) = tmp_vector; ref_interp_LLH.row(n) = tmp_vector.t();
} }
//compute error vectors //compute error vectors
arma::mat error_R_eb_e = arma::zeros(3, R_eb_e.n_cols);
arma::mat error_R_eb_e = arma::zeros(R_eb_e.n_rows, 3); arma::mat error_V_eb_e = arma::zeros(3, V_eb_e.n_cols);
arma::mat error_V_eb_e = arma::zeros(V_eb_e.n_rows, 3); arma::mat error_LLH = arma::zeros(3, LLH.n_cols);
arma::mat error_LLH = arma::zeros(LLH.n_rows, 3);
error_R_eb_e = R_eb_e - ref_interp_R_eb_e; 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_V_eb_e = V_eb_e - ref_interp_V_eb_e;
error_LLH = LLH - ref_interp_LLH; 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_R_eb_e = arma::zeros(R_eb_e.n_cols, 1);
arma::vec error_module_V_eb_e = arma::zeros(V_eb_e.n_rows, 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_rows; n++) 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_R_eb_e(n) = arma::norm(error_R_eb_e.col(n));
error_module_V_eb_e(n) = arma::norm(error_V_eb_e.row(n)); error_module_V_eb_e(n) = arma::norm(error_V_eb_e.col(n));
} }
//Error statistics //Error statistics
arma::vec tmp_vec; arma::vec tmp_vec;
//RMSE, Mean, Variance and peaks //RMSE, Mean, Variance and peaks
@ -750,7 +644,12 @@ void PositionSystemTest::check_results()
<< " [m/s]" << std::endl; << " [m/s]" << std::endl;
std::cout.precision(ss); std::cout.precision(ss);
//plots // plots
if (FLAGS_plot_position_test == true)
{
const std::string gnuplot_executable(FLAGS_gnuplot_executable);
if (!gnuplot_executable.empty())
{
Gnuplot g1("points"); Gnuplot g1("points");
if (FLAGS_show_plots) if (FLAGS_show_plots)
{ {
@ -763,9 +662,13 @@ void PositionSystemTest::check_results()
g1.set_title("3D ECEF error coordinates"); g1.set_title("3D ECEF error coordinates");
g1.set_grid(); g1.set_grid();
//conversion between arma::vec and std:vector //conversion between arma::vec and std:vector
std::vector<double> X(error_R_eb_e.colptr(0), error_R_eb_e.colptr(0) + error_R_eb_e.n_rows); arma::rowvec arma_vec_error_x = error_R_eb_e.row(0);
std::vector<double> Y(error_R_eb_e.colptr(1), error_R_eb_e.colptr(1) + error_R_eb_e.n_rows); arma::rowvec arma_vec_error_y = error_R_eb_e.row(1);
std::vector<double> Z(error_R_eb_e.colptr(2), error_R_eb_e.colptr(2) + error_R_eb_e.n_rows); arma::rowvec arma_vec_error_z = error_R_eb_e.row(2);
std::vector<double> X(arma_vec_error_x.colptr(0), arma_vec_error_x.colptr(0) + arma_vec_error_x.n_rows);
std::vector<double> Y(arma_vec_error_y.colptr(0), arma_vec_error_y.colptr(0) + arma_vec_error_y.n_rows);
std::vector<double> 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.cmd("set key box opaque");
g1.plot_xyz(X, Y, Z, "ECEF 3D error"); g1.plot_xyz(X, Y, Z, "ECEF 3D error");
@ -841,12 +744,12 @@ void PositionSystemTest::check_results()
g4.savetops("Velocity_3d_error_" + config_filename_no_extension); g4.savetops("Velocity_3d_error_" + config_filename_no_extension);
} }
} }
}
}
} }
void PositionSystemTest::print_results(const std::vector<double>& east, void PositionSystemTest::print_results(arma::mat R_eb_enu)
const std::vector<double>& north,
const std::vector<double>& up)
{ {
const std::string gnuplot_executable(FLAGS_gnuplot_executable); const std::string gnuplot_executable(FLAGS_gnuplot_executable);
if (gnuplot_executable.empty()) if (gnuplot_executable.empty())
@ -857,29 +760,40 @@ void PositionSystemTest::print_results(const std::vector<double>& east,
} }
else else
{ {
double sigma_E_2_precision = std::pow(compute_stdev_precision(east), 2.0); double sigma_E_2_precision = arma::var(R_eb_enu.row(0));
double sigma_N_2_precision = std::pow(compute_stdev_precision(north), 2.0); double sigma_N_2_precision = arma::var(R_eb_enu.row(1));
double sigma_U_2_precision = std::pow(compute_stdev_precision(up), 2.0); 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_east = arma::mean(R_eb_enu.row(0));
double mean_north = std::accumulate(north.begin(), north.end(), 0.0) / north.size(); 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)); double it_max_east = arma::max(R_eb_enu.row(0) - mean_east);
auto it_min_east = std::min_element(std::begin(east), std::end(east)); double it_min_east = arma::min(R_eb_enu.row(0) - mean_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));
auto east_range = std::max(*it_max_east, std::abs(*it_min_east)); double it_max_north = arma::max(R_eb_enu.row(1) - mean_north);
auto north_range = std::max(*it_max_north, std::abs(*it_min_north)); double it_min_north = arma::min(R_eb_enu.row(1) - mean_north);
auto up_range = std::max(*it_max_up, std::abs(*it_min_up));
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 = std::max(east_range, north_range) * 1.1;
double range_3d = std::max(std::max(east_range, north_range), up_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 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); 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<double> east(arma_east.colptr(0), arma_east.row(0).colptr(0) + arma_east.row(0).n_cols);
std::vector<double> north(arma_north.colptr(0), arma_north.colptr(0) + arma_north.n_cols);
std::vector<double> up(arma_up.colptr(0), arma_up.colptr(0) + arma_up.n_cols);
try try
{ {
boost::filesystem::path p(gnuplot_executable); boost::filesystem::path p(gnuplot_executable);
@ -903,6 +817,7 @@ void PositionSystemTest::print_results(const std::vector<double>& east,
g1.cmd("set xrange [-" + std::to_string(range) + ":" + std::to_string(range) + "]"); 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.cmd("set yrange [-" + std::to_string(range) + ":" + std::to_string(range) + "]");
g1.plot_xy(east, north, "2D Position Fixes"); 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, "2DRMS");
g1.set_style("lines").plot_circle(mean_east, mean_north, two_drms / 2.0, "DRMS"); g1.set_style("lines").plot_circle(mean_east, mean_north, two_drms / 2.0, "DRMS");

2
src/tests/unit-tests/signal-processing-blocks/pvt/rtklib_solver_test.cc
View File

@ -452,7 +452,7 @@ TEST(RTKLibSolverTest, test1)
std::cout << "3D positioning error: " << error_3d_m << " [meters]" << std::endl; std::cout << "3D positioning error: " << error_3d_m << " [meters]" << std::endl;
//check results against the test tolerance //check results against the test tolerance
ASSERT_LT(error_3d_m, 1.0); ASSERT_LT(error_3d_m, 0.2);
pvt_valid = true; pvt_valid = true;
} }
} }