diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index cf3c80b25b51685df6362d5ab9e6757cb3073782..146bdc584155a9b4a22cef831846e9e24ffe6e75 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -118,7 +118,7 @@ struct Options
int tropopt; // troposphere option: TROPOPT_OFF(0):correction off, TROPOPT_SAAS(1):Saastamoinen model, TROPOPT_SBAS(2):SBAS model, TROPOPT_EST(3):troposphere option: ZTD estimation, TROPOPT_ESTG(4):ZTD+grad estimation, TROPOPT_ZTD(5):ZTD correction,6:ZTD+grad correction
int sbascorr; // SBAS correction options (can be added): SBSOPT_LCORR(1): long term correction, SBSOPT_FCORR(2): fast correction, SBSOPT_ICORR(4): ionosphere correction, SBSOPT_RANGE(8): ranging
int raim; // RAIM removed sats
- double elmin; // min elevation (degrees)
+ double elmin; // min elevation (rad)
double maxgdop; // maxgdop: reject threshold of gdop
bool GPS,SBS,GLO,GAL,QZS,CMP,IRN,LEO; // constellations used
TdcpOptions tdcp; // TDCP options
@@ -173,7 +173,7 @@ class Range;
// Typedefs
typedef std::map<int,Satellite> Satellites;
typedef std::map<int,Range> Ranges;
-typedef std::map<int,Eigen::Vector2d> Azels;
+typedef std::map<int,Eigen::Vector2d> Azels; // Azimuth and elevation (rad)
// pointer typedefs
typedef std::shared_ptr<Observations> ObservationsPtr;
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index 468c418d2e618647e423a76d42c428c361a4908a..fdf52af10071f8fcf68573f3226c1f2e6524a629 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 caf9e9db23c97692ba3e4bb1a438e8162b5c24ea..d3b1d4d9b1b78e8be24b6d0d7c548c564dc529f4 100644
--- a/include/gnss_utils/range.h
+++ b/include/gnss_utils/range.h
@@ -18,6 +18,7 @@ namespace GnssUtils
class Range
{
public:
+ int sys = SYS_GPS;
int sat = 0;
double P = -1;
double P_var = 1;
@@ -28,6 +29,9 @@ class Range
double L = -1;
double L_corrected = -1;
double L_var = 1;
+ double L2 = -1;
+ double L2_corrected = -1;
+ double L2_var = 1;
public:
Range();
@@ -39,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 e2ef0cefc5d3144f0e916e92619edd2b50e4fd37..49a167503ad07aabeca7036337e2d57d9fb86ff0 100644
--- a/include/gnss_utils/snapshot.h
+++ b/include/gnss_utils/snapshot.h
@@ -27,16 +27,15 @@ public:
// Public methods
void loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
+ double getGPST() const;
- ObservationsPtr getObservations() const;
+ // Navigation
NavigationPtr getNavigation() const;
- const Satellites& getSatellites() const;
- Satellites& getSatellites();
- void setObservations(ObservationsPtr obs);
void setNavigation(NavigationPtr nav);
- void computeSatellites(const int& eph_opt); // see rtklib.h L396);
- bool satellitesComputed() const;
+ // Observations
+ ObservationsPtr getObservations() const;
+ void setObservations(ObservationsPtr obs);
std::set<int> filterObservations(const std::set<int>& discarded_sats,
const Eigen::Vector3d& x_r,
@@ -49,14 +48,21 @@ public:
const bool& check_carrier_phase,
const Options& opt);
- // Pseudo-ranges
+ // Satellites
+ const Satellites& getSatellites() const;
+ Satellites& getSatellites();
+ void computeSatellites(const int& eph_opt); // see rtklib.h L396);
+ bool satellitesComputed() const;
+
+ // Ranges
Ranges& getRanges();
const Ranges& getRanges() const;
bool rangesComputed() const;
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:
@@ -81,6 +87,11 @@ private:
namespace GnssUtils
{
+inline double Snapshot::getGPST() const
+{
+ return obs_->getObservations()[0].time.time + obs_->getObservations()[0].time.sec;
+}
+
inline GnssUtils::ObservationsPtr Snapshot::getObservations() const
{
return obs_;
@@ -122,7 +133,15 @@ inline std::set<int> Snapshot::filterObservations(const std::set<int>& disc
const bool& check_carrier_phase,
const Options& opt)
{
- return obs_->filter(sats_, discarded_sats, x_r, check_code, check_carrier_phase, opt);
+ std::set<int> filtered_sats = obs_->filter(sats_, discarded_sats, x_r, check_code, check_carrier_phase, opt);
+
+ for (auto sat : filtered_sats)
+ {
+ sats_.erase(sat);
+ ranges_.erase(sat);
+ }
+
+ return filtered_sats;
}
inline std::set<int> Snapshot::filterObservations(const std::set<int>& discarded_sats,
@@ -131,7 +150,15 @@ inline std::set<int> Snapshot::filterObservations(const std::set<int>& discarde
const bool& check_carrier_phase,
const Options& opt)
{
- return obs_->filter(sats_, discarded_sats, azels, check_code, check_carrier_phase, opt);
+ std::set<int> filtered_sats = obs_->filter(sats_, discarded_sats, azels, check_code, check_carrier_phase, opt);
+
+ for (auto sat : filtered_sats)
+ {
+ sats_.erase(sat);
+ ranges_.erase(sat);
+ }
+
+ return filtered_sats;
}
inline const Ranges& Snapshot::getRanges() const
diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 143fa67b4d775ae123fafffbb43df65700445946..dadb9384f8786ac3d2f135e5016eceec55cfd0d2 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -14,40 +14,53 @@ struct TdcpBatchParams
int raim_n;
double raim_min_residual;
bool relinearize_jacobian;
- bool old_nav;
int max_iterations;
};
-bool Tdcp(SnapshotPtr snapshot_r,
- SnapshotPtr snapshot_k,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- const std::set<int>& discarded_sats,
- std::set<int>& raim_discarded_sats,
- const TdcpBatchParams& tdcp_params,
- const Options& opt);
-
-bool Tdcp(SnapshotPtr snapshot_r,
- SnapshotPtr snapshot_k,
- const Eigen::Vector3d& x_r,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- const std::set<int>& discarded_sats,
- std::set<int>& raim_discarded_sats,
- const TdcpBatchParams& tdcp_params,
- const Options& opt);
-
-bool Tdcp(SnapshotPtr snapshot_r,
- SnapshotPtr snapshot_k,
- const Eigen::Vector3d& x_r,
- 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);
+struct TdcpOutput
+{
+ bool success = false;
+ std::string msg = "";
+ std::set<int> raim_discarded_sats;
+ std::set<int> used_sats;
+ Eigen::Vector4d d = Eigen::Vector4d::Zero();
+ Eigen::Matrix4d cov_d = Eigen::Matrix4d::Zero();
+ double dt = 0;
+ double residual = 0;
+};
+
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const TdcpBatchParams& tdcp_params,
+ const Options& opt);
+
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const TdcpBatchParams& tdcp_params,
+ const Options& opt);
+
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const std::set<int>& common_sats,
+ const Eigen::Vector4d& d_0,
+ const TdcpBatchParams& tdcp_params);
+
+TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
+ Eigen::MatrixXd& A,
+ Eigen::VectorXd& r,
+ Eigen::VectorXd& drho_m,
+ Eigen::MatrixXd& s_k,
+ Eigen::MatrixXd& s_r,
+ const Eigen::Vector4d& d_0,
+ std::set<int>& raim_discarded_rows,
+ const TdcpBatchParams& tdcp_params);
+
} // namespace GnssUtils
#endif /* INCLUDE_GNSS_UTILS_Tdcp_H_ */
diff --git a/include/gnss_utils/utils/satellite.h b/include/gnss_utils/utils/satellite.h
index 491f331cba40bf7de69909cc7a712909afae87c5..93d6681adc216524ec4cc5cb82fdaacc5258e4d5 100644
--- a/include/gnss_utils/utils/satellite.h
+++ b/include/gnss_utils/utils/satellite.h
@@ -19,12 +19,16 @@ 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 Eigen::Vector3d& sat_ecef, const Eigen::Vector3d& receiver_ecef);
+
+ Eigen::Vector2d computeAzel(const Satellite& sat, const Eigen::Vector3d& receiver_ecef);
+
+ Azels computeAzels(const Satellites& sats, const Eigen::Vector3d& receiver_ecef);
+
struct Satellite
{
int sys;
diff --git a/src/observations.cpp b/src/observations.cpp
index bb4bb5e01e9d58e605950f4469a0059ffaee2b04..cb43c9113feded557ab7f33552a1ca1c26407d27 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -304,18 +304,19 @@ 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 (auto&& sat_idx_pair : sat_2_idx_)
{
- auto&& obs_sat = getObservationByIdx(obs_i);
- const int& sat_number = obs_sat.sat;
+ const int& sat_number = sat_idx_pair.first;
+ assert(azels.count(sat_number) && "azel missing for a satellite of this observation");
const double& elevation(azels.at(sat_number)(1));
+ auto&& obs_sat = getObservationByIdx(sat_idx_pair.second);
// check elevation
if (elevation < elmin)
@@ -354,27 +355,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;
- 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));
- }
-
- 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,
@@ -403,51 +383,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());
+ assert(!x_r.isApprox(Eigen::Vector3d::Zero(), 1e-3));
- // Code
- if (check_code)
- {
- std::set<int> remove_sats_code = filterByCode();
- remove_sats.insert(remove_sats_code.begin(), remove_sats_code.end());
- }
+ Azels azels = computeAzels(sats, x_r);
- // Carrier phase
- if (check_carrier_phase)
- {
- std::set<int> remove_sats_carrier = filterByCarrierPhase();
- remove_sats.insert(remove_sats_carrier.begin(), remove_sats_carrier.end());
- }
-
- // 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());
- }
-
- 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 a990cbfabdaaa8f07530cccf7dcc8976c4c3fbe7..86871b4dce6ac8294964dd23a29c148762b78ad0 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 283cd3c593aa8dfe091212d6171beb658a378b30..49e13bb30152fb6393e995603bc5e6c46b9a65f4 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 1bea6dab33bd705a8c5fee41bfe052b8bade5953..e56c28506a70813966adeeba9f81b7e3c1b354f3 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -6,117 +6,100 @@
namespace GnssUtils
{
-bool Tdcp(SnapshotPtr snapshot_r,
- SnapshotPtr snapshot_k,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- const std::set<int>& discarded_sats,
- std::set<int>& raim_discarded_sats,
- const TdcpBatchParams& tdcp_params,
- const Options& opt)
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const TdcpBatchParams& tdcp_params,
+ const Options& opt)
{
return Tdcp(snapshot_r,
snapshot_k,
computePos(*snapshot_r->getObservations(), *snapshot_r->getNavigation(), opt).pos,
- d,
- cov_d,
- residual,
+ d_0,
discarded_sats,
- raim_discarded_sats,
tdcp_params,
opt);
}
-bool Tdcp(SnapshotPtr snapshot_r,
- SnapshotPtr snapshot_k,
- const Eigen::Vector3d& x_r,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- const std::set<int>& discarded_sats,
- std::set<int>& raim_discarded_sats,
- const TdcpBatchParams& tdcp_params,
- const Options& opt)
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const TdcpBatchParams& tdcp_params,
+ const Options& opt)
{
// If use old nav temporary change navigation to (re)compute satellites positions
auto nav_k = snapshot_k->getNavigation();
- if (tdcp_params.old_nav)
+ if (tdcp_params.tdcp.use_old_nav)
{
- snapshot_k->getSatellites().clear();
- snapshot_k->setNavigation(snapshot_r->getNavigation());
+ auto new_snapshot_k = std::make_shared<Snapshot>(std::make_shared<Observations>(*snapshot_k->getObservations()),
+ std::make_shared<Navigation>(*snapshot_k->getNavigation()));
+ snapshot_k = new_snapshot_k;
}
// 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);
// 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);
- // COMPUTE TDCP
- bool tdcp_ok = Tdcp(snapshot_r,
- snapshot_k,
- x_r,
- common_sats,
- d,
- cov_d,
- residual,
- raim_discarded_sats,
- tdcp_params);
-
- // UNDO temporary change navigation
- if (tdcp_params.old_nav)
+ // 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
+
+ // FIND COMMON SATELLITES
+ std::set<int> common_sats = Range::findCommonSatellites(snapshot_r->getRanges(),
+ snapshot_k->getRanges());
+
+ if (common_sats.empty())
{
- snapshot_k->setNavigation(nav_k);
- snapshot_k->computeSatellites(opt.sateph);
+ TdcpOutput output;
+ output.success = false;
+ output.msg = "No common satellites after filtering observations.";
+ return output;
}
- return tdcp_ok;
+ // COMPUTE TDCP
+ return Tdcp(snapshot_r,
+ snapshot_k,
+ x_r,
+ common_sats,
+ d_0,
+ tdcp_params);
}
-bool Tdcp(SnapshotPtr snapshot_r,
- SnapshotPtr snapshot_k,
- const Eigen::Vector3d& x_r,
- 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)
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const std::set<int>& common_sats,
+ const Eigen::Vector4d& d_0,
+ const TdcpBatchParams& tdcp_params)
{
- // Checks
+
+ TdcpOutput output;
+
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();
@@ -127,56 +110,43 @@ bool Tdcp(SnapshotPtr snapshot_r,
printf("Tdcp: NOT ENOUGH COMMON SATS");
printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]", n_common_sats, required_n_sats);
#endif
- return false;
+ output.msg = "Not enough common sats";
+ output.success = false;
+ return output;
}
// Times
- double tr(-1), tk(-1);
+ double tr = snapshot_r->getGPST();
+ double tk = snapshot_k->getGPST();
// MULTI-FREQUENCY
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);
-
- // fill times
- if (tr < 0)
- {
- tr = obs_r.time.time + obs_r.time.sec;
- tk = obs_k.time.time + obs_k.time.sec;
- }
+ 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();
- // Initial guess
- Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
-
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
- std::cout << "d initial guess: " << d_0.transpose() << std::endl;
+ std::cout << "d initial guess: " << d.transpose() << std::endl;
std::cout << "common sats: ";
for (auto row_sat_freq_it : row_2_sat_freq)
std::cout << row_sat_freq_it.second.first << " ";
@@ -184,12 +154,11 @@ bool Tdcp(SnapshotPtr snapshot_r,
#endif
// FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
- Eigen::Matrix<double, Eigen::Dynamic, 4> A(Eigen::Matrix<double, Eigen::Dynamic, 4>::Zero(n_differences, 4));
- Eigen::VectorXd r(Eigen::VectorXd::Zero(n_differences));
- Eigen::VectorXd drho_m(Eigen::VectorXd::Zero(n_differences));
- Eigen::Matrix<double, 3, Eigen::Dynamic> s_k(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
- Eigen::Matrix<double, 3, Eigen::Dynamic> s_r(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
- int sat_i = 0;
+ Eigen::MatrixXd A(Eigen::MatrixXd::Zero(n_differences, 4));
+ Eigen::VectorXd r(Eigen::VectorXd::Zero(n_differences));
+ Eigen::VectorXd drho_m(Eigen::VectorXd::Zero(n_differences));
+ Eigen::MatrixXd s_k(Eigen::MatrixXd::Zero(3, n_differences));
+ Eigen::MatrixXd s_r(Eigen::MatrixXd::Zero(3, n_differences));
for (auto row_sat_freq_it : row_2_sat_freq)
{
@@ -197,39 +166,33 @@ 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 << "\tsat " << sat_number;
+ 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
if (!tdcp_params.relinearize_jacobian)
{
// Unit vectors from receivers to satellites
- Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
+ Eigen::Vector3d u_k = (s_k.col(row) - x_r - d_0.head(3)).normalized();
// Fill Jacobian matrix
A(row, 0) = u_k(0);
@@ -240,11 +203,58 @@ bool Tdcp(SnapshotPtr snapshot_r,
}
// LEAST SQUARES SOLUTION =======================================================================
+ std::set<int> raim_discarded_rows;
+ output = Tdcp(x_r, A, r, drho_m, s_k, s_r, d_0, raim_discarded_rows, tdcp_params);
+
+ // FILL OUTPUT
+ output.used_sats = common_sats;
+ for (auto disc_row : raim_discarded_rows)
+ {
+ output.raim_discarded_sats.insert(row_2_sat_freq[disc_row].first);
+ output.used_sats.erase(row_2_sat_freq[disc_row].first);
+ }
+ output.dt = tk - tr;
+
+ // weight covariance with the measurement noise (proportional to time)
+ double sq_sigma_Tdcp = (tk - tr) * tdcp_params.tdcp.sigma_atm * tdcp_params.tdcp.sigma_atm +
+ 2 * tdcp_params.tdcp.sigma_carrier * tdcp_params.tdcp.sigma_carrier;
+ output.cov_d *= sq_sigma_Tdcp;
+
+ return output;
+}
+
+TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
+ Eigen::MatrixXd& A,
+ Eigen::VectorXd& r,
+ Eigen::VectorXd& drho_m,
+ Eigen::MatrixXd& s_k,
+ Eigen::MatrixXd& s_r,
+ const Eigen::Vector4d& d_0,
+ std::set<int>& raim_discarded_rows,
+ const TdcpBatchParams& tdcp_params)
+{
+ assert(A.cols() == 4);
+ assert(s_k.rows() == 3);
+ assert(s_r.rows() == 3);
+
+ TdcpOutput output;
+ Eigen::Vector4d& d(output.d);
+ Eigen::Matrix4d& cov_d(output.cov_d);
+ double& residual(output.residual);
+
+ // Initial guess
+ Eigen::Vector4d delta_d(Eigen::Vector4d::Zero());
+ d = d_0;
+
for (int j = 0; j < tdcp_params.max_iterations; j++)
{
// fill A and r
for (int row = 0; row < A.rows(); row++)
{
+ // skip discarded rows
+ if (raim_discarded_rows.count(row) != 0)
+ continue;
+
// Evaluate r
r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
@@ -271,7 +281,22 @@ bool Tdcp(SnapshotPtr snapshot_r,
if (d.array().isNaN().any())
{
std::cout << "Tdcp: NLS DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
- return false;
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl;
+ std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
+ printf("Ref distance = %7.3f m\n", d_0.norm());
+ printf("Computed distance = %7.3f m\n", d.head<3>().norm());
+ printf("Tdcp: rows = %lu\n", r.rows());
+ std::cout << "Tdcp: r = " << r.transpose() << std::endl;
+ std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
+ std::cout << "Tdcp: A = \n" << A << std::endl;
+ std::cout << "Tdcp: H = \n" << A.transpose() * A << std::endl;
+ std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl;
+ printf("Tdcp: dT = %8.3f secs\n", d(3));
+ #endif
+ output.msg = "NaN values in NLS";
+ output.success = false;
+ return output;
}
// residual
@@ -286,7 +311,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
printf("Ref distance = %7.3f m\n", d_0.norm());
printf("Computed distance = %7.3f m\n", d.head<3>().norm());
printf("Tdcp: residual = %13.10f\n", residual);
- printf("Tdcp: row = %lu\n", r.rows());
+ printf("Tdcp: rows = %lu\n", r.rows());
std::cout << "Tdcp: r = " << r.transpose() << std::endl;
std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
std::cout << "Tdcp: A = \n" << A << std::endl;
@@ -301,10 +326,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
int worst_sat_row = -1;
double best_residual = 1e12;
Eigen::Vector4d best_d;
- int n_removed_rows = 1;
// remove some satellites
- while (raim_discarded_sats.size() < tdcp_params.raim_n)
+ while (raim_discarded_rows.size() < tdcp_params.raim_n)
{
auto A_raim = A;
auto r_raim = r;
@@ -312,21 +336,13 @@ bool Tdcp(SnapshotPtr snapshot_r,
// solve removing each satellite
for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
{
- int sat_removed = row_2_sat_freq.at(row_removed).first;
-
- // Multi-freq: some rows for the same satellite
- n_removed_rows = 1;
- if (tdcp_params.tdcp.multi_freq)
- while (row_removed + n_removed_rows < A_raim.rows() and
- row_2_sat_freq.at(row_removed + n_removed_rows).first == sat_removed)
- n_removed_rows++;
+ // skip already discarded rows
+ if (raim_discarded_rows.count(row_removed) != 0)
+ continue;
- // remove satellite rows
- for (auto r = 0; r < n_removed_rows; r++)
- {
- A_raim.row(row_removed + r) = Eigen::Vector4d::Zero().transpose();
- r_raim(row_removed + r) = 0; // not necessary
- }
+ // remove satellite row
+ A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose();
+ r_raim(row_removed) = 0; // not necessary
// solve
Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
@@ -340,17 +356,15 @@ bool Tdcp(SnapshotPtr snapshot_r,
// residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
#if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "RAIM excluding sat " << sat_removed << " - row " << row_removed << "(n_rows "
- << n_removed_rows << ")" << std::endl;
+ std::cout << "RAIM excluding row " << row_removed << std::endl;
std::cout << "Displacement vector =" << d_raim.head<3>().transpose() << std::endl;
std::cout << "Displacement update =" << delta_d_raim.head<3>().transpose() << std::endl;
printf("Ref distance = %7.3f m\n", d_0.norm());
printf("Computed distance = %7.3f m\n", d_raim.head<3>().norm());
printf("Tdcp: residual = %13.10f\n", residual);
std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl;
- std::cout << "Tdcp: A = \n" << A << std::endl;
- std::cout << "Tdcp: H = \n" << A.transpose() * A << std::endl;
- std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl;
+ std::cout << "Tdcp: A = \n" << A_raim << std::endl;
+ std::cout << "Tdcp: H = \n" << A_raim.transpose() * A_raim << std::endl;
printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
#endif
@@ -363,29 +377,27 @@ bool Tdcp(SnapshotPtr snapshot_r,
}
}
// restore initial A and r
- for (auto row = 0; row < n_removed_rows; row++)
- {
- A_raim.row(row_removed + row) = A.row(row_removed + row);
- r_raim(row_removed + row) = r(row_removed + row);
- }
- row_removed += (n_removed_rows - 1);
+ A_raim.row(row_removed) = A.row(row_removed);
+ r_raim(row_removed) = r(row_removed);
}
// No successful RAIM solution
if (worst_sat_row == -1)
{
printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
- return false;
+ output.msg = "NaN values in NLS after RAIM";
+ output.success = false;
+ return output;
}
// store removed sat
- raim_discarded_sats.insert(row_2_sat_freq[worst_sat_row].first);
+ raim_discarded_rows.insert(worst_sat_row);
- // decrease n_common_sats
- n_differences -= n_removed_rows;
- n_common_sats--;
+ // remove row
+ A.row(worst_sat_row) = Eigen::Vector4d::Zero().transpose();
+ r(worst_sat_row) = 0; // not necessary
- // Remove selected satellite from problem
+ /*// Remove selected satellite from problem
std::cout << "resizing problem..." << std::endl;
auto A_aux = A;
auto r_aux = r;
@@ -406,7 +418,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
s_k.rightCols(back_elements_changing) = s_k_aux.rightCols(back_elements_changing);
r.tail(back_elements_changing) = r_aux.tail(back_elements_changing);
drho_m.tail(back_elements_changing) = drho_m_aux.tail(back_elements_changing);
- }
+ }*/
// apply the RAIM solution
d = best_d;
@@ -415,9 +427,10 @@ bool Tdcp(SnapshotPtr snapshot_r,
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Tdcp After RAIM " << std::endl;
- std::cout << "\tExcluded sats : ";
- std::cout << "\tCommon sats : " << n_common_sats << std::endl;
- std::cout << "\tRows : " << n_differences << std::endl;
+ std::cout << "\tExcluded rows : ";
+ for (auto excl_row : raim_discarded_rows)
+ std::cout << excl_row << " ";
+ std::cout << std::endl;
std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
std::cout << "\tClock offset = " << d(3) << std::endl;
@@ -430,9 +443,10 @@ bool Tdcp(SnapshotPtr snapshot_r,
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Tdcp iteration " << j << std::endl;
- std::cout << "\tExcluded sats : ";
- std::cout << "\tCommon sats : " << common_sats.size() << std::endl;
- std::cout << "\tRows : " << n_differences << std::endl;
+ std::cout << "\tExcluded rows : ";
+ for (auto excl_row : raim_discarded_rows)
+ std::cout << excl_row << " ";
+ std::cout << std::endl;
std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
std::cout << "\tClock offset = " << d(3) << std::endl;
@@ -443,19 +457,19 @@ bool Tdcp(SnapshotPtr snapshot_r,
#endif
}
- // weight covariance with the measurement noise (proportional to time)
- double sq_sigma_Tdcp = (tk - tr) * tdcp_params.tdcp.sigma_atm * tdcp_params.tdcp.sigma_atm +
- 2 * tdcp_params.tdcp.sigma_carrier * tdcp_params.tdcp.sigma_carrier;
- cov_d *= sq_sigma_Tdcp;
- // residual = (r - A * d).norm() / A.rows();
-
// Computing error on measurement space
- for (int row = 0; row < n_differences; row++)
- r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
+ r.setZero();
+ for (int row = 0; row < r.size(); row++)
+ if (raim_discarded_rows.count(row) == 0)
+ r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
- residual = sqrt(r.squaredNorm() / r.size());
+ // residual = (r - A * d).norm() / A.rows();
+ residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
+
+ output.success = true;
- return true;
+ return output;
}
+
} // namespace GnssUtils
diff --git a/src/utils/satellite.cpp b/src/utils/satellite.cpp
index 242f4aded99a222d654c93776510a0cdb5926c93..22e694828c40ace1d618a14f6b93e21415d70c01 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/CMakeLists.txt b/test/CMakeLists.txt
index e7ef282189d9297ca55770cbad6e797b36cad125..4b38d2e10f58811e98ba3d282072327530cc0892 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
new file mode 100644
index 0000000000000000000000000000000000000000..1338b7e09879b6d6ae93c551a52f6165e34f0112
--- /dev/null
+++ b/test/gtest_tdcp.cpp
@@ -0,0 +1,330 @@
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/tdcp.h"
+#include "gnss_utils/snapshot.h"
+#include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/utils/transformations.h"
+
+using namespace GnssUtils;
+using namespace Eigen;
+
+Vector3d computeRandomReceiverLatLonAlt()
+{
+ 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])
+ receiver_LLA(2) *= 5e2; // in [0, 1e3]
+
+ return receiver_LLA;
+}
+
+void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
+ Vector3d& sat_ENU,
+ Vector3d& sat_ECEF,
+ Vector2d& sat_azel,
+ double& range)
+{
+ 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 = 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 = 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;
+
+ // Range
+ range = (sat_ECEF - t_ECEF_ENU).norm();
+}
+
+TEST(Tdcp, Tdcp)
+{
+ int N_sats = 20;
+
+ TdcpBatchParams tdcp_params;
+ tdcp_params.max_iterations = 10;
+ tdcp_params.min_common_sats = 6;
+ tdcp_params.raim_n = 0;
+ tdcp_params.raim_min_residual = 0;
+ tdcp_params.relinearize_jacobian = true;
+ tdcp_params.tdcp.multi_freq = false;
+
+ Vector3d sat_ENU, sat_ECEF;
+ Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
+ Matrix3d R_ENU_ECEF;
+ Vector2d azel, azel2;
+ Vector4d d_gt;
+ Matrix4d cov_d;
+ double range, residual;
+
+ // Snapshots
+ auto snapshot_r = std::make_shared<Snapshot>();
+ auto snapshot_k = std::make_shared<Snapshot>();
+
+ // Observations (just needed for the GPST)
+ snapshot_r->setObservations(std::make_shared<Observations>());
+ snapshot_k->setObservations(std::make_shared<Observations>());
+ obsd_t obs_r;
+ obs_r.time.sec = 0;
+ obs_r.time.time = 0;
+ snapshot_r->getObservations()->addObservation(obs_r);
+ obsd_t obs_k;
+ obs_k.time.sec = 0;
+ obs_k.time.time = 1;
+ snapshot_k->getObservations()->addObservation(obs_k);
+
+ // Random receiver position
+ for (auto i = 0; i<100; i++)
+ {
+ // clear satellites and ranges
+ snapshot_r->getSatellites().clear();
+ snapshot_k->getSatellites().clear();
+ snapshot_r->getRanges().clear();
+ snapshot_k->getRanges().clear();
+
+ // random setup
+ 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);
+
+ double clock_r = Vector2d::Random()(0) * 1e-6;
+ double clock_k = Vector2d::Random()(0) * 1e-6;
+
+ // groundtruth
+ d_gt.head<3>() = d_ECEF;
+ d_gt(3) = (clock_k - clock_r) * CLIGHT;
+
+ std::cout << "Iteration " << i << ":\n";
+ std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
+ std::cout << "\tclock_r " << clock_r << ":\n";
+ std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
+ std::cout << "\tclock_k " << clock_k << ":\n";
+ std::cout << "\td_gt " << d_gt.transpose() << ":\n";
+
+ std::set<int> common_sats;
+ std::set<int> raim_discarded_sats;
+
+ // random visible satellites
+ for (auto j = 0; j<N_sats; j++)
+ {
+ common_sats.insert(j);
+
+ // Satellite r (random)
+ computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range);
+ ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
+
+ Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+ snapshot_r->getSatellites().emplace(j, sat_r);
+
+ // Range r
+ Range range_r;
+ range_r.sat = j;
+ range_r.L_corrected = range + CLIGHT * clock_r;
+ range_r.L_var = 1;
+ range_r.L2_corrected = range_r.L_corrected;
+ range_r.L2_var = range_r.L_var;
+
+ snapshot_r->getRanges().emplace(j, range_r);
+
+ std::cout << "\tsat: " << j << "\n";
+ std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
+ std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
+
+ // Satellite k (random)
+ computeRandomVisibleSatellite(x_k_LLA, sat_ENU, sat_ECEF, azel, range);
+ ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
+
+ Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+ snapshot_k->getSatellites().emplace(j, sat_k);
+
+ // Range k
+ Range range_k;
+ range_k.sat = j;
+ range_k.L_corrected = range + CLIGHT * clock_k;
+ range_k.L_var = 1;
+ range_k.L2_corrected = range_k.L_corrected;
+ range_k.L2_var = range_k.L_var;
+
+ snapshot_k->getRanges().emplace(j, range_k);
+
+ std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
+ std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
+ }
+
+ // TDCP
+ auto tdcp_out = Tdcp(snapshot_r,
+ snapshot_k,
+ x_r_ECEF,
+ common_sats,
+ Vector4d::Zero(),
+ tdcp_params);
+
+ // CHECKS
+ std::cout << "CHECKS iteration " << i << std::endl;
+ ASSERT_TRUE(tdcp_out.success);
+ ASSERT_LE(tdcp_out.residual, 1e-9);
+ ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6);
+ }
+}
+
+TEST(Tdcp, Tdcp_raim)
+{
+ int N_sats = 20;
+
+ TdcpBatchParams tdcp_params;
+ tdcp_params.max_iterations = 10;
+ tdcp_params.min_common_sats = 6;
+ tdcp_params.raim_n = 2;
+ tdcp_params.raim_min_residual = 0;
+ tdcp_params.relinearize_jacobian = true;
+ tdcp_params.tdcp.multi_freq = false;
+
+ Vector3d sat_ENU, sat_ECEF;
+ Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
+ Matrix3d R_ENU_ECEF;
+ Vector2d azel, azel2;
+ Vector4d d, d_gt;
+ Matrix4d cov_d;
+ double range, residual;
+
+ // Snapshots
+ auto snapshot_r = std::make_shared<Snapshot>();
+ auto snapshot_k = std::make_shared<Snapshot>();
+
+ // Observations (just needed for the GPST)
+ snapshot_r->setObservations(std::make_shared<Observations>());
+ snapshot_k->setObservations(std::make_shared<Observations>());
+ obsd_t obs_r;
+ obs_r.time.sec = 0;
+ obs_r.time.time = 0;
+ snapshot_r->getObservations()->addObservation(obs_r);
+ obsd_t obs_k;
+ obs_k.time.sec = 0;
+ obs_k.time.time = 1;
+ snapshot_k->getObservations()->addObservation(obs_k);
+
+ // Random receiver position
+ for (auto i = 0; i<100; i++)
+ {
+ // clear satellites and ranges
+ snapshot_r->getSatellites().clear();
+ snapshot_k->getSatellites().clear();
+ snapshot_r->getRanges().clear();
+ snapshot_k->getRanges().clear();
+
+ // random setup
+ 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);
+
+ double clock_r = Vector2d::Random()(0) * 1e-6;
+ double clock_k = Vector2d::Random()(0) * 1e-6;
+
+ // groundtruth
+ d_gt.head<3>() = d_ECEF;
+ d_gt(3) = (clock_k - clock_r) * CLIGHT;
+
+ std::cout << "Iteration " << i << ":\n";
+ std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
+ std::cout << "\tclock_r " << clock_r << ":\n";
+ std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
+ std::cout << "\tclock_k " << clock_k << ":\n";
+ std::cout << "\td_gt " << d_gt.transpose() << ":\n";
+
+ std::set<int> common_sats;
+ std::set<int> raim_discarded_sats;
+
+ // random visible satellites
+ for (auto j = 0; j<N_sats; j++)
+ {
+ common_sats.insert(j);
+
+ // Satellite r (random)
+ computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range);
+ ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
+
+ Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+ snapshot_r->getSatellites().emplace(j, sat_r);
+
+ // Range r
+ Range range_r;
+ range_r.sat = j;
+ range_r.L_corrected = range + CLIGHT * clock_r;
+ range_r.L_var = 1;
+ range_r.L2_corrected = range_r.L_corrected;
+ range_r.L2_var = range_r.L_var;
+
+ snapshot_r->getRanges().emplace(j, range_r);
+
+ std::cout << "\tsat: " << j << "\n";
+ std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
+ std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
+
+ // Satellite k (random)
+ computeRandomVisibleSatellite(x_k_LLA, sat_ENU, sat_ECEF, azel, range);
+ ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
+
+ Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
+ snapshot_k->getSatellites().emplace(j, sat_k);
+
+ // Range k
+ Range range_k;
+ range_k.sat = j;
+ range_k.L_corrected = range + CLIGHT * clock_k;
+ range_k.L_var = 1;
+ range_k.L2_corrected = range_k.L_corrected;
+ range_k.L2_var = range_k.L_var;
+
+ snapshot_k->getRanges().emplace(j, range_k);
+
+ std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
+ std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
+ }
+
+ // Distort 2 ranges -> to be detected by RAIM
+ int wrong_sat1 = i % N_sats;
+ snapshot_k->getRanges().at(wrong_sat1).L_corrected += 20;
+ int wrong_sat2 = (i + N_sats / 2) % N_sats;
+ snapshot_r->getRanges().at(wrong_sat2).L_corrected -= 10;
+
+
+ // TDCP
+ auto tdcp_out = Tdcp(snapshot_r,
+ snapshot_k,
+ x_r_ECEF,
+ common_sats,
+ Vector4d::Zero(),
+ tdcp_params);
+
+ // CHECKS
+ std::cout << "CHECKS iteration " << i << std::endl;
+ ASSERT_EQ(tdcp_out.raim_discarded_sats.size(), 2);
+ ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat1));
+ ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat2));
+ ASSERT_TRUE(tdcp_out.success);
+ ASSERT_LE(tdcp_out.residual, 1e-9);
+ ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 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);
}
}
}