From 626cddbcfaf16661679022ac6c2b76d31cfa6b3c Mon Sep 17 00:00:00 2001
From: PepMS <jmarti@iri.upc.edu>
Date: Fri, 3 Apr 2020 09:25:48 +0200
Subject: [PATCH] [files] wip
---
CMakeLists.txt | 10 +-
include/gnss_utils/TDCP.h | 2 +-
include/gnss_utils/observations.h | 2 +-
include/gnss_utils/ublox_raw.h | 2 +-
include/gnss_utils/{ => utils}/gnss_utils.h | 13 +-
include/gnss_utils/utils/transformations.h | 39 ++++
include/gnss_utils/{ => utils}/utils.h | 0
src/examples/gnss_utils_test.cpp | 2 +-
src/observations.cpp | 2 +-
src/{ => utils}/gnss_utils.cpp | 207 +++-----------------
src/utils/transformations.cpp | 147 ++++++++++++++
src/{ => utils}/utils.cpp | 2 +-
test/gtest_navigation.cpp | 2 +-
test/gtest_observations.cpp | 2 +-
test/gtest_transformations.cpp | 2 +-
15 files changed, 232 insertions(+), 202 deletions(-)
rename include/gnss_utils/{ => utils}/gnss_utils.h (80%)
create mode 100644 include/gnss_utils/utils/transformations.h
rename include/gnss_utils/{ => utils}/utils.h (100%)
rename src/{ => utils}/gnss_utils.cpp (63%)
create mode 100644 src/utils/transformations.cpp
rename src/{ => utils}/utils.cpp (97%)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 33c5324..1403ef8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,11 +49,12 @@ SET(RTKLIB_SRC_DIR ${RTKLIB_DIR}/src)
# driver source files
SET(SOURCES
- src/gnss_utils.cpp
+ src/utils/gnss_utils.cpp
+ src/utils/utils.cpp
+ src/utils/transformations.cpp
src/observations.cpp
src/navigation.cpp
src/TDCP.cpp
- src/utils.cpp
src/ublox_raw.cpp)
SET(RTKLIB_SRC
@@ -88,8 +89,9 @@ SET(RTKLIB_SRC
# application header files
SET(HEADERS
- include/gnss_utils/gnss_utils.h
- include/gnss_utils/utils.h
+ include/gnss_utils/utils/gnss_utils.h
+ include/gnss_utils/utils/utils.h
+ include/gnss_utils/utils/transformations.h
include/gnss_utils/observations.h
include/gnss_utils/navigation.h
include/gnss_utils/TDCP.h
diff --git a/include/gnss_utils/TDCP.h b/include/gnss_utils/TDCP.h
index 439a981..130d9eb 100644
--- a/include/gnss_utils/TDCP.h
+++ b/include/gnss_utils/TDCP.h
@@ -11,7 +11,7 @@
#define GNSS_UTILS_TDCP_DEBUG 0
#include <set>
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index 5610a7e..9b0cb1d 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -7,7 +7,7 @@
#include <memory>
#include <cassert>
-#include "gnss_utils/utils.h"
+#include "gnss_utils/utils/utils.h"
extern "C" {
#include "rtklib.h"
diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h
index c3e4cc1..7c8c6eb 100644
--- a/include/gnss_utils/ublox_raw.h
+++ b/include/gnss_utils/ublox_raw.h
@@ -1,7 +1,7 @@
#ifndef UBLOX_RAW_H
#define UBLOX_RAW_H
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
namespace GNSSUtils
{
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/utils/gnss_utils.h
similarity index 80%
rename from include/gnss_utils/gnss_utils.h
rename to include/gnss_utils/utils/gnss_utils.h
index a16976a..af36b62 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/utils/gnss_utils.h
@@ -12,6 +12,7 @@
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/transformations.h"
extern "C" {
#include "rtklib.h"
@@ -62,18 +63,6 @@ int estposOwn(const obsd_t* obs,
double* resp,
char* msg);
-Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef);
-Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon);
-Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef);
-Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu);
-
-void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
- Eigen::Matrix3d& R_ENU_ECEF,
- Eigen::Vector3d& t_ENU_ECEF);
-void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
- Eigen::Matrix3d& R_ENU_ECEF,
- Eigen::Vector3d& t_ENU_ECEF);
-
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
void computeSatellitesPositions(const Observations& obs,
diff --git a/include/gnss_utils/utils/transformations.h b/include/gnss_utils/utils/transformations.h
new file mode 100644
index 0000000..a13f6ba
--- /dev/null
+++ b/include/gnss_utils/utils/transformations.h
@@ -0,0 +1,39 @@
+/*
+ * TDCP.h
+ *
+ * Created on: April 3, 2020
+ * Author: Pep MartÃ-Saumell
+ */
+
+#ifndef INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
+#define INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
+
+// #include <vector>
+// #include <iostream>
+// #include <memory>
+// #include <string>
+
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+#include <eigen3/Eigen/Sparse>
+
+extern "C" {
+#include "rtklib.h"
+}
+
+namespace GNSSUtils
+{
+Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef);
+Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon);
+Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef);
+Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu);
+
+void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF);
+void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF);
+
+} // namespace GNSSUtils
+#endif // INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
\ No newline at end of file
diff --git a/include/gnss_utils/utils.h b/include/gnss_utils/utils/utils.h
similarity index 100%
rename from include/gnss_utils/utils.h
rename to include/gnss_utils/utils/utils.h
diff --git a/src/examples/gnss_utils_test.cpp b/src/examples/gnss_utils_test.cpp
index 814ee44..df97ca9 100644
--- a/src/examples/gnss_utils_test.cpp
+++ b/src/examples/gnss_utils_test.cpp
@@ -1,6 +1,6 @@
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
#include <typeinfo>
diff --git a/src/observations.cpp b/src/observations.cpp
index ea2dc15..3e8762d 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -1,4 +1,4 @@
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
#include "gnss_utils/observations.h"
using namespace GNSSUtils;
diff --git a/src/gnss_utils.cpp b/src/utils/gnss_utils.cpp
similarity index 63%
rename from src/gnss_utils.cpp
rename to src/utils/gnss_utils.cpp
index 70bad32..feca02d 100644
--- a/src/gnss_utils.cpp
+++ b/src/utils/gnss_utils.cpp
@@ -1,4 +1,4 @@
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
namespace GNSSUtils
{
@@ -19,38 +19,35 @@ ComputePosOutput computePos(const GNSSUtils::Observations& _observations,
sol_t sol;
sol = { { 0 } };
- output.pos_stat = pntpos(_observations.data(), _observations.size(),
- &(_navigation.getNavigation()),
- &_prcopt, &sol, NULL, NULL, msg);
-
- if (output.pos_stat == 0)
- {
- std::cout << "computePos: error in computing positioning, message: " << msg << "\n";
- }
-
- output.time = sol.time.time;
- output.sec = sol.time.sec;
- output.pos = Eigen::Vector3d(sol.rr);
- output.vel = Eigen::Vector3d(&sol.rr[3]);
- output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5],
- sol.qr[3], sol.qr[1], sol.qr[4],
- sol.qr[5], sol.qr[4], sol.qr[2];
- //std::cout << "Compute pos: " << output.pos.transpose() << "\n";
- //std::cout << "Covariance:\n" << output.pos_covar << "\n";
-
- // XXX: segmentation fault here.
- // if (sol.dtr != NULL)
- // {
- // output.rcv_bias << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5];
- // }
- output.type = sol.type;
- output.stat = sol.stat;
- output.ns = sol.ns;
- output.age = sol.age;
- output.ratio = sol.ratio;
- output.lat_lon = ecefToLatLonAlt(output.pos);
-
- return output;
+ output.pos_stat = pntpos(
+ _observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg);
+
+ if (output.pos_stat == 0)
+ {
+ std::cout << "computePos: error in computing positioning, message: " << msg << "\n";
+ }
+
+ output.time = sol.time.time;
+ output.sec = sol.time.sec;
+ output.pos = Eigen::Vector3d(sol.rr);
+ output.vel = Eigen::Vector3d(&sol.rr[3]);
+ output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5], sol.qr[3], sol.qr[1], sol.qr[4], sol.qr[5], sol.qr[4], sol.qr[2];
+ // std::cout << "Compute pos: " << output.pos.transpose() << "\n";
+ // std::cout << "Covariance:\n" << output.pos_covar << "\n";
+
+ // XXX: segmentation fault here.
+ // if (sol.dtr != NULL)
+ // {
+ // output.rcv_bias << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5];
+ // }
+ output.type = sol.type;
+ output.stat = sol.stat;
+ output.ns = sol.ns;
+ output.age = sol.age;
+ output.ratio = sol.ratio;
+ output.lat_lon = ecefToLatLonAlt(output.pos);
+
+ return output;
}
// ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations,
@@ -256,150 +253,6 @@ int estposOwn(const obsd_t* obs,
return 0;
}
-Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef)
-{
- Eigen::Vector3d pos;
- ecef2pos(_ecef.data(), pos.data());
-
- return pos;
-}
-
-Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _pos)
-{
- Eigen::Vector3d ecef;
- pos2ecef(_pos.data(), ecef.data());
-
- return ecef;
-}
-
-Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef)
-{
- Eigen::Matrix3d cov_enu;
-
- /* RTKLIB transform covariance to local tangental coordinate --------------------------
- * transform ecef covariance to local tangental coordinate
- * args : double *pos I geodetic position {lat,lon} (rad)
- * double *P I covariance in ecef coordinate
- * double *Q O covariance in local tangental coordinate
- * return : none
- *-----------------------------------------------------------------------------*/
- // extern void covenu(const double *pos, const double *P, double *Q);
- covenu(_latlon.data(), _cov_ecef.data(), cov_enu.data());
-
- return cov_enu;
-}
-
-Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu)
-{
- Eigen::Matrix3d cov_ecef;
-
- /* RTKLIB transform local enu coordinate covariance to xyz-ecef -----------------------
- * transform local enu covariance to xyz-ecef coordinate
- * args : double *pos I geodetic position {lat,lon} (rad)
- * double *Q I covariance in local enu coordinate
- * double *P O covariance in xyz-ecef coordinate
- * return : none
- *-----------------------------------------------------------------------------*/
- // extern void covecef(const double *pos, const double *Q, double *P)
- covecef(_latlon.data(), _cov_enu.data(), cov_ecef.data());
-
- return cov_ecef;
-}
-
-void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
- Eigen::Matrix3d& R_ENU_ECEF,
- Eigen::Vector3d& t_ENU_ECEF)
-{
- // Convert ECEF coordinates to geodetic coordinates.
- // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
- // to geodetic coordinates," IEEE Transactions on Aerospace and
- // Electronic Systems, vol. 30, pp. 957-961, 1994.
-
- // double r = std::sqrt(_t_ECEF_ENU(0) * _t_ECEF_ENU(0) + _t_ECEF_ENU(1) * _t_ECEF_ENU(1));
- // double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
- // double F = 54 * kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) * _t_ECEF_ENU(2);
- // double G = r * r + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) - kFirstEccentricitySquared
- // * Esq; double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3); double S =
- // cbrt(1 + C + sqrt(C * C + 2 * C)); double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G); double Q = sqrt(1 + 2 *
- // kFirstEccentricitySquared * kFirstEccentricitySquared * P); double r_0 = -(P * kFirstEccentricitySquared * r) /
- // (1 + Q)
- // + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
- // - P * (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P *
- // r * r);
- // double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2)
- // * _t_ECEF_ENU(2)); double Z_0 = kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) / (kSemimajorAxis * V);
- //
- // double latitude = atan((_t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
- // double longitude = atan2(_t_ECEF_ENU(1), _t_ECEF_ENU(0));
- //
- //
- // double sLat = sin(latitude);
- // double cLat = cos(latitude);
- // double sLon = sin(longitude);
- // double cLon = cos(longitude);
- //
- // R_ENU_ECEF(0,0) = -sLon;
- // R_ENU_ECEF(0,1) = cLon;
- // R_ENU_ECEF(0,2) = 0.0;
- //
- // R_ENU_ECEF(1,0) = -sLat*cLon;
- // R_ENU_ECEF(1,1) = -sLat * sLon;
- // R_ENU_ECEF(1,2) = cLat;
- //
- // R_ENU_ECEF(2,0) = cLat * cLon;
- // R_ENU_ECEF(2,1) = cLat * sLon;
- // R_ENU_ECEF(2,2) = sLat;
- //
- // t_ENU_ECEF = -R_ENU_ECEF*_t_ECEF_ENU;
-
- Eigen::Vector3d ENU_lat_lon_alt = GNSSUtils::ecefToLatLonAlt(_t_ECEF_ENU);
-
- double sLat = sin(ENU_lat_lon_alt(0));
- double cLat = cos(ENU_lat_lon_alt(0));
- double sLon = sin(ENU_lat_lon_alt(1));
- double cLon = cos(ENU_lat_lon_alt(1));
-
- R_ENU_ECEF(0, 0) = -sLon;
- R_ENU_ECEF(0, 1) = cLon;
- R_ENU_ECEF(0, 2) = 0.0;
-
- R_ENU_ECEF(1, 0) = -sLat * cLon;
- R_ENU_ECEF(1, 1) = -sLat * sLon;
- R_ENU_ECEF(1, 2) = cLat;
-
- R_ENU_ECEF(2, 0) = cLat * cLon;
- R_ENU_ECEF(2, 1) = cLat * sLon;
- R_ENU_ECEF(2, 2) = sLat;
-
- t_ENU_ECEF = -R_ENU_ECEF * _t_ECEF_ENU;
-}
-
-void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
- Eigen::Matrix3d& R_ENU_ECEF,
- Eigen::Vector3d& t_ENU_ECEF)
-{
- double sLat = sin(_ENU_latlonalt(0));
- double cLat = cos(_ENU_latlonalt(0));
- double sLon = sin(_ENU_latlonalt(1));
- double cLon = cos(_ENU_latlonalt(1));
-
- R_ENU_ECEF(0, 0) = -sLon;
- R_ENU_ECEF(0, 1) = cLon;
- R_ENU_ECEF(0, 2) = 0.0;
-
- R_ENU_ECEF(1, 0) = -sLat * cLon;
- R_ENU_ECEF(1, 1) = -sLat * sLon;
- R_ENU_ECEF(1, 2) = cLat;
-
- R_ENU_ECEF(2, 0) = cLat * cLon;
- R_ENU_ECEF(2, 1) = cLat * sLon;
- R_ENU_ECEF(2, 2) = sLat;
-
- Eigen::Vector3d t_ECEF_ENU = latLonAltToEcef(_ENU_latlonalt);
-
- t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU;
-}
-
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
{
// ecef 2 geodetic
diff --git a/src/utils/transformations.cpp b/src/utils/transformations.cpp
new file mode 100644
index 0000000..fa8c0e2
--- /dev/null
+++ b/src/utils/transformations.cpp
@@ -0,0 +1,147 @@
+#include "gnss_utils/utils/transformations.h"
+
+using namespace GNSSUtils;
+
+Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef)
+{
+ Eigen::Vector3d pos;
+ ecef2pos(_ecef.data(), pos.data());
+
+ return pos;
+}
+
+Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _pos)
+{
+ Eigen::Vector3d ecef;
+ pos2ecef(_pos.data(), ecef.data());
+
+ return ecef;
+}
+
+Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef)
+{
+ Eigen::Matrix3d cov_enu;
+
+ /* RTKLIB transform covariance to local tangental coordinate --------------------------
+ * transform ecef covariance to local tangental coordinate
+ * args : double *pos I geodetic position {lat,lon} (rad)
+ * double *P I covariance in ecef coordinate
+ * double *Q O covariance in local tangental coordinate
+ * return : none
+ *-----------------------------------------------------------------------------*/
+ // extern void covenu(const double *pos, const double *P, double *Q);
+ covenu(_latlon.data(), _cov_ecef.data(), cov_enu.data());
+
+ return cov_enu;
+}
+
+Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu)
+{
+ Eigen::Matrix3d cov_ecef;
+
+ /* RTKLIB transform local enu coordinate covariance to xyz-ecef -----------------------
+ * transform local enu covariance to xyz-ecef coordinate
+ * args : double *pos I geodetic position {lat,lon} (rad)
+ * double *Q I covariance in local enu coordinate
+ * double *P O covariance in xyz-ecef coordinate
+ * return : none
+ *-----------------------------------------------------------------------------*/
+ // extern void covecef(const double *pos, const double *Q, double *P)
+ covecef(_latlon.data(), _cov_enu.data(), cov_ecef.data());
+
+ return cov_ecef;
+}
+
+void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF)
+{
+ // Convert ECEF coordinates to geodetic coordinates.
+ // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
+ // to geodetic coordinates," IEEE Transactions on Aerospace and
+ // Electronic Systems, vol. 30, pp. 957-961, 1994.
+
+ // double r = std::sqrt(_t_ECEF_ENU(0) * _t_ECEF_ENU(0) + _t_ECEF_ENU(1) * _t_ECEF_ENU(1));
+ // double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
+ // double F = 54 * kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) * _t_ECEF_ENU(2);
+ // double G = r * r + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) - kFirstEccentricitySquared
+ // * Esq; double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3); double S =
+ // cbrt(1 + C + sqrt(C * C + 2 * C)); double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G); double Q = sqrt(1 + 2 *
+ // kFirstEccentricitySquared * kFirstEccentricitySquared * P); double r_0 = -(P * kFirstEccentricitySquared * r) /
+ // (1 + Q)
+ // + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
+ // - P * (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P *
+ // r * r);
+ // double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2)
+ // * _t_ECEF_ENU(2)); double Z_0 = kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) / (kSemimajorAxis * V);
+ //
+ // double latitude = atan((_t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
+ // double longitude = atan2(_t_ECEF_ENU(1), _t_ECEF_ENU(0));
+ //
+ //
+ // double sLat = sin(latitude);
+ // double cLat = cos(latitude);
+ // double sLon = sin(longitude);
+ // double cLon = cos(longitude);
+ //
+ // R_ENU_ECEF(0,0) = -sLon;
+ // R_ENU_ECEF(0,1) = cLon;
+ // R_ENU_ECEF(0,2) = 0.0;
+ //
+ // R_ENU_ECEF(1,0) = -sLat*cLon;
+ // R_ENU_ECEF(1,1) = -sLat * sLon;
+ // R_ENU_ECEF(1,2) = cLat;
+ //
+ // R_ENU_ECEF(2,0) = cLat * cLon;
+ // R_ENU_ECEF(2,1) = cLat * sLon;
+ // R_ENU_ECEF(2,2) = sLat;
+ //
+ // t_ENU_ECEF = -R_ENU_ECEF*_t_ECEF_ENU;
+
+ Eigen::Vector3d ENU_lat_lon_alt = GNSSUtils::ecefToLatLonAlt(_t_ECEF_ENU);
+
+ double sLat = sin(ENU_lat_lon_alt(0));
+ double cLat = cos(ENU_lat_lon_alt(0));
+ double sLon = sin(ENU_lat_lon_alt(1));
+ double cLon = cos(ENU_lat_lon_alt(1));
+
+ R_ENU_ECEF(0, 0) = -sLon;
+ R_ENU_ECEF(0, 1) = cLon;
+ R_ENU_ECEF(0, 2) = 0.0;
+
+ R_ENU_ECEF(1, 0) = -sLat * cLon;
+ R_ENU_ECEF(1, 1) = -sLat * sLon;
+ R_ENU_ECEF(1, 2) = cLat;
+
+ R_ENU_ECEF(2, 0) = cLat * cLon;
+ R_ENU_ECEF(2, 1) = cLat * sLon;
+ R_ENU_ECEF(2, 2) = sLat;
+
+ t_ENU_ECEF = -R_ENU_ECEF * _t_ECEF_ENU;
+}
+
+void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
+ Eigen::Matrix3d& R_ENU_ECEF,
+ Eigen::Vector3d& t_ENU_ECEF)
+{
+ double sLat = sin(_ENU_latlonalt(0));
+ double cLat = cos(_ENU_latlonalt(0));
+ double sLon = sin(_ENU_latlonalt(1));
+ double cLon = cos(_ENU_latlonalt(1));
+
+ R_ENU_ECEF(0, 0) = -sLon;
+ R_ENU_ECEF(0, 1) = cLon;
+ R_ENU_ECEF(0, 2) = 0.0;
+
+ R_ENU_ECEF(1, 0) = -sLat * cLon;
+ R_ENU_ECEF(1, 1) = -sLat * sLon;
+ R_ENU_ECEF(1, 2) = cLat;
+
+ R_ENU_ECEF(2, 0) = cLat * cLon;
+ R_ENU_ECEF(2, 1) = cLat * sLon;
+ R_ENU_ECEF(2, 2) = sLat;
+
+ Eigen::Vector3d t_ECEF_ENU = latLonAltToEcef(_ENU_latlonalt);
+
+ t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU;
+}
\ No newline at end of file
diff --git a/src/utils.cpp b/src/utils/utils.cpp
similarity index 97%
rename from src/utils.cpp
rename to src/utils/utils.cpp
index cd697b3..3dcd1e2 100644
--- a/src/utils.cpp
+++ b/src/utils/utils.cpp
@@ -1,4 +1,4 @@
-#include "gnss_utils/utils.h"
+#include "gnss_utils/utils/utils.h"
namespace GNSSUtils
{
diff --git a/test/gtest_navigation.cpp b/test/gtest_navigation.cpp
index 5170cf1..18e8c63 100644
--- a/test/gtest_navigation.cpp
+++ b/test/gtest_navigation.cpp
@@ -1,5 +1,5 @@
#include "gtest/utils_gtest.h"
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
TEST(NavigationTests, Whatever)
{
diff --git a/test/gtest_observations.cpp b/test/gtest_observations.cpp
index fc24115..57244d4 100644
--- a/test/gtest_observations.cpp
+++ b/test/gtest_observations.cpp
@@ -1,5 +1,5 @@
#include "gtest/utils_gtest.h"
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
#include "gnss_utils/observations.h"
using namespace GNSSUtils;
diff --git a/test/gtest_transformations.cpp b/test/gtest_transformations.cpp
index cae4e24..8b819bb 100644
--- a/test/gtest_transformations.cpp
+++ b/test/gtest_transformations.cpp
@@ -1,5 +1,5 @@
#include "gtest/utils_gtest.h"
-#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/gnss_utils.h"
// Geodetic system parameters
static double kSemimajorAxis = 6378137;
--
GitLab