From 9030b5891bc7826e92d76f838d5cc0ed09f5b339 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 19 Feb 2021 19:05:25 +0100
Subject: [PATCH] WIP
---
include/gnss_utils/utils/satellite.h | 20 +-----
src/observations.cpp | 11 +---
src/utils/satellite.cpp | 54 ++++++++++++----
test/gtest_tdcp.cpp | 93 ++++++++++++++++++++++++++++
test/gtest_transformations.cpp | 76 +++++++++++++++--------
5 files changed, 190 insertions(+), 64 deletions(-)
create mode 100644 test/gtest_tdcp.cpp
diff --git a/include/gnss_utils/utils/satellite.h b/include/gnss_utils/utils/satellite.h
index 2f8dac5..93d6681 100644
--- a/include/gnss_utils/utils/satellite.h
+++ b/include/gnss_utils/utils/satellite.h
@@ -19,29 +19,15 @@ namespace GnssUtils
class Observations;
class Navigation;
- double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
-
Satellites computeSatellites(const Observations& obs,
const Navigation& nav,
const int& eph_opt); // see rtklib.h L396
- Eigen::Vector2d computeAzel(const Satellite& sat, const Eigen::Vector3d& receiver_ecef)
- {
- Eigen::Vector3d unit_vector;
- geodist(sat.pose.data(), receiver_ecef.data(), unit_vector.data());
+ Eigen::Vector2d computeAzel(const Eigen::Vector3d& sat_ecef, const Eigen::Vector3d& receiver_ecef);
- Eigen::Vector2d azel;
- Eigen::Vector3d pos_enu; // TODO
- satazel(pos_enu.data(), unit_vector.data(), azel.data());
- }
+ Eigen::Vector2d computeAzel(const Satellite& sat, const Eigen::Vector3d& receiver_ecef);
- Azels computeAzels(const Satellites& sats, const Eigen::Vector3d& receiver_ecef)
- {
- Azels azels;
- for (auto sat : sats)
- azels.insert(sat.first, computeAzel(sat, receiver_ecef));
- return azels;
- }
+ Azels computeAzels(const Satellites& sats, const Eigen::Vector3d& receiver_ecef);
struct Satellite
{
diff --git a/src/observations.cpp b/src/observations.cpp
index bb4bb5e..f4f59d7 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -361,16 +361,7 @@ std::set<int> Observations::filterByElevationSnr(const Eigen::Vector3d& x_r,
const bool &multi_freq)
{
std::set<int> remove_sats;
- std::map<int,Eigen::Vector2d> azels;
-
- for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
- {
- auto&& obs_sat = getObservationByIdx(obs_i);
- const int& sat_number = obs_sat.sat;
-
- double elevation = computeSatElevation(x_r, sats.at(obs_sat.sat).pos);
- azels.emplace(obs_sat.sat,Eigen::Vector2d(0.0,elevation));
- }
+ Azels azels = computeAzels(sats, x_r);
return filterByElevationSnr(azels, snrmask, elmin, multi_freq);
}
diff --git a/src/utils/satellite.cpp b/src/utils/satellite.cpp
index f0ebbfc..9682bfd 100644
--- a/src/utils/satellite.cpp
+++ b/src/utils/satellite.cpp
@@ -14,24 +14,56 @@
namespace GnssUtils
{
-double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
+
+Eigen::Vector2d computeAzel(const Eigen::Vector3d& sat_ecef, const Eigen::Vector3d& receiver_ecef)
{
- // ecef 2 geodetic
+ // receiver position (geo)
Eigen::Vector3d receiver_geo;
ecef2pos(receiver_ecef.data(), receiver_geo.data());
- // receiver-sat vector ecef
- Eigen::Vector3d receiver_sat_ecef = sat_ecef - receiver_ecef;
+ // receiver-sat unit vector (ecef)
+ Eigen::Vector3d e_ecef = (sat_ecef - receiver_ecef).normalized();
+
+ // receiver-sat unit vector (enu)
+ Eigen::Vector3d e_enu;
+ ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad)
+ e_ecef.data(), // vector in ecef coordinate {x,y,z}
+ e_enu.data()); // vector in local tangental coordinate {e,n,u}
- // receiver-sat vector enu (receiver ecef as origin)
- Eigen::Vector3d receiver_sat_enu;
- ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad)
- receiver_sat_ecef.data(), // vector in ecef coordinate {x,y,z}
- receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
+ Eigen::Vector2d azel;
+ // azimuth
+ azel(0) = atan2(e_enu(0), e_enu(1));
// elevation
- return atan2(receiver_sat_enu(2),
- sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
+ azel(1) = asin(e_enu(2));
+
+ return azel;
+}
+
+Eigen::Vector2d computeAzel(const Satellite& sat, const Eigen::Vector3d& receiver_ecef)
+{
+ return computeAzel(sat.pos, receiver_ecef);
+ /*// compute receiver-to-satellilte unit vector (ecef)
+ Eigen::Vector3d e;
+ geodist(sat.pos.data(), receiver_ecef.data(), e.data());
+
+ // compute receiver position in geodetic
+ Eigen::Vector3d receiver_geo;
+ ecef2pos(receiver_ecef.data(), receiver_geo.data());
+
+ // compute azimut and elevation
+ Eigen::Vector2d azel;
+ satazel(receiver_geo.data(), e.data(), azel.data());
+
+ return azel;*/
+}
+
+Azels computeAzels(const Satellites& sats, const Eigen::Vector3d& receiver_ecef)
+{
+ Azels azels;
+ for (auto sat : sats)
+ azels.emplace(sat.first, computeAzel(sat.second, receiver_ecef));
+ return azels;
}
Satellites computeSatellites(const Observations& obs,
diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp
new file mode 100644
index 0000000..447c273
--- /dev/null
+++ b/test/gtest_tdcp.cpp
@@ -0,0 +1,93 @@
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/utils/transformations.h"
+
+// Geodetic system parameters
+static double kSemimajorAxis = 6378137;
+static double kSemiminorAxis = 6356752.3142;
+static double kFirstEccentricitySquared = 6.69437999014 * 0.001;
+static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
+
+using namespace GnssUtils;
+
+Eigen::Vector3d computeRandomReceiverLatLonAlt()
+{
+ Eigen::Vector3d receiver_LLA = Eigen::Vector3d::Random(); // in [-1, 1]
+ receiver_LLA(0) *= M_PI / 2; // in [-PI/2, PI/2]
+ receiver_LLA(1) *= M_PI; // in [-PI, PI]
+ receiver_LLA(2) += 1; // ([0, 2])
+ receiver_LLA(2) *= 5e2; // in [0, 1e3]
+
+ return receiver_LLA;
+}
+
+void computeRandomVisibleSatellite(const Eigen::Vector3d& receiver_latlonalt,
+ Eigen::Vector3d& sat_ENU,
+ Eigen::Vector3d& sat_ECEF,
+ Eigen::Vector2d& sat_azel,
+ double range)
+{
+ Eigen::Vector3d t_ECEF_ENU, t_ENU_ECEF;
+ Eigen::Matrix3d R_ENU_ECEF;
+
+ t_ECEF_ENU = latLonAltToEcef(receiver_latlonalt);
+
+ computeEnuEcefFromLatLonAlt(receiver_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
+
+ // random elevation and azimuth
+ sat_azel = Eigen::Vector2d::Random(); // in [-1, 1]
+ sat_azel(0) *= M_PI; // in [-pi, pi]
+ sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2; // in [0, pi/2]
+ range = Eigen::VectorXd::Random(1)(0) * 5e2 + 1e3; // in [500, 1500]
+
+ // ENU
+ sat_ENU << sin(sat_azel(0)) * cos(sat_azel(1)) * range,
+ cos(sat_azel(0)) * cos(sat_azel(1)) * range,
+ sin(sat_azel(1)) * range;
+
+ // ECEF
+ sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * sat_ENU;
+}
+
+
+TEST(Tdcp, Tdcp)
+{
+ Eigen::Vector3d t_ENU_ECEF,t_ECEF_ENU,t_ENU_latlonalt, sat_ENU, sat_ECEF;
+ Eigen::Vector3d r1_LLA, r1_ECEF, r2_ECEF, d_ECEF;
+ Eigen::Matrix3d R_ENU_ECEF;
+ Eigen::Vector2d azel, azel2;
+ double range;
+
+ Satellite sat1({0,0,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),1.0,0,0,1});
+ Satellite sat2(sat1);
+
+ // Random receiver position
+ for (auto i = 0; i<100; i++)
+ {
+ r1_LLA = computeRandomReceiverLatLonAlt();
+ r1_ECEF = latLonAltToEcef(r1_LLA);
+ d_ECEF = Eigen::Vector3d::Random() * 10;
+ r2_ECEF = r1_ECEF + d_ECEF;
+
+ Satellites sats1, sats2;
+
+ // random visible satellites
+ for (auto j = 0; j<10; j++)
+ {
+ sat1.pos;
+
+ computeRandomVisibleSatellite(r1_LLA, sat_ENU, sat_ECEF, azel, range);
+ computeRandomVisibleSatellite(r2_LLA, sat_ENU, sat_ECEF, azel, range);
+
+ azel2 = computeAzel(sat_ECEF, t_ECEF_ENU);
+
+ ASSERT_MATRIX_APPROX(azel,azel2,1e-6);
+ }
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_transformations.cpp b/test/gtest_transformations.cpp
index cf5a3b0..2becc26 100644
--- a/test/gtest_transformations.cpp
+++ b/test/gtest_transformations.cpp
@@ -10,6 +10,46 @@ static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
using namespace GnssUtils;
+Eigen::Vector3d computeRandomReceiverLatLonAlt()
+{
+ Eigen::Vector3d receiver_LLA = Eigen::Vector3d::Random(); // in [-1, 1]
+ receiver_LLA(0) *= M_PI / 2; // in [-PI/2, PI/2]
+ receiver_LLA(1) *= M_PI; // in [-PI, PI]
+ receiver_LLA(2) += 1; // ([0, 2])
+ receiver_LLA(2) *= 5e2; // in [0, 1e3]
+
+ return receiver_LLA;
+}
+
+void computeRandomVisibleSatellite(const Eigen::Vector3d& receiver_latlonalt,
+ Eigen::Vector3d& sat_ENU,
+ Eigen::Vector3d& sat_ECEF,
+ Eigen::Vector2d& sat_azel,
+ double range)
+{
+ Eigen::Vector3d t_ECEF_ENU, t_ENU_ECEF;
+ Eigen::Matrix3d R_ENU_ECEF;
+
+ t_ECEF_ENU = latLonAltToEcef(receiver_latlonalt);
+
+ computeEnuEcefFromLatLonAlt(receiver_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
+
+ // random elevation and azimuth
+ sat_azel = Eigen::Vector2d::Random(); // in [-1, 1]
+ sat_azel(0) *= M_PI; // in [-pi, pi]
+ sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2; // in [0, pi/2]
+ range = Eigen::VectorXd::Random(1)(0) * 5e2 + 1e3; // in [500, 1500]
+
+ // ENU
+ sat_ENU << sin(sat_azel(0)) * cos(sat_azel(1)) * range,
+ cos(sat_azel(0)) * cos(sat_azel(1)) * range,
+ sin(sat_azel(1)) * range;
+
+ // ECEF
+ sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * sat_ENU;
+}
+
+
TEST(TransformationsTest, ecefToLatLonAlt)
{
Eigen::Vector3d ecef, latlonalt;
@@ -78,10 +118,7 @@ TEST(TransformationsTest, latLonAltToEcef_ecefToLatLonAlt)
// latlon -> ecef -> latlon
for (auto i = 0; i<100; i++)
{
- latlonalt0 = Eigen::Vector3d::Random();
- latlonalt0(0) *= M_PI / 2;
- latlonalt0(1) *= M_PI;
- latlonalt0(2) *= 1e3;
+ latlonalt0 = computeRandomReceiverLatLonAlt();
ecef0 = latLonAltToEcef(latlonalt0);
latlonalt1 = ecefToLatLonAlt(ecef0);
@@ -98,10 +135,7 @@ TEST(TransformationsTest, computeEnuEcef)
// random
for (auto i = 0; i<100; i++)
{
- t_ENU_latlonalt = Eigen::Vector3d::Random();
- t_ENU_latlonalt(0) *= M_PI / 2;
- t_ENU_latlonalt(1) *= M_PI;
- t_ENU_latlonalt(2) *= 1e3;
+ t_ENU_latlonalt = computeRandomReceiverLatLonAlt();
t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt);
@@ -159,37 +193,27 @@ TEST(TransformationsTest, computeEnuEcef)
}
}
-TEST(TransformationsTest, computeSatElevation)
+TEST(TransformationsTest, computeAzel)
{
- Eigen::Vector3d t_ENU_ECEF,t_ECEF_ENU,t_ENU_latlonalt,rec_sat_ENU, sat_ECEF;
+ Eigen::Vector3d t_ENU_ECEF,t_ECEF_ENU,t_ENU_latlonalt, sat_ENU, sat_ECEF;
Eigen::Matrix3d R_ENU_ECEF;
+ Eigen::Vector2d azel, azel2;
+ double range;
// random receiver position
for (auto i = 0; i<100; i++)
{
- t_ENU_latlonalt = Eigen::Vector3d::Random();
- t_ENU_latlonalt(0) *= M_PI / 2;
- t_ENU_latlonalt(1) *= M_PI;
- t_ENU_latlonalt(2) *= 1e3;
-
+ t_ENU_latlonalt = computeRandomReceiverLatLonAlt();
t_ECEF_ENU = latLonAltToEcef(t_ENU_latlonalt);
- computeEnuEcefFromLatLonAlt(t_ENU_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
-
// random elevation and heading
for (auto j = 0; j<100; j++)
{
- Eigen::Vector2d rand2 = Eigen::Vector2d::Random();
- double heading = rand2(0) * M_PI;
- double elevation = rand2(0) * M_PI / 2;
-
- rec_sat_ENU << cos(heading) * 1000, sin(heading) * 1000, tan(elevation) * 1000;
-
- sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * rec_sat_ENU;
+ computeRandomVisibleSatellite(t_ENU_latlonalt, sat_ENU, sat_ECEF, azel, range);
- double elevation2 = computeSatElevation(t_ECEF_ENU, sat_ECEF);
+ azel2 = computeAzel(sat_ECEF, t_ECEF_ENU);
- ASSERT_NEAR(elevation, elevation2,1e-6);
+ ASSERT_MATRIX_APPROX(azel,azel2,1e-6);
}
}
}
--
GitLab