From b61a49a4a43dbe4ae1ce46542811ec7dc273ac71 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 17 Mar 2021 19:41:17 +0100
Subject: [PATCH] WIP
---
include/gnss_utils/observations.h | 13 ++--
include/gnss_utils/range.h | 6 +-
include/gnss_utils/snapshot.h | 3 +-
src/observations.cpp | 78 +++++----------------
src/range.cpp | 49 +++++++++++++
src/snapshot.cpp | 10 +++
src/tdcp.cpp | 112 ++++++++++++++----------------
test/CMakeLists.txt | 16 +++--
test/gtest_tdcp.cpp | 110 ++++++++++++++++-------------
9 files changed, 212 insertions(+), 185 deletions(-)
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index 468c418..fdf52af 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -42,15 +42,10 @@ public:
std::set<int> filterByCode();
std::set<int> filterByCarrierPhase();
std::set<int> filterByConstellations(const int& navsys);
- std::set<int> filterByElevationSnr(const Eigen::Vector3d& x_r,
- const Satellites& sats,
- const snrmask_t& snrmask,
- const double& elmin,
- const bool& multi_freq);
- std::set<int> filterByElevationSnr(const Azels& azels,
- const snrmask_t& snrmask,
- const double& elmin,
- const bool& multi_freq);
+ std::set<int> filterByElevationSnr(const Azels& azels,
+ const snrmask_t& snrmask,
+ const double& elmin,
+ const bool& multi_freq);
std::set<int> filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const Eigen::Vector3d& x_r,
diff --git a/include/gnss_utils/range.h b/include/gnss_utils/range.h
index 7c893a3..d3b1d4d 100644
--- a/include/gnss_utils/range.h
+++ b/include/gnss_utils/range.h
@@ -29,7 +29,9 @@ class Range
double L = -1;
double L_corrected = -1;
double L_var = 1;
- int freq = 0;
+ double L2 = -1;
+ double L2_corrected = -1;
+ double L2_var = 1;
public:
Range();
@@ -41,6 +43,8 @@ class Range
const Azels& azel,
const Eigen::Vector3d& latlonalt,
const Options& opt);
+
+ static std::set<int> findCommonSatellites(const Ranges& ranges_1, const Ranges& ranges_2);
};
} /* namespace GnssUtils */
diff --git a/include/gnss_utils/snapshot.h b/include/gnss_utils/snapshot.h
index f1ebc63..84bbcb0 100644
--- a/include/gnss_utils/snapshot.h
+++ b/include/gnss_utils/snapshot.h
@@ -61,7 +61,8 @@ public:
void computeRanges(const Azels& azel,
const Eigen::Vector3d& latlonalt,
const Options& opt);
-
+ void computeRanges(const Eigen::Vector3d& x_ecef,
+ const Options& opt);
void print();
private:
diff --git a/src/observations.cpp b/src/observations.cpp
index f4f59d7..00a140f 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -304,18 +304,20 @@ std::set<int> Observations::filterByConstellations(const int& navsys)
return remove_sats;
}
-std::set<int> Observations::filterByElevationSnr(const std::map<int,Eigen::Vector2d>& azels,
+std::set<int> Observations::filterByElevationSnr(const Azels& azels,
const snrmask_t& snrmask,
const double& elmin,
const bool &multi_freq)
{
std::set<int> remove_sats;
- for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ //for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ for (auto& azel_pair : azels)
{
- auto&& obs_sat = getObservationByIdx(obs_i);
- const int& sat_number = obs_sat.sat;
- const double& elevation(azels.at(sat_number)(1));
+ const int& sat_number = azel_pair.first;
+ const double& elevation(azel_pair.second(1));
+ assert(hasSatellite(sat_number));
+ auto&& obs_sat = getObservationBySat(sat_number);
// check elevation
if (elevation < elmin)
@@ -354,18 +356,6 @@ std::set<int> Observations::filterByElevationSnr(const std::map<int,Eigen::Vecto
return remove_sats;
}
-std::set<int> Observations::filterByElevationSnr(const Eigen::Vector3d& x_r,
- const Satellites& sats,
- const snrmask_t& snrmask,
- const double& elmin,
- const bool &multi_freq)
-{
- std::set<int> remove_sats;
- Azels azels = computeAzels(sats, x_r);
-
- return filterByElevationSnr(azels, snrmask, elmin, multi_freq);
-}
-
std::set<int> Observations::filter(const Satellites& sats,
const std::set<int>& discarded_sats,
const Eigen::Vector3d& x_r,
@@ -394,51 +384,19 @@ std::set<int> Observations::filter(const Satellites& sats,
const double& elmin,
const bool& multi_freq)
{
- //std::cout << "filter: initial size: " << obs_.size() << std::endl;
- // Ephemeris
- std::set<int> remove_sats = filterByEphemeris(sats);
-
- // Discarded sats
- std::set<int> remove_sats_discarded = filterBySatellites(discarded_sats);
- remove_sats.insert(remove_sats_discarded.begin(), remove_sats_discarded.end());
-
- // Code
- if (check_code)
- {
- std::set<int> remove_sats_code = filterByCode();
- remove_sats.insert(remove_sats_code.begin(), remove_sats_code.end());
- }
-
- // Carrier phase
- if (check_carrier_phase)
- {
- std::set<int> remove_sats_carrier = filterByCarrierPhase();
- remove_sats.insert(remove_sats_carrier.begin(), remove_sats_carrier.end());
- }
+ assert(!x_r.isApprox(Eigen::Vector3d::Zero(), 1e-3));
- // satellite flag and ephemeris variance
- for (auto sat_pair : sats)
- if (remove_sats.count(sat_pair.first) == 0)
- if (satexclude(sat_pair.first,sat_pair.second.var, sat_pair.second.svh, NULL))
- {
- //std::cout << "Discarding sat " << sat_pair.first << ": unhealthy or huge variance svh = " << sat_pair.second.svh << std::endl;
- removeObservationBySat(sat_pair.first);
- remove_sats.insert(sat_pair.first);
- }
-
- // Constellations
- std::set<int> remove_sats_constellations = filterByConstellations(navsys);
- remove_sats.insert(remove_sats_constellations.begin(), remove_sats_constellations.end());
-
- // Elevation and SNR
- if (!x_r.isApprox(Eigen::Vector3d::Zero(), 1e-3))
- {
- std::set<int> remove_sats_elevation = filterByElevationSnr(x_r, sats, snrmask, elmin, multi_freq);
- remove_sats.insert(remove_sats_elevation.begin(), remove_sats_elevation.end());
- }
+ Azels azels = computeAzels(sats, x_r);
- return remove_sats;
- // std::cout << "final size: " << obs_.size() << std::endl;
+ return filter(sats,
+ discarded_sats,
+ azels,
+ check_code,
+ check_carrier_phase,
+ navsys,
+ snrmask,
+ elmin,
+ multi_freq);
}
std::set<int> Observations::filter(const Satellites& sats,
diff --git a/src/range.cpp b/src/range.cpp
index a990cbf..86871b4 100644
--- a/src/range.cpp
+++ b/src/range.cpp
@@ -103,9 +103,21 @@ Ranges Range::computeRanges(ObservationsPtr obs,
ranges[sat].P_var = varerr(&prcopt,azel_i[1],prcopt.err[5],sat_i.sys)+sat_i.var+vmeas+vion+vtrp;
/* ------------------- Carrier phase ------------------- */
+ // L1
ranges[sat].L = obs_i.L[0]*nav->getNavigation().lam[sat-1][0];
ranges[sat].L_corrected = ranges[sat].L;
+ int sys = satsys(sat, NULL);
+ // L2 (GPS/GLO/QZS)
+ if (sys & (SYS_GPS | SYS_GLO | SYS_QZS))
+ ranges[sat].L2 = obs_i.L[1]*nav->getNavigation().lam[sat-1][1];
+ // E5 (GAL)
+ else if (sys & SYS_GAL)
+ ranges[sat].L2 = obs_i.L[2]*nav->getNavigation().lam[sat-1][2];
+ else
+ ranges[sat].L2 = 0;
+ ranges[sat].L2_corrected = ranges[sat].L2;
+
/* ionospheric corrections */
if (opt.tdcp.corr_iono)
{
@@ -115,19 +127,29 @@ Ranges Range::computeRanges(ObservationsPtr obs,
//ranges[sat].L_corrected = obs_i.L[0]*nav->getNavigation().lam[sat-1][0];
}
else
+ {
ranges[sat].L_corrected -= ranges[sat].iono_corr;
+ ranges[sat].L2_corrected -= ranges[sat].iono_corr; //FIXME: different?
+ }
}
/* tropospheric corrections */
if (opt.tdcp.corr_tropo)
+ {
ranges[sat].L_corrected += ranges[sat].tropo_corr;
+ ranges[sat].L2_corrected += ranges[sat].tropo_corr; //FIXME: different?
+ }
/* sat clock corrections */
if (opt.tdcp.corr_clock)
+ {
ranges[sat].L_corrected += ranges[sat].sat_clock_corr;
+ ranges[sat].L2_corrected += ranges[sat].sat_clock_corr; //FIXME: different?
+ }
/* carrier phase variance */
ranges[sat].L_var = opt.tdcp.sigma_carrier * opt.tdcp.sigma_carrier;
+ ranges[sat].L2_var = opt.tdcp.sigma_carrier * opt.tdcp.sigma_carrier;
//std::cout << std::endl
// << "\t\tprange: " << pranges[sat].prange << std::endl
@@ -141,4 +163,31 @@ Ranges Range::computeRanges(ObservationsPtr obs,
return ranges;
}
+
+
+std::set<int> Range::findCommonSatellites(const Ranges& ranges_1, const Ranges& ranges_2)
+{
+ std::set<int> common_sats;
+
+ // std::cout << "ranges_1: ";
+ // for (auto&& range_pair : ranges_1)
+ // std::cout << range_pair.first << " ";
+ // std::cout << std::endl;
+ // std::cout << "ranges_2: ";
+ // for (auto&& range_pair : ranges_2)
+ // std::cout << range_pair.first << " ";
+ // std::cout << std::endl;
+
+ for (auto&& range_pair : ranges_1)
+ if (ranges_2.count(range_pair.first))
+ common_sats.insert(range_pair.first);
+
+ // std::cout << "common sats: ";
+ // for (auto sat : common_sats)
+ // std::cout << sat << " ";
+ // std::cout << std::endl;
+
+ return common_sats;
+}
+
} /* namespace GnssUtils */
diff --git a/src/snapshot.cpp b/src/snapshot.cpp
index 283cd3c..49e13bb 100644
--- a/src/snapshot.cpp
+++ b/src/snapshot.cpp
@@ -1,5 +1,6 @@
#include "gnss_utils/snapshot.h"
#include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/utils/transformations.h"
using namespace GnssUtils;
@@ -41,3 +42,12 @@ void Snapshot::computeRanges(const Azels& azel,
ranges_ = Range::computeRanges(obs_, nav_, sats_, azel, latlonalt, opt);
}
+
+void Snapshot::computeRanges(const Eigen::Vector3d& x_ecef,
+ const Options& opt)
+{
+ ranges_ = Range::computeRanges(obs_, nav_, sats_,
+ computeAzels(sats_, x_ecef),
+ ecefToLatLonAlt(x_ecef),
+ opt);
+}
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index 25a1726..bb36f5b 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -48,28 +48,24 @@ bool Tdcp(SnapshotPtr snapshot_r,
}
// COMPUTE SATELLITES POSITION
- if (!snapshot_r->satellitesComputed())
+ if (not snapshot_r->satellitesComputed())
snapshot_r->computeSatellites(opt.sateph);
- if (!snapshot_k->satellitesComputed())
+ if (not snapshot_k->satellitesComputed())
snapshot_k->computeSatellites(opt.sateph);
+ // COMPUTE RANGES
+ if (not snapshot_r->rangesComputed())
+ snapshot_r->computeRanges(x_r, opt);
+ if (not snapshot_k->rangesComputed())
+ snapshot_k->computeRanges(x_r, opt); // the x_r is only used to compute Azels -> corrections
+
// FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR)
- snapshot_r->getObservations()->filter(snapshot_r->getSatellites(),
- discarded_sats,
- x_r,
- false,
- true,
- opt);
- snapshot_k->getObservations()->filter(snapshot_k->getSatellites(),
- discarded_sats,
- x_r,
- false,
- true,
- opt);
-
- // FIND COMMON SATELLITES OBSERVATIONS
- std::set<int> common_sats = Observations::findCommonObservations(*snapshot_r->getObservations(),
- *snapshot_k->getObservations());
+ snapshot_r->filterObservations(discarded_sats, x_r, false, true, opt);
+ snapshot_k->filterObservations(discarded_sats, x_r, false, true, opt);
+
+ // FIND COMMON SATELLITES
+ std::set<int> common_sats = Range::findCommonSatellites(snapshot_r->getRanges(),
+ snapshot_k->getRanges());
// COMPUTE TDCP
bool tdcp_ok = Tdcp(snapshot_r,
@@ -102,24 +98,29 @@ bool Tdcp(SnapshotPtr snapshot_r,
std::set<int> raim_discarded_sats,
const TdcpBatchParams& tdcp_params)
{
- // TODO: change obs -> ranges
- // Checks
+ // CHECKS
+ if (not raim_discarded_sats.empty())
+ {
+ std::cout << "TDCP: 'raim_discarded_sats' is just an output param, it will be cleared.\n";
+ raim_discarded_sats.clear();
+ }
assert(snapshot_r->satellitesComputed() && "this TDCP assumes satellites already computed");
assert(snapshot_k->satellitesComputed() && "this TDCP assumes satellites already computed");
- assert(snapshot_r->getSatellites().size() > snapshot_r->getObservations()->size() && "Satellites without position not filtered");
- assert(snapshot_k->getSatellites().size() > snapshot_k->getObservations()->size() && "Satellites without position not filtered");
- assert(snapshot_r->getSatellites().size() > common_sats.size() && "Too many common sats");
- assert(snapshot_k->getSatellites().size() > common_sats.size() && "Too many common sats");
- assert(raim_discarded_sats.empty() && "just output param");
-
+ assert(snapshot_r->rangesComputed() && "this TDCP assumes ranges already computed");
+ assert(snapshot_k->rangesComputed() && "this TDCP assumes ranges already computed");
+ assert(snapshot_r->getSatellites().size() >= common_sats.size() && "Too many common sats");
+ assert(snapshot_k->getSatellites().size() >= common_sats.size() && "Too many common sats");
+ assert(snapshot_r->getRanges().size() >= common_sats.size() && "Too many common sats");
+ assert(snapshot_k->getRanges().size() >= common_sats.size() && "Too many common sats");
for (auto sat : common_sats)
{
- assert(snapshot_r->getObservations()->hasSatellite(sat) && "wrong common sat");
- assert(snapshot_k->getObservations()->hasSatellite(sat) && "wrong common sat");
- assert(snapshot_r->getSatellites().count(sat) && "common sat without position");
- assert(snapshot_k->getSatellites().count(sat) && "common sat without position");
+ assert(snapshot_r->getSatellites().count(sat) && "common sat not stored in satellites");
+ assert(snapshot_k->getSatellites().count(sat) && "common sat not stored in satellites");
+ assert(snapshot_r->getRanges().count(sat) && "common sat not stored in ranges");
+ assert(snapshot_k->getRanges().count(sat) && "common sat not stored in ranges");
}
+
int n_common_sats = common_sats.size();
int required_n_sats(std::max(tdcp_params.min_common_sats + tdcp_params.raim_n, 4 + tdcp_params.raim_n));
if (n_common_sats < required_n_sats)
@@ -136,33 +137,28 @@ bool Tdcp(SnapshotPtr snapshot_r,
double tk = snapshot_k->getGPST();
// MULTI-FREQUENCY
+ // TODO: change obs -> ranges
std::map<int, std::pair<int, int>> row_2_sat_freq;
int row = 0;
for (auto sat : common_sats)
{
- assert(snapshot_r->getObservations()->hasSatellite(sat));
- assert(snapshot_k->getObservations()->hasSatellite(sat));
+ assert(snapshot_r->getRanges().count(sat));
+ assert(snapshot_k->getRanges().count(sat));
- auto&& obs_r = snapshot_r->getObservations()->getObservationBySat(sat);
- auto&& obs_k = snapshot_k->getObservations()->getObservationBySat(sat);
+ auto&& range_r = snapshot_r->getRanges().at(sat);
+ auto&& range_k = snapshot_k->getRanges().at(sat);
// Carrier phase
// L1/E1
- if (std::abs(obs_r.L[0]) > 1e-12 and std::abs(obs_k.L[0]) > 1e-12)
+ if (std::abs(range_r.L_corrected) > 1e-12 and std::abs(range_k.L_corrected) > 1e-12)
row_2_sat_freq[row++] = std::make_pair(sat, 0);
if (!tdcp_params.tdcp.multi_freq)
continue;
- // L2 (GPS/GLO/QZS)
- int sys = satsys(sat, NULL);
- if (NFREQ >= 2 and (sys & (SYS_GPS | SYS_GLO | SYS_QZS)) and std::abs(obs_r.L[1]) > 1e-12 and
- std::abs(obs_k.L[1]) > 1e-12)
+ // L2 (GPS/GLO/QZS) / E5 (GAL)
+ if (std::abs(range_r.L2_corrected) > 1e-12 and std::abs(range_k.L2_corrected) > 1e-12)
row_2_sat_freq[row++] = std::make_pair(sat, 1);
-
- // E5 (GAL)
- else if (NFREQ >= 3 and (sys & SYS_GAL) and std::abs(obs_r.L[2]) > 1e-12 and std::abs(obs_k.L[2]) > 1e-12)
- row_2_sat_freq[row++] = std::make_pair(sat, 2);
}
int n_differences = row_2_sat_freq.size();
@@ -188,32 +184,26 @@ bool Tdcp(SnapshotPtr snapshot_r,
const int& sat_number = row_sat_freq_it.second.first;
const int& freq = row_sat_freq_it.second.second;
- auto obs_r = snapshot_r->getObservations()->getObservationBySat(sat_number);
- auto obs_k = snapshot_k->getObservations()->getObservationBySat(sat_number);
- auto nav_r = snapshot_r->getNavigation()->getNavigation();
- auto nav_k = snapshot_k->getNavigation()->getNavigation();
+ auto&& range_r = snapshot_r->getRanges().at(sat_number);
+ auto&& range_k = snapshot_k->getRanges().at(sat_number);
// Satellite's positions
s_r.col(row) = snapshot_r->getSatellites().at(sat_number).pos;
s_k.col(row) = snapshot_k->getSatellites().at(sat_number).pos;
- //nav_k.ion_gps;
-
- drho_m(row) = (obs_k.L[freq] * nav_k.lam[sat_number - 1][freq]) -
- (obs_r.L[freq] * nav_r.lam[sat_number - 1][freq]);
+ if (freq == 0)
+ drho_m(row) = range_k.L_corrected - range_r.L_corrected;
+ else
+ drho_m(row) = range_k.L2_corrected - range_r.L2_corrected;
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "\tsat " << sat_number
- << " frequency " << freq
- << " wavelength = " << nav_r.lam[sat_number-1][freq] << std::endl;
- std::cout << std::setprecision(10) << "\tpositions:\n\t\ts_r: " << s_r.col(row).transpose()
- << "\n\t\ts_k: " << s_k.col(row).transpose() << std::endl;
- std::cout << "\tobs_r.L: " << obs_r.L[freq] << std::endl;
- std::cout << "\tobs_k.L: " << obs_k.L[freq] << std::endl;
- std::cout << "\tnav_r.getNavigation().lam[sat_number-1][freq]: " << nav_r.lam[sat_number-1][freq] << std::endl;
- std::cout << "\tnav_k.getNavigation().lam[sat_number-1][freq]: " << nav_k.lam[sat_number-1][freq] << std::endl;
- std::cout << "\trho_r: " << obs_r.L[freq] * nav_r.lam[sat_number-1][freq] << std::endl;
- std::cout << "\trho_k: " << obs_k.L[freq] * nav_k.lam[sat_number-1][freq] << std::endl;
+ std::cout << std::setprecision(10)
+ << "\tpositions:" << std::endl
+ << "\t\ts_r: " << s_r.col(row).transpose() << std::endl
+ << "\t\ts_k: " << s_k.col(row).transpose() << std::endl;
+ std::cout << "\trange_r.L: " << (freq == 0 ? range_r.L_corrected : range_r.L2_corrected) << std::endl;
+ std::cout << "\trange_k.L: " << (freq == 0 ? range_k.L_corrected : range_k.L2_corrected) << std::endl;
std::cout << "\tdrho_m: " << drho_m(row) << std::endl;
#endif
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index e7ef282..4b38d2e 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -12,14 +12,18 @@ include_directories(${GTEST_INCLUDE_DIRS})
# #
##############################################################
-# Transformations test
-gnss_utils_add_gtest(gtest_transformations gtest_transformations.cpp)
-target_link_libraries(gtest_transformations ${PROJECT_NAME})
+# Navigation test
+gnss_utils_add_gtest(gtest_navigation gtest_navigation.cpp)
+target_link_libraries(gtest_navigation libgtest ${PROJECT_NAME})
# Observations test
gnss_utils_add_gtest(gtest_observations gtest_observations.cpp)
target_link_libraries(gtest_observations libgtest ${PROJECT_NAME})
-# Navigation test
-gnss_utils_add_gtest(gtest_navigation gtest_navigation.cpp)
-target_link_libraries(gtest_navigation libgtest ${PROJECT_NAME})
\ No newline at end of file
+# TDCP test
+gnss_utils_add_gtest(gtest_tdcp gtest_tdcp.cpp)
+target_link_libraries(gtest_tdcp ${PROJECT_NAME})
+
+# Transformations test
+gnss_utils_add_gtest(gtest_transformations gtest_transformations.cpp)
+target_link_libraries(gtest_transformations ${PROJECT_NAME})
\ No newline at end of file
diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp
index 3b5d0dc..b60f7f2 100644
--- a/test/gtest_tdcp.cpp
+++ b/test/gtest_tdcp.cpp
@@ -2,17 +2,12 @@
#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;
+using namespace Eigen;
-Eigen::Vector3d computeRandomReceiverLatLonAlt()
+Vector3d computeRandomReceiverLatLonAlt()
{
- Eigen::Vector3d receiver_LLA = Eigen::Vector3d::Random(); // in [-1, 1]
+ Vector3d receiver_LLA = 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])
@@ -21,24 +16,24 @@ Eigen::Vector3d computeRandomReceiverLatLonAlt()
return receiver_LLA;
}
-void computeRandomVisibleSatellite(const Eigen::Vector3d& receiver_latlonalt,
- Eigen::Vector3d& sat_ENU,
- Eigen::Vector3d& sat_ECEF,
- Eigen::Vector2d& sat_azel,
+void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
+ Vector3d& sat_ENU,
+ Vector3d& sat_ECEF,
+ Vectox_kd& sat_azel,
double range)
{
- Eigen::Vector3d t_ECEF_ENU, t_ENU_ECEF;
- Eigen::Matrix3d R_ENU_ECEF;
+ Vector3d t_ECEF_ENU, t_ENU_ECEF;
+ 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 = Vectox_kd::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]
+ range = VectorXd::Random(1)(0) * 5e2 + 1e3; // in [500, 1500]
// ENU
sat_ENU << sin(sat_azel(0)) * cos(sat_azel(1)) * range,
@@ -52,55 +47,76 @@ void computeRandomVisibleSatellite(const Eigen::Vector3d& receiver_latlonalt,
TEST(Tdcp, Tdcp)
{
- Eigen::Vector3d sat_ENU, sat_ECEF;
- Eigen::Vector3d r1_LLA, r1_ECEF, r2_LLA, r2_ECEF, d_ECEF;
- Eigen::Matrix3d R_ENU_ECEF;
- Eigen::Vector2d azel, azel2;
+ TdcpBatchParams tdcp_params;
+
+ Vector3d sat_ENU, sat_ECEF;
+ Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
+ Matrix3d R_ENU_ECEF;
+ Vectox_kd azel, azel2;
+ Vector4d d, d_gt;
+ Matrix4d cov_d;
+ double residual;
double range;
- Satellite sat1({0,0,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),1.0,0,0,1});
- Satellite sat2(sat1);
+ Satellite sat_def({0,0,Vector3d::Zero(),Vector3d::Zero(),1.0,0,0,1});
- auto shapshot1 = std::make_shared<Snapshot>();
- auto shapshot2 = std::make_shared<Snapshot>();
+ auto snapshot_r = std::make_shared<Snapshot>();
+ auto snapshot_k = std::make_shared<Snapshot>();
// 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;
- r2_LLA = ecefToLatLonAlt(r2_ECEF);
+ x_r_LLA = computeRandomReceiverLatLonAlt();
+ x_r_ECEF = latLonAltToEcef(x_r_LLA);
+ d_ECEF = Vector3d::Random() * 10;
+ x_k_ECEF = x_r_ECEF + d_ECEF;
+ x_k_LLA = ecefToLatLonAlt(x_k_ECEF);
+ // TODO: random clock biases
+ double clock_r, clock_k;
+
+ // groundtruth
+ d_gt.head<3>() = d_ECEF;
+ d_gt(3) = clock_k - clock_r;
+
+ std::set<int> common_sats;
+ std::set<int> raim_discarded_sats;
// random visible satellites
for (auto j = 0; j<10; j++)
{
- sat1.sat = j;
- sat2.sat = j;
+ common_sats.insert(j);
+ // Satellite 1 (random)
+ computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range);
+ EXPECT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-6);
- computeRandomVisibleSatellite(r1_LLA, sat_ENU, sat_ECEF, azel, range);
- sat1.pos = sat_ECEF;
- computeRandomVisibleSatellite(r2_LLA, sat_ENU, sat_ECEF, azel, range);
+ Satellite sat1({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+ snapshot1.getSatellites().emplace(j, sat1);
- azel2 = computeAzel(sat_ECEF, t_ECEF_ENU);
+ // TODO: compute range and add random clock bias
- ASSERT_MATRIX_APPROX(azel,azel2,1e-6);
- }
+ // Satellite 2 (random)
+ computeRandomVisibleSatellite(x_k_LLA, sat_ENU, sat_ECEF, azel, range);
+ EXPECT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-6);
- bool Tdcp(SnapshotPtr snapshot_r,
- SnapshotPtr snapshot_k,
- r1_ECEF,
- const std::set<int> common_sats,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- std::set<int> raim_discarded_sats,
- const TdcpBatchParams& tdcp_params)
- }
+ Satellite sat2({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+ snapshot2.getSatellites().emplace(j, sat2);
+ // TODO: compute range and add random clock bias
+ }
+ bool tdcp_ok = Tdcp(snapshot_r,
+ snapshot_k,
+ x_r_ECEF,
+ common_sats,
+ d,
+ cov_d,
+ residual,
+ raim_discarded_sats,
+ tdcp_params);
+
+ EXPECT_TRUE(tdcp_ok);
+ }
}
int main(int argc, char **argv)
--
GitLab