mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-15 20:50:33 +00:00
Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next
This commit is contained in:
Carles Fernandez
commit
c605a52072
@ -24,10 +24,10 @@
|
||||
# also defined, but not for general use are
|
||||
# GPSTK_LIBRARY, where to find the GPSTK library.
|
||||
|
||||
FIND_PATH(GPSTK_INCLUDE_DIR Rinex3ObsBase.hpp
|
||||
HINTS /usr/include/gpstk
|
||||
/usr/local/include/gpstk
|
||||
/opt/local/include/gpstk )
|
||||
FIND_PATH(GPSTK_INCLUDE_DIR gpstk/Rinex3ObsBase.hpp
|
||||
HINTS /usr/include
|
||||
/usr/local/include
|
||||
/opt/local/include )
|
||||
|
||||
SET(GPSTK_NAMES ${GPSTK_NAMES} gpstk libgpstk)
|
||||
FIND_LIBRARY(GPSTK_LIBRARY NAMES ${GPSTK_NAMES}
|
||||
|
||||
@ -28,11 +28,10 @@
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
#include "geofunctions.h"
|
||||
#include <iomanip>
|
||||
|
||||
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,7 +205,7 @@ 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
|
||||
dP = P - (N_phi + (*h)) * cosphi;
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -239,6 +239,7 @@ public:
|
||||
void check_results_duplicated_satellite(
|
||||
arma::mat& measured_sat1,
|
||||
arma::mat& measured_sat2,
|
||||
int ch_id,
|
||||
std::string data_title);
|
||||
|
||||
HybridObservablesTest()
|
||||
@ -1006,13 +1007,40 @@ void HybridObservablesTest::check_results_carrier_doppler(
|
||||
void HybridObservablesTest::check_results_duplicated_satellite(
|
||||
arma::mat& measured_sat1,
|
||||
arma::mat& measured_sat2,
|
||||
int ch_id,
|
||||
std::string data_title)
|
||||
{
|
||||
//1. True value interpolation to match the measurement times
|
||||
|
||||
double t0 = measured_sat1(0, 0);
|
||||
//define the common measured time interval
|
||||
double t0_sat1 = measured_sat1(0, 0);
|
||||
int size1 = measured_sat1.col(0).n_rows;
|
||||
double t1 = measured_sat1(size1 - 1, 0);
|
||||
double t1_sat1 = measured_sat1(size1 - 1, 0);
|
||||
|
||||
double t0_sat2 = measured_sat2(0, 0);
|
||||
int size2 = measured_sat2.col(0).n_rows;
|
||||
double t1_sat2 = measured_sat2(size2 - 1, 0);
|
||||
|
||||
double t0;
|
||||
double t1;
|
||||
if (t0_sat1 > t0_sat2)
|
||||
{
|
||||
t0 = t0_sat1;
|
||||
}
|
||||
else
|
||||
{
|
||||
t0 = t0_sat2;
|
||||
}
|
||||
|
||||
if (t1_sat1 > t1_sat2)
|
||||
{
|
||||
t1 = t1_sat2;
|
||||
}
|
||||
else
|
||||
{
|
||||
t1 = t1_sat1;
|
||||
}
|
||||
|
||||
arma::vec t = arma::linspace<arma::vec>(t0, t1, floor((t1 - t0) * 1e3));
|
||||
//conversion between arma::vec and std:vector
|
||||
arma::vec t_from_start = arma::linspace<arma::vec>(0, t1 - t0, floor((t1 - t0) * 1e3));
|
||||
@ -1048,6 +1076,15 @@ void HybridObservablesTest::check_results_duplicated_satellite(
|
||||
//compute error
|
||||
err_ch0_hz = meas_sat1_doppler_interp - meas_sat2_doppler_interp;
|
||||
|
||||
//save matlab file for further analysis
|
||||
std::vector<double> tmp_vector_common_time_s(t.colptr(0),
|
||||
t.colptr(0) + t.n_rows);
|
||||
|
||||
std::vector<double> tmp_vector_err_ch0_hz(err_ch0_hz.colptr(0),
|
||||
err_ch0_hz.colptr(0) + err_ch0_hz.n_rows);
|
||||
save_mat_xy(tmp_vector_common_time_s, tmp_vector_err_ch0_hz, std::string("measured_doppler_error_ch_" + std::to_string(ch_id)));
|
||||
|
||||
//compute statistics
|
||||
arma::vec err2_ch0 = arma::square(err_ch0_hz);
|
||||
double rmse_ch0 = sqrt(arma::mean(err2_ch0));
|
||||
|
||||
@ -1089,19 +1126,26 @@ void HybridObservablesTest::check_results_duplicated_satellite(
|
||||
}
|
||||
|
||||
//check results against the test tolerance
|
||||
ASSERT_LT(error_mean_ch0, 5);
|
||||
ASSERT_GT(error_mean_ch0, -5);
|
||||
EXPECT_LT(error_mean_ch0, 5);
|
||||
EXPECT_GT(error_mean_ch0, -5);
|
||||
//assuming PLL BW=35
|
||||
ASSERT_LT(error_var_ch0, 250);
|
||||
ASSERT_LT(max_error_ch0, 100);
|
||||
ASSERT_GT(min_error_ch0, -100);
|
||||
ASSERT_LT(rmse_ch0, 30);
|
||||
EXPECT_LT(error_var_ch0, 250);
|
||||
EXPECT_LT(max_error_ch0, 100);
|
||||
EXPECT_GT(min_error_ch0, -100);
|
||||
EXPECT_LT(rmse_ch0, 30);
|
||||
|
||||
//Carrier Phase error
|
||||
//2. RMSE
|
||||
arma::vec err_carrier_phase;
|
||||
|
||||
err_carrier_phase = delta_measured_carrier_phase_cycles;
|
||||
|
||||
//save matlab file for further analysis
|
||||
std::vector<double> tmp_vector_err_carrier_phase(err_carrier_phase.colptr(0),
|
||||
err_carrier_phase.colptr(0) + err_carrier_phase.n_rows);
|
||||
save_mat_xy(tmp_vector_common_time_s, tmp_vector_err_carrier_phase, std::string("measured_carrier_phase_error_ch_" + std::to_string(ch_id)));
|
||||
|
||||
|
||||
arma::vec err2_carrier_phase = arma::square(err_carrier_phase);
|
||||
double rmse_carrier_phase = sqrt(arma::mean(err2_carrier_phase));
|
||||
|
||||
@ -1143,18 +1187,24 @@ void HybridObservablesTest::check_results_duplicated_satellite(
|
||||
}
|
||||
|
||||
//check results against the test tolerance
|
||||
ASSERT_LT(rmse_carrier_phase, 0.25);
|
||||
ASSERT_LT(error_mean_carrier_phase, 0.2);
|
||||
ASSERT_GT(error_mean_carrier_phase, -0.2);
|
||||
ASSERT_LT(error_var_carrier_phase, 0.5);
|
||||
ASSERT_LT(max_error_carrier_phase, 0.5);
|
||||
ASSERT_GT(min_error_carrier_phase, -0.5);
|
||||
EXPECT_LT(rmse_carrier_phase, 0.25);
|
||||
EXPECT_LT(error_mean_carrier_phase, 0.2);
|
||||
EXPECT_GT(error_mean_carrier_phase, -0.2);
|
||||
EXPECT_LT(error_var_carrier_phase, 0.5);
|
||||
EXPECT_LT(max_error_carrier_phase, 0.5);
|
||||
EXPECT_GT(min_error_carrier_phase, -0.5);
|
||||
|
||||
//Pseudorange error
|
||||
//2. RMSE
|
||||
arma::vec err_pseudorange;
|
||||
|
||||
err_pseudorange = delta_measured_dist_m;
|
||||
|
||||
//save matlab file for further analysis
|
||||
std::vector<double> tmp_vector_err_pseudorange(err_pseudorange.colptr(0),
|
||||
err_pseudorange.colptr(0) + err_pseudorange.n_rows);
|
||||
save_mat_xy(tmp_vector_common_time_s, tmp_vector_err_pseudorange, std::string("measured_pr_error_ch_" + std::to_string(ch_id)));
|
||||
|
||||
arma::vec err2_pseudorange = arma::square(err_pseudorange);
|
||||
double rmse_pseudorange = sqrt(arma::mean(err2_pseudorange));
|
||||
|
||||
@ -1196,12 +1246,12 @@ void HybridObservablesTest::check_results_duplicated_satellite(
|
||||
}
|
||||
|
||||
//check results against the test tolerance
|
||||
ASSERT_LT(rmse_pseudorange, 3.0);
|
||||
ASSERT_LT(error_mean_pseudorange, 1.0);
|
||||
ASSERT_GT(error_mean_pseudorange, -1.0);
|
||||
ASSERT_LT(error_var_pseudorange, 10.0);
|
||||
ASSERT_LT(max_error_pseudorange, 10.0);
|
||||
ASSERT_GT(min_error_pseudorange, -10.0);
|
||||
EXPECT_LT(rmse_pseudorange, 3.0);
|
||||
EXPECT_LT(error_mean_pseudorange, 1.0);
|
||||
EXPECT_GT(error_mean_pseudorange, -1.0);
|
||||
EXPECT_LT(error_var_pseudorange, 10.0);
|
||||
EXPECT_LT(max_error_pseudorange, 10.0);
|
||||
EXPECT_GT(min_error_pseudorange, -10.0);
|
||||
}
|
||||
|
||||
bool HybridObservablesTest::save_mat_xy(std::vector<double>& x, std::vector<double>& y, std::string filename)
|
||||
@ -1827,6 +1877,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
|
||||
check_results_duplicated_satellite(
|
||||
measured_obs_vec.at(sat1_ch_id),
|
||||
measured_obs_vec.at(sat2_ch_id),
|
||||
sat1_ch_id,
|
||||
"Duplicated sat [CH " + std::to_string(sat1_ch_id) + "," + std::to_string(sat2_ch_id) + "] PRNs " + std::to_string(gnss_synchro_vec.at(sat1_ch_id).PRN) + "," + std::to_string(gnss_synchro_vec.at(sat2_ch_id).PRN) + " ");
|
||||
}
|
||||
else
|
||||
|
||||
@ -17,3 +17,7 @@
|
||||
#
|
||||
|
||||
add_subdirectory(front-end-cal)
|
||||
|
||||
if(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA)
|
||||
add_subdirectory(assist)
|
||||
endif(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA)
|
||||
|
||||
56
src/utils/assist/CMakeLists.txt
Normal file
56
src/utils/assist/CMakeLists.txt
Normal file
@ -0,0 +1,56 @@
|
||||
# Copyright (C) 2012-2018 (see AUTHORS file for a list of contributors)
|
||||
#
|
||||
# This file is part of GNSS-SDR.
|
||||
#
|
||||
# GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation, either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# GNSS-SDR is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
||||
#
|
||||
|
||||
find_package(GPSTK QUIET)
|
||||
if(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK)
|
||||
set(GPSTK_LIBRARY ${CMAKE_CURRENT_SOURCE_DIR}/../../../thirdparty/gpstk-${GNSSSDR_GPSTK_LOCAL_VERSION}/install/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gpstk${CMAKE_SHARED_LIBRARY_SUFFIX} )
|
||||
set(GPSTK_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../thirdparty/gpstk-${GNSSSDR_GPSTK_LOCAL_VERSION}/install/include )
|
||||
endif(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK)
|
||||
|
||||
set(CMAKE_INCLUDE_PATH ${CMAKE_INCLUDE_PATH} ${GPSTK_INCLUDE_DIR}/gpstk)
|
||||
|
||||
include_directories(
|
||||
${CMAKE_SOURCE_DIR}/src/core/system_parameters
|
||||
${GFlags_INCLUDE_DIRS}
|
||||
${Boost_INCLUDE_DIRS}
|
||||
${GPSTK_INCLUDE_DIR}/gpstk
|
||||
${GPSTK_INCLUDE_DIR}
|
||||
)
|
||||
|
||||
add_executable(rinex2assist ${CMAKE_CURRENT_SOURCE_DIR}/main.cc)
|
||||
|
||||
target_link_libraries(rinex2assist
|
||||
${Boost_LIBRARIES}
|
||||
${GPSTK_LIBRARY}
|
||||
${GFlags_LIBS}
|
||||
gnss_sp_libs
|
||||
gnss_rx)
|
||||
|
||||
if(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK)
|
||||
add_dependencies(rinex2assist gpstk-${GNSSSDR_GPSTK_LOCAL_VERSION})
|
||||
endif(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK)
|
||||
|
||||
|
||||
add_custom_command(TARGET rinex2assist POST_BUILD
|
||||
COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_FILE:rinex2assist>
|
||||
${CMAKE_SOURCE_DIR}/install/$<TARGET_FILE_NAME:rinex2assist>)
|
||||
|
||||
install(TARGETS rinex2assist
|
||||
RUNTIME DESTINATION bin
|
||||
COMPONENT "rinex2assist"
|
||||
)
|
||||
226
src/utils/assist/main.cc
Normal file
226
src/utils/assist/main.cc
Normal file
@ -0,0 +1,226 @@
|
||||
/*!
|
||||
* \file main.cc
|
||||
* \brief converts navigation RINEX files into XML files for Assisted GNSS.
|
||||
* \author Carles Fernandez-Prades, 2018. cfernandez(at)cttc.cat
|
||||
*
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
//#include "gnss_sdr_flags.h"
|
||||
#include "gps_ephemeris.h"
|
||||
#include "galileo_ephemeris.h"
|
||||
#include <gflags/gflags.h>
|
||||
#include <gpstk/Rinex3NavHeader.hpp>
|
||||
#include <gpstk/Rinex3NavData.hpp>
|
||||
#include <gpstk/Rinex3NavStream.hpp>
|
||||
#include <boost/archive/xml_oarchive.hpp>
|
||||
#include <boost/serialization/map.hpp>
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
const std::string intro_help(
|
||||
std::string("\n rinex2assist converts navigation RINEX files into XML files for Assisted GNSS\n") +
|
||||
"Copyright (C) 2018 (see AUTHORS file for a list of contributors)\n" +
|
||||
"This program comes with ABSOLUTELY NO WARRANTY;\n" +
|
||||
"See COPYING file to see a copy of the General Public License.\n \n" +
|
||||
"Usage: \n" +
|
||||
" rinex2assist <RINEX Nav file input> [<XML file output>]");
|
||||
|
||||
google::SetUsageMessage(intro_help);
|
||||
google::SetVersionString("1.0");
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
if ((argc < 2) or (argc > 3))
|
||||
{
|
||||
std::cerr << "Usage:" << std::endl;
|
||||
std::cerr << " " << argv[0]
|
||||
<< " <RINEX Nav file input> [<XML file output>]"
|
||||
<< std::endl;
|
||||
google::ShutDownCommandLineFlags();
|
||||
return 1;
|
||||
}
|
||||
std::string xml_filename;
|
||||
if (argc == 3)
|
||||
{
|
||||
xml_filename = argv[2];
|
||||
}
|
||||
//std::string filename_rinex_nav = FLAGS_filename_rinex_nav;
|
||||
std::map<int, Gps_Ephemeris> eph_map;
|
||||
std::map<int, Galileo_Ephemeris> eph_gal_map;
|
||||
|
||||
int i = 0;
|
||||
int j = 0;
|
||||
try
|
||||
{
|
||||
// Read nav file
|
||||
gpstk::Rinex3NavStream rnffs(argv[1]); // Open navigation data file
|
||||
gpstk::Rinex3NavData rne;
|
||||
gpstk::Rinex3NavHeader hdr;
|
||||
|
||||
// read header
|
||||
rnffs >> hdr;
|
||||
|
||||
// Check that it really is a RINEX navigation file
|
||||
if (hdr.fileType.substr(0, 1).compare("N") != 0)
|
||||
{
|
||||
std::cout << "This is not a valid RINEX navigation file" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
while (rnffs >> rne)
|
||||
{
|
||||
if (rne.satSys.compare("G") == 0 or rne.satSys.empty())
|
||||
{
|
||||
// Fill ephemeris object
|
||||
Gps_Ephemeris eph;
|
||||
eph.i_satellite_PRN = rne.PRNID;
|
||||
eph.d_TOW = rne.xmitTime;
|
||||
eph.d_IODE_SF2 = rne.IODE;
|
||||
eph.d_IODE_SF3 = rne.IODE;
|
||||
eph.d_Crs = rne.Crs;
|
||||
eph.d_Delta_n = rne.dn;
|
||||
eph.d_M_0 = rne.M0;
|
||||
eph.d_Cuc = rne.Cuc;
|
||||
eph.d_e_eccentricity = rne.ecc;
|
||||
eph.d_Cus = rne.Cus;
|
||||
eph.d_sqrt_A = rne.Ahalf;
|
||||
eph.d_Toe = rne.Toe;
|
||||
eph.d_Toc = rne.Toc;
|
||||
eph.d_Cic = rne.Cic;
|
||||
eph.d_OMEGA0 = rne.OMEGA0;
|
||||
eph.d_Cis = rne.Cis;
|
||||
eph.d_i_0 = rne.i0;
|
||||
eph.d_Crc = rne.Crc;
|
||||
eph.d_OMEGA = rne.w;
|
||||
eph.d_OMEGA_DOT = rne.OMEGAdot;
|
||||
eph.d_IDOT = rne.idot;
|
||||
eph.i_code_on_L2 = rne.codeflgs; //!< If 1, P code ON in L2; if 2, C/A code ON in L2;
|
||||
eph.i_GPS_week = rne.weeknum;
|
||||
eph.b_L2_P_data_flag = rne.L2Pdata;
|
||||
eph.i_SV_accuracy = rne.accuracy;
|
||||
eph.i_SV_health = rne.health;
|
||||
eph.d_TGD = rne.Tgd;
|
||||
eph.d_IODC = rne.IODC;
|
||||
eph.i_AODO = 0; //
|
||||
eph.b_fit_interval_flag = (rne.fitint > 4) ? 1 : 0;
|
||||
eph.d_spare1 = 0.0;
|
||||
eph.d_spare2 = 0.0;
|
||||
eph.d_A_f0 = rne.af0;
|
||||
eph.d_A_f1 = rne.af1;
|
||||
eph.d_A_f2 = rne.af2;
|
||||
eph.b_integrity_status_flag = 0; //
|
||||
eph.b_alert_flag = 0; //
|
||||
eph.b_antispoofing_flag = 0; //
|
||||
eph_map[i] = eph;
|
||||
i++;
|
||||
}
|
||||
if (rne.satSys.compare("E") == 0)
|
||||
{
|
||||
// Read Galileo ephemeris
|
||||
Galileo_Ephemeris eph;
|
||||
eph.i_satellite_PRN = rne.PRNID;
|
||||
eph.M0_1 = rne.M0;
|
||||
eph.e_1 = rne.ecc;
|
||||
eph.A_1 = rne.Ahalf;
|
||||
eph.OMEGA_0_2 = rne.OMEGA0;
|
||||
eph.i_0_2 = rne.i0;
|
||||
eph.omega_2 = rne.w;
|
||||
eph.OMEGA_dot_3 = rne.OMEGAdot;
|
||||
eph.iDot_2 = rne.idot;
|
||||
eph.C_uc_3 = rne.Cuc;
|
||||
eph.C_us_3 = rne.Cus;
|
||||
eph.C_rc_3 = rne.Crc;
|
||||
eph.C_rs_3 = rne.Crs;
|
||||
eph.C_ic_4 = rne.Cic;
|
||||
eph.C_is_4 = rne.Cis;
|
||||
eph.t0e_1 = rne.Toe;
|
||||
eph.t0c_4 = rne.Toc;
|
||||
eph.af0_4 = rne.af0;
|
||||
eph.af1_4 = rne.af1;
|
||||
eph.af2_4 = rne.af2;
|
||||
eph_gal_map[j] = eph;
|
||||
j++;
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (std::exception& e)
|
||||
{
|
||||
std::cout << "Error reading the RINEX file: " << e.what() << std::endl;
|
||||
google::ShutDownCommandLineFlags();
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (i == 0 and j == 0)
|
||||
{
|
||||
std::cout << "No data found in the RINEX file. No XML file will be created." << std::endl;
|
||||
google::ShutDownCommandLineFlags();
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Write XML
|
||||
if (i != 0)
|
||||
{
|
||||
std::ofstream ofs;
|
||||
if (xml_filename.empty())
|
||||
{
|
||||
xml_filename = "eph_GPS_L1CA.xml";
|
||||
}
|
||||
try
|
||||
{
|
||||
ofs.open(xml_filename.c_str(), std::ofstream::trunc | std::ofstream::out);
|
||||
boost::archive::xml_oarchive xml(ofs);
|
||||
xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", eph_map);
|
||||
}
|
||||
catch (std::exception& e)
|
||||
{
|
||||
std::cout << "Problem creating the XML file: " << e.what() << std::endl;
|
||||
google::ShutDownCommandLineFlags();
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
if (j != 0)
|
||||
{
|
||||
std::ofstream ofs2;
|
||||
if (xml_filename.empty())
|
||||
{
|
||||
xml_filename = "eph_Galileo_E1.xml";
|
||||
}
|
||||
try
|
||||
{
|
||||
ofs2.open(xml_filename.c_str(), std::ofstream::trunc | std::ofstream::out);
|
||||
boost::archive::xml_oarchive xml(ofs2);
|
||||
xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", eph_gal_map);
|
||||
}
|
||||
catch (std::exception& e)
|
||||
{
|
||||
std::cout << "Problem creating the XML file: " << e.what() << std::endl;
|
||||
google::ShutDownCommandLineFlags();
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
google::ShutDownCommandLineFlags();
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user