WIP

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
     {

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);
 }

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,

test/gtest_tdcp.cpp

+#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();
+}

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);
         }
     }
 }