diff --git a/include/gnss_utils/utils/satellite.h b/include/gnss_utils/utils/satellite.h index 2f8dac50b926408d12507a77b8d45fd96c332c7f..93d6681adc216524ec4cc5cb82fdaacc5258e4d5 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 bb4bb5e01e9d58e605950f4469a0059ffaee2b04..f4f59d74205ce8ce480685983882990518b41670 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 f0ebbfc96b80ee69240a66cb7c33823665d46332..9682bfde2fa9436d36ec48cabdd127b733f40e9c 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 0000000000000000000000000000000000000000..447c273aab089959db8f905184516e5e4d4146e5 --- /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 cf5a3b05838cc18dff9fe057688c74bd2338104c..2becc26aa19f7703c8308e14fe93a4428a09d6fd 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); } } }