diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3c6cd3802f06a76c8dc5f9aff1cea5d460897a66..b3beddcb57454472c4f278701a5db658da2e4ae9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -80,9 +80,10 @@ SET(SOURCES
src/utils/utils.cpp
src/utils/transformations.cpp
src/utils/rcv_position.cpp
- src/utils/sat_position.cpp
+ src/utils/satellite.cpp
src/observations.cpp
src/navigation.cpp
+ src/snapshot.cpp
src/tdcp.cpp
src/ublox_raw.cpp)
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index 4d9f83f2bd6ab7a7a70fa1b63e8c3d577be4011b..0c32ca8f6361c8dfba86dced6134eff2fb49e6a9 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -5,4 +5,38 @@ extern "C" {
#include "rtklib.h"
}
-#endif
\ No newline at end of file
+// std includes
+#include <set>
+#include <map>
+#include <vector>
+#include <iostream>
+#include <memory>
+#include <cassert>
+// eigen
+#include <eigen3/Eigen/Dense>
+
+namespace GnssUtils
+{
+
+// forward declarations
+class Observations;
+class Navigation;
+class Snapshot;
+
+// Typedefs
+typedef std::map<int,Eigen::Vector3d> SatellitesPositions;
+
+// pointer typedefs
+typedef std::shared_ptr<Observations> ObservationsPtr;
+typedef std::shared_ptr<const Observations> ObservationsConstPtr;
+
+typedef std::shared_ptr<Navigation> NavigationPtr;
+typedef std::shared_ptr<const Navigation> NavigationConstPtr;
+
+typedef std::shared_ptr<Snapshot> SnapshotPtr;
+typedef std::shared_ptr<const Snapshot> SnapshotConstPtr;
+
+
+}
+
+#endif
diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h
index ccd1ac812fcd829e5ffcca14021d0fc15c65e595..f47a7bc0617c1250f2a99233c83d67e58f81f048 100644
--- a/include/gnss_utils/navigation.h
+++ b/include/gnss_utils/navigation.h
@@ -1,18 +1,11 @@
#ifndef INCLUDE_GNSS_UTILS_NAVIGATION_H_
#define INCLUDE_GNSS_UTILS_NAVIGATION_H_
-#include <vector>
-#include <iostream>
-#include <memory>
-
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/utils.h"
namespace GnssUtils
{
-class Navigation;
-typedef std::shared_ptr<Navigation> NavigationPtr;
-typedef std::shared_ptr<const Navigation> NavigationConstPtr;
class Navigation
{
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index 7e6328607715cb44088428bfb9bc0a3e4f9625f2..cd6bea585de5766dc43bf2f912e2000f635efc84 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -1,20 +1,10 @@
#ifndef INCLUDE_GNSS_UTILS_OBSERVATIONS_H_
#define INCLUDE_GNSS_UTILS_OBSERVATIONS_H_
-#include <vector>
-#include <map>
-#include <iostream>
-#include <memory>
-#include <cassert>
-
#include "gnss_utils/gnss_utils.h"
-#include "gnss_utils/utils/utils.h"
namespace GnssUtils
{
-class Observations;
-typedef std::shared_ptr<Observations> ObservationsPtr;
-typedef std::shared_ptr<const Observations> ObservationsConstPtr;
class Observations
{
@@ -54,10 +44,25 @@ public:
void printByIdx(const int& _idx);
void print();
- static void findCommonObservations(const Observations& obs_1,
- const Observations& obs_2,
- Observations& common_obs_1,
- Observations& common_obs_2);
+ // Filter observations
+ std::set<int> filterByEphemeris(const SatellitesPositions& sats_pos);
+ std::set<int> filterBySatellites(const std::set<int>& discarded_sats);
+ 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 SatellitesPositions& sats_pos,
+ const snrmask_t& snrmask,
+ const double& elmin);
+ std::set<int> filter(const SatellitesPositions& sats_pos,
+ const std::set<int>& discarded_sats,
+ const Eigen::Vector3d& x_r,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const prcopt_t& opt);
+
+ static std::set<int> findCommonObservations(const Observations& obs_1,
+ const Observations& obs_2);
bool operator==(const Observations& other_obs) const;
bool operator !=(const Observations &other_obs) const;
@@ -71,6 +76,15 @@ private:
// Private methods
};
+} // namespace GnssUtils
+
+// IMPLEMENTATION
+#include "gnss_utils/utils/utils.h"
+#include "gnss_utils/utils/satellite.h"
+
+namespace GnssUtils
+{
+
inline const std::vector<obsd_t>& Observations::getObservations() const
{
return this->obs_;
diff --git a/include/gnss_utils/snapshot.h b/include/gnss_utils/snapshot.h
new file mode 100644
index 0000000000000000000000000000000000000000..6d8f1dc36afd69cfb5a8af2f1c2312427bc3c634
--- /dev/null
+++ b/include/gnss_utils/snapshot.h
@@ -0,0 +1,114 @@
+#ifndef INCLUDE_GNSS_UTILS_SNAPSHOT_H_
+#define INCLUDE_GNSS_UTILS_SNAPSHOT_H_
+
+#include <map>
+#include <set>
+#include <iostream>
+#include <memory>
+#include <cassert>
+
+#include <eigen3/Eigen/Dense>
+
+#include "gnss_utils/gnss_utils.h"
+
+namespace GnssUtils
+{
+
+class Snapshot
+{
+
+public:
+ // Constructor & Destructor
+ Snapshot();
+ Snapshot(ObservationsPtr obs, NavigationPtr nav);
+ ~Snapshot(){};
+
+ // Public objects
+
+ // Public methods
+ void loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
+
+ ObservationsPtr getObservations() const;
+ NavigationPtr getNavigation() const;
+ const SatellitesPositions& getSatellitesPositions() const;
+ SatellitesPositions& getSatellitesPositions();
+ void setObservations(ObservationsPtr obs);
+ void setNavigation(NavigationPtr nav);
+
+ void computeSatellitesPositions(const int& eph_opt); // see rtklib.h L396);
+ bool satellitesPositionsComputed() const;
+
+ std::set<int> filterObservations(std::set<int> &discarded_sats,
+ const Eigen::Vector3d &x_r,
+ const bool &check_code,
+ const bool &check_carrier_phase,
+ const prcopt_t &opt);
+
+ void print();
+
+private:
+ // Private objects
+ SatellitesPositions sats_pos_; //< key: sat number
+ ObservationsPtr obs_;
+ NavigationPtr nav_;
+
+ // Private methods
+};
+
+} // namespace GnssUtils
+
+// IMPLEMENTATION
+#include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/utils.h"
+
+namespace GnssUtils
+{
+
+inline GnssUtils::ObservationsPtr Snapshot::getObservations() const
+{
+ return obs_;
+}
+
+inline GnssUtils::NavigationPtr Snapshot::getNavigation() const
+{
+ return nav_;
+}
+
+inline SatellitesPositions& Snapshot::getSatellitesPositions()
+{
+ return sats_pos_;
+}
+
+inline const SatellitesPositions& Snapshot::getSatellitesPositions() const
+{
+ return sats_pos_;
+}
+
+inline void Snapshot::setObservations(ObservationsPtr obs)
+{
+ obs_ = obs;
+}
+
+inline void Snapshot::setNavigation(NavigationPtr nav)
+{
+ nav_ = nav;
+}
+
+inline bool Snapshot::satellitesPositionsComputed() const
+{
+ return !sats_pos_.empty();
+}
+
+inline std::set<int> Snapshot::filterObservations(std::set<int> &discarded_sats,
+ const Eigen::Vector3d &x_r,
+ const bool &check_code,
+ const bool &check_carrier_phase,
+ const prcopt_t &opt)
+{
+ return obs_->filter(sats_pos_, discarded_sats, x_r, check_code, check_carrier_phase, opt);
+}
+
+} // namespace GnssUtils
+#endif // INCLUDE_GNSS_UTILS_SNAPSHOT_H_
diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 6d64e885bec90541f3fd4f3cc29fcc605576f277..45504e0dcd1c78d2ed34d0f56224eeb5427cb859 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -1,20 +1,9 @@
-/*
- * tdcp.h
- *
- * Created on: Dec 4, 2019
- * Author: jvallve
- */
-
#ifndef INCLUDE_GNSS_UTILS_TDCP_H_
#define INCLUDE_GNSS_UTILS_TDCP_H_
-#define GNSS_UTILS_TDCP_DEBUG 0
+#define GNSS_UTILS_TDCP_DEBUG 1
-#include <set>
-#include "gnss_utils/utils/rcv_position.h"
-#include "gnss_utils/utils/sat_position.h"
-#include "gnss_utils/observations.h"
-#include "gnss_utils/navigation.h"
+#include "gnss_utils/gnss_utils.h"
namespace GnssUtils
{
@@ -36,51 +25,36 @@ struct TdcpParams
double time_window;
};
-bool Tdcp(const Observations& obs_r,
- Navigation& nav_r,
- const Observations& obs_k,
- const Navigation& nav_k,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- std::set<int>& discarded_sats,
- const TdcpParams& sd_opt,
- const prcopt_t& opt);
-
-bool Tdcp(const Observations& obs_r,
- const Navigation& nav_r,
+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 TdcpParams& sd_opt,
+ const prcopt_t& opt);
+
+bool Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
- const Observations& obs_k,
- const Navigation& nav_k,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
- std::set<int>& discarded_sats,
- const TdcpParams& sd_opt,
+ const std::set<int>& discarded_sats,
+ std::set<int>& raim_discarded_sats,
+ const TdcpParams& sd_params,
const prcopt_t& opt);
-bool Tdcp(const Observations& common_obs_r,
- const Navigation& nav_r,
- const std::map<int, Eigen::Vector3d>& common_sats_pos_r,
- const Eigen::Vector3d& x_r,
- const Observations& common_obs_k,
- const Navigation& nav_k,
- const std::map<int, Eigen::Vector3d>& common_sats_pos_k,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- std::set<int>& discarded_sats,
- const TdcpParams& sd_opt);
-
-void filterCommonSatellites(Observations& common_obs_r,
- std::map<int, Eigen::Vector3d>& common_sats_pos_r,
- Observations& common_obs_k,
- std::map<int, Eigen::Vector3d>& common_sats_pos_k,
- std::set<int>& discarded_sats,
- const Eigen::Vector3d& x_r,
- const TdcpParams& sd_params,
- const prcopt_t& 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 TdcpParams& sd_params);
} // namespace GnssUtils
#endif /* INCLUDE_GNSS_UTILS_Tdcp_H_ */
diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h
index ecee80c79c836464b482ec686b264e7c4d87235c..f03bb73188d98b53f011b72e0cd1dd6932e76b70 100644
--- a/include/gnss_utils/ublox_raw.h
+++ b/include/gnss_utils/ublox_raw.h
@@ -44,12 +44,12 @@ private:
void updateObservations();
};
-inline const GnssUtils::Observations& UBloxRaw::getObservations()
+inline const Observations& UBloxRaw::getObservations()
{
return obs_;
}
-inline const GnssUtils::Navigation& UBloxRaw::getNavigation()
+inline const Navigation& UBloxRaw::getNavigation()
{
return nav_;
}
diff --git a/include/gnss_utils/utils/sat_position.h b/include/gnss_utils/utils/sat_position.h
deleted file mode 100644
index 3e90436c2caea89e3e77fd57e99acbe8ce727813..0000000000000000000000000000000000000000
--- a/include/gnss_utils/utils/sat_position.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
- * sat_position.h
- *
- * Created on: April 3, 2020
- * Author: Joan Vallvé, Pep MartÃ-Saumell
- */
-
-#ifndef INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
-#define INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
-
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-#include <eigen3/Eigen/Sparse>
-
-#include "gnss_utils/observations.h"
-#include "gnss_utils/navigation.h"
-#include "gnss_utils/utils/transformations.h"
-#include "gnss_utils/gnss_utils.h"
-
-namespace GnssUtils
-{
-double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
-
-void computeSatellitesPositions(const Observations& obs,
- const Navigation& nav,
- const prcopt_t& opt,
- std::map<int, Eigen::Vector3d>& sats_pos);
-} // namespace GnssUtils
-#endif // INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
\ No newline at end of file
diff --git a/include/gnss_utils/utils/satellite.h b/include/gnss_utils/utils/satellite.h
new file mode 100644
index 0000000000000000000000000000000000000000..8ca85f3aaf937a36146e3d8dd02cfe65b6a641b6
--- /dev/null
+++ b/include/gnss_utils/utils/satellite.h
@@ -0,0 +1,34 @@
+/*
+ * sat_position.h
+ *
+ * Created on: April 3, 2020
+ * Author: Joan Vallvé, Pep MartÃ-Saumell
+ */
+
+#ifndef INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
+#define INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
+
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+
+#include <set>
+#include <map>
+
+
+namespace GnssUtils
+{
+ // forward declarations
+ class Observations;
+ class Navigation;
+
+ typedef std::map<int,Eigen::Vector3d> SatellitesPositions;
+
+ double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
+
+ void computeSatellitesPositions(const Observations& obs,
+ const Navigation& nav,
+ const int& eph_opt, // see rtklib.h L396
+ std::map<int, Eigen::Vector3d>& sats_pos);
+
+} // namespace GnssUtils
+#endif // INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
diff --git a/include/gnss_utils/utils/transformations.h b/include/gnss_utils/utils/transformations.h
index caa22c473af8509cda43970bf7f98cdef05ed99f..10ddc0ac8462d9bd8636dc9a42d273cfcaed9a81 100644
--- a/include/gnss_utils/utils/transformations.h
+++ b/include/gnss_utils/utils/transformations.h
@@ -17,8 +17,8 @@ namespace GnssUtils
{
Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef);
Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon);
-Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef);
-Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu);
+Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d& _cov_ecef);
+Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d& _cov_enu);
void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
Eigen::Matrix3d& R_ENU_ECEF,
@@ -28,4 +28,4 @@ void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
Eigen::Vector3d& t_ENU_ECEF);
} // namespace GnssUtils
-#endif // INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
\ No newline at end of file
+#endif // INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
diff --git a/src/observations.cpp b/src/observations.cpp
index f9d1e9393c8e0e34bbaa7ae53824c299676ef233..956c13f5b8b07b954f6e34a6ab0e84a3e158e63e 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -1,4 +1,5 @@
#include "gnss_utils/observations.h"
+#include "gnss_utils/utils/satellite.h"
using namespace GnssUtils;
@@ -191,15 +192,216 @@ void Observations::print()
}
}
-void Observations::findCommonObservations(const Observations& obs_1,
- const Observations& obs_2,
- Observations& common_obs_1,
- Observations& common_obs_2)
+std::set<int> Observations::filterByEphemeris(const SatellitesPositions& sats_pos)
{
- // clear and reserve
- common_obs_1.clearObservations();
- common_obs_2.clearObservations();
+ std::set<int> remove_sats;
+ for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ {
+ auto&& obs_sat = getObservationByIdx(obs_i);
+ const int& sat_number = obs_sat.sat;
+
+ // bad or inexistent satellite position (satellite is not included in the discarded list)
+ if (sats_pos.count(sat_number) == 0 or
+ sats_pos.at(sat_number).isApprox(Eigen::Vector3d::Zero(), 1e-3) or
+ sats_pos.at(sat_number).isApprox(Eigen::Vector3d::Zero(), 1e-3))
+ {
+ std::cout << "Discarding sat " << sat_number << ": wrong satellite position: \n\t" << sats_pos.at(sat_number).transpose() << std::endl;
+ remove_sats.insert(sat_number);
+ }
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(hasSatellite(sat));
+ removeObservationBySat(sat);
+ }
+
+ return remove_sats;
+}
+
+std::set<int> Observations::filterBySatellites(const std::set<int>& discarded_sats)
+{
+ std::set<int> remove_sats;
+
+ for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ {
+ auto&& obs_sat = getObservationByIdx(obs_i);
+ const int& sat_number = obs_sat.sat;
+
+ // discard sats
+ if (discarded_sats.count(sat_number) != 0)
+ remove_sats.insert(sat_number);
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(hasSatellite(sat));
+ removeObservationBySat(sat);
+ }
+
+ return remove_sats;
+}
+
+std::set<int> Observations::filterByCode()
+{
+ std::set<int> remove_sats;
+
+ for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ {
+ auto&& obs_sat = getObservationByIdx(obs_i);
+ const int& sat_number = obs_sat.sat;
+
+ // wrong data
+ if (std::abs(obs_sat.P[0]) < 1e-12)
+ {
+ std::cout << "Discarding sat " << sat_number << ": wrong code data: " << obs_sat.P[0] << std::endl;
+ remove_sats.insert(sat_number);
+ }
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(hasSatellite(sat));
+ removeObservationBySat(sat);
+ }
+
+ return remove_sats;
+}
+
+std::set<int> Observations::filterByCarrierPhase()
+{
+ std::set<int> remove_sats;
+
+ for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ {
+ auto&& obs_sat = getObservationByIdx(obs_i);
+ const int& sat_number = obs_sat.sat;
+
+ // wrong data
+ if (std::abs(obs_sat.L[0]) < 1e-12)
+ {
+ std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_sat.L[0] << std::endl;
+ remove_sats.insert(sat_number);
+ }
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(hasSatellite(sat));
+ removeObservationBySat(sat);
+ }
+
+ return remove_sats;
+}
+
+std::set<int> Observations::filterByConstellations(const int& navsys)
+{
+ std::set<int> remove_sats;
+
+ for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ {
+ auto&& obs_sat = getObservationByIdx(obs_i);
+ const int& sat_number = obs_sat.sat;
+
+ // constellation
+ int sys = satsys(obs_sat.sat, NULL);
+ if (!(sys & navsys))
+ {
+ std::cout << "Discarding sat " << sat_number << ": not selected constellation " << sys << " - mask: " << navsys << std::endl;
+ remove_sats.insert(sat_number);
+ continue;
+ }
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(hasSatellite(sat));
+ removeObservationBySat(sat);
+ }
+
+ return remove_sats;
+}
+
+std::set<int> Observations::filterByElevationSnr(const Eigen::Vector3d& x_r,
+ const SatellitesPositions& sats_pos,
+ const snrmask_t& snrmask,
+ const double& elmin)
+{
+ std::set<int> remove_sats;
+
+ for (int obs_i = 0; obs_i < obs_.size(); obs_i++)
+ {
+ auto&& obs_sat = getObservationByIdx(obs_i);
+ const int& sat_number = obs_sat.sat;
+
+ // check elevation
+ double elevation = computeSatElevation(x_r, sats_pos.at(sat_number));
+ if (elevation < elmin)
+ {
+ std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << elmin << std::endl;
+ remove_sats.insert(sat_number);
+ continue;
+ }
+
+ // snr TODO: multifrequency (2nd param and 3rd idx)
+ if (testsnr(0, 0, elevation, obs_sat.SNR[0] * 0.25, &snrmask) == 1)
+ {
+ std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl;
+ remove_sats.insert(sat_number);
+ }
+ }
+
+ // remove sats
+ // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
+ for (auto sat : remove_sats)
+ {
+ assert(hasSatellite(sat));
+ removeObservationBySat(sat);
+ }
+
+ return remove_sats;
+}
+
+std::set<int> Observations::filter(const SatellitesPositions& sats_pos,
+ const std::set<int>& discarded_sats,
+ const Eigen::Vector3d& x_r,
+ const bool& check_code,
+ const bool& check_carrier_phase,
+ const prcopt_t& opt)
+{
+ //std::cout << "filter: initial size: " << obs_.size() << std::endl;
+
+ std::set<int> remove_sats_1 = filterByEphemeris(sats_pos);
+ std::set<int> remove_sats_2 = filterBySatellites(discarded_sats);
+ std::set<int> remove_sats_3 = filterByCode();
+ std::set<int> remove_sats_4 = filterByCarrierPhase();
+ std::set<int> remove_sats_5 = filterByConstellations(opt.navsys);
+ std::set<int> remove_sats_6 = filterByElevationSnr(x_r, sats_pos, opt.snrmask, opt.elmin);
+
+ remove_sats_1.insert(remove_sats_2.begin(), remove_sats_2.end());
+ remove_sats_1.insert(remove_sats_3.begin(), remove_sats_3.end());
+ remove_sats_1.insert(remove_sats_4.begin(), remove_sats_4.end());
+ remove_sats_1.insert(remove_sats_5.begin(), remove_sats_5.end());
+ remove_sats_1.insert(remove_sats_6.begin(), remove_sats_6.end());
+
+ return remove_sats_1;
+ // std::cout << "final size: " << obs_.size() << std::endl;
+}
+
+std::set<int> Observations::findCommonObservations(const Observations& obs_1,
+ const Observations& obs_2)
+{
// std::cout << "obs 1 sats: ";
// for (auto&& obs_1_ref : obs_1.getObservations())
// std::cout << (int)obs_1_ref.sat << " ";
@@ -209,20 +411,22 @@ void Observations::findCommonObservations(const Observations& obs_1,
// std::cout << (int)obs_2_ref.sat << " ";
// std::cout << std::endl;
+ std::set<int> common_sats;
+
// iterate 1
for (auto&& obs_1_ref : obs_1.getObservations())
if (obs_2.hasSatellite(obs_1_ref.sat))
- {
- common_obs_1.addObservation(obs_1_ref);
- common_obs_2.addObservation(obs_2.getObservationBySat(obs_1_ref.sat));
- }
+ common_sats.insert(obs_1_ref.sat);
// std::cout << "common sats: ";
- // for (auto&& obs_2_ref : common_obs_1.getObservations())
- // std::cout << (int)obs_2_ref.sat << " ";
+ // for (auto sat : common_sats)
+ // std::cout << sat << " ";
// std::cout << std::endl;
+
+ return common_sats;
}
+
bool Observations::operator ==(const Observations &other_obs) const
{
diff --git a/src/snapshot.cpp b/src/snapshot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..87a08ede1ce51e16b1dd3809aeb2039d553d9c23
--- /dev/null
+++ b/src/snapshot.cpp
@@ -0,0 +1,32 @@
+#include "gnss_utils/snapshot.h"
+#include "gnss_utils/utils/satellite.h"
+
+using namespace GnssUtils;
+
+Snapshot::Snapshot()
+ : obs_(nullptr)
+ , nav_(nullptr)
+{
+ //
+}
+
+Snapshot::Snapshot(ObservationsPtr obs, NavigationPtr nav)
+ : obs_(obs)
+ , nav_(nav)
+{
+ //
+}
+
+void Snapshot::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt)
+{
+ throw std::runtime_error("not implemented!");
+}
+
+void Snapshot::computeSatellitesPositions(const int& eph_opt)
+{
+ assert(obs_!=nullptr && "null obs");
+ assert(nav_!=nullptr && "null nav");
+ assert(!satellitesPositionsComputed() && "satellites positions already computed");
+
+ GnssUtils::computeSatellitesPositions(*obs_, *nav_, eph_opt, sats_pos_);
+}
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index b38f9e26bd0924f693d045437f3477245af4810d..e105a695def616b479c87357d16db09d56da8e91 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -1,579 +1,470 @@
-/*
- * tdcp.cpp
- *
- * Created on: Dec 4, 2019
- * Author: jvallve
- */
-
#include "gnss_utils/tdcp.h"
+#include "gnss_utils/utils/rcv_position.h"
+#include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/snapshot.h"
namespace GnssUtils
{
-bool Tdcp(const Observations& obs_r,
- Navigation& nav_r,
- const Observations& obs_k,
- const Navigation& nav_k,
+bool Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
- std::set<int>& discarded_sats,
+ const std::set<int>& discarded_sats,
+ std::set<int>& raim_discarded_sats,
const TdcpParams& sd_params,
const prcopt_t& opt)
{
- // COMPUTE SINGLE DIFFERENCES
- return Tdcp(obs_r,
- nav_r,
- computePos(obs_r, nav_r, opt).pos,
- obs_k,
- nav_k,
- d,
- cov_d,
- residual,
- discarded_sats,
- sd_params,
- opt);
+ return Tdcp(snapshot_r,
+ snapshot_k,
+ computePos(*snapshot_r->getObservations(), *snapshot_r->getNavigation(), opt).pos,
+ d,
+ cov_d,
+ residual,
+ discarded_sats,
+ raim_discarded_sats,
+ sd_params,
+ opt);
}
-bool Tdcp(const Observations& obs_r,
- const Navigation& nav_r,
+bool Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
- const Observations& obs_k,
- const Navigation& nav_k,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
- std::set<int>& discarded_sats,
+ const std::set<int>& discarded_sats,
+ std::set<int>& raim_discarded_sats,
const TdcpParams& sd_params,
const prcopt_t& opt)
{
- // FIND COMMON SATELLITES OBSERVATIONS
- Observations common_obs_r, common_obs_k;
- Observations::findCommonObservations(obs_r, obs_k, common_obs_r, common_obs_k);
- int n_common_sats = common_obs_r.getObservations().size();
-
- // COMPUTE COMMON SATELLITES POSITION
- std::map<int, Eigen::Vector3d> common_sats_pos_r, common_sats_pos_k;
- computeSatellitesPositions(common_obs_r, nav_r, opt, common_sats_pos_r);
- computeSatellitesPositions(common_obs_k, sd_params.use_old_nav ? nav_r : nav_k, opt, common_sats_pos_k);
-
- // FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR)
- filterCommonSatellites(
- common_obs_r, common_sats_pos_r, common_obs_k, common_sats_pos_k, discarded_sats, x_r, sd_params, opt);
-
- // COMPUTE SINGLE DIFFERENCES
- return Tdcp(common_obs_r,
- nav_r,
- common_sats_pos_r,
- x_r,
- common_obs_k,
- nav_k,
- common_sats_pos_k,
- d,
- cov_d,
- residual,
- discarded_sats,
- sd_params);
-}
-
-bool Tdcp(const Observations& common_obs_r,
- const Navigation& nav_r,
- const std::map<int, Eigen::Vector3d>& common_sats_pos_r,
- const Eigen::Vector3d& x_r,
- const Observations& common_obs_k,
- const Navigation& nav_k,
- const std::map<int, Eigen::Vector3d>& common_sats_pos_k,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- std::set<int>& discarded_sats,
- const TdcpParams& sd_params)
-{
- assert(common_obs_r.size() == common_obs_k.size());
- assert(common_obs_r.size() == common_sats_pos_r.size());
- assert(common_obs_k.size() == common_sats_pos_k.size());
-
- double tr(common_obs_r.getObservations().front().time.time + common_obs_r.getObservations().front().time.sec);
- double tk(common_obs_k.getObservations().front().time.time + common_obs_k.getObservations().front().time.sec);
-
- int required_n_sats(std::max(sd_params.min_common_sats + sd_params.raim_n, 4 + sd_params.raim_n));
- int n_common_sats = common_sats_pos_k.size();
-
- if (n_common_sats < required_n_sats)
- {
-#if GNSS_UTILS_TDCP_DEBUG == 1
- 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;
- }
-
- // MULTI-FREQUENCY
- std::map<int, std::pair<int, int>> row_2_sat_freq;
- int row = 0;
- for (auto obs_idx = 0; obs_idx < common_obs_k.size(); obs_idx++)
- {
- auto&& obs_r = common_obs_r.getObservationByIdx(obs_idx);
- auto&& obs_k = common_obs_k.getObservationByIdx(obs_idx);
-
- const int& sat_number = obs_k.sat;
-
- if (sd_params.use_carrier_phase)
+ // If use old nav temporary change navigation to (re)compute satellites positions
+ auto nav_k = snapshot_k->getNavigation();
+ if (sd_params.use_old_nav)
{
- // L1/E1
- if (std::abs(obs_r.L[0]) > 1e-12 and std::abs(obs_k.L[0]) > 1e-12)
- {
- row_2_sat_freq[row] = std::make_pair(sat_number, 0);
- row++;
- }
- if (!sd_params.use_multi_freq)
- continue;
-
- // L2 (GPS/GLO/QZS)
- int sys = satsys(sat_number, 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)
- {
- row_2_sat_freq[row] = std::make_pair(sat_number, 1);
- row++;
- }
- // 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_number, 2);
- row++;
- }
+ snapshot_k->getSatellitesPositions().clear();
+ snapshot_k->setNavigation(snapshot_r->getNavigation());
}
- else
- // L1/E1
- if (std::abs(obs_r.P[0]) > 1e-12 and std::abs(obs_k.P[0]) > 1e-12)
+
+ // COMPUTE SATELLITES POSITION
+ if (!snapshot_r->satellitesPositionsComputed())
+ snapshot_r->computeSatellitesPositions(opt.sateph);
+ if (!snapshot_k->satellitesPositionsComputed())
+ snapshot_k->computeSatellitesPositions(opt.sateph);
+
+ // FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR)
+ snapshot_r->getObservations()->filter(snapshot_r->getSatellitesPositions(),
+ discarded_sats,
+ x_r,
+ !sd_params.use_carrier_phase,
+ sd_params.use_carrier_phase,
+ opt);
+ snapshot_k->getObservations()->filter(snapshot_k->getSatellitesPositions(),
+ discarded_sats,
+ x_r,
+ !sd_params.use_carrier_phase,
+ sd_params.use_carrier_phase,
+ opt);
+
+ // FIND COMMON SATELLITES OBSERVATIONS
+ std::set<int> common_sats = Observations::findCommonObservations(*snapshot_r->getObservations(),
+ *snapshot_k->getObservations());
+
+ // COMPUTE TDCP
+ bool tdcp_ok = Tdcp(snapshot_r,
+ snapshot_k,
+ x_r,
+ common_sats,
+ d,
+ cov_d,
+ residual,
+ raim_discarded_sats,
+ sd_params);
+
+ // UNDO temporary change navigation
+ if (sd_params.use_old_nav)
{
- row_2_sat_freq[row] = std::make_pair(sat_number, 0);
- row++;
+ snapshot_k->setNavigation(nav_k);
+ snapshot_k->computeSatellitesPositions(opt.sateph);
}
- }
- 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 << "common sats: ";
- for (auto row_sat_freq_it : row_2_sat_freq)
- std::cout << row_sat_freq_it.second.first << " ";
- std::cout << std::endl;
- std::cout << "discarded_sats: ";
- for (auto sat : discarded_sats)
- std::cout << sat << " ";
- std::cout << std::endl;
-#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;
-
- for (auto row_sat_freq_it : row_2_sat_freq)
- {
- const int& row = row_sat_freq_it.first;
- const int& sat_number = row_sat_freq_it.second.first;
- const int& freq = row_sat_freq_it.second.second;
-
- auto obs_r = common_obs_r.getObservationBySat(sat_number);
- auto obs_k = common_obs_k.getObservationBySat(sat_number);
-
- // excluded satellite
- if (discarded_sats.count(sat_number) != 0)
+
+ return tdcp_ok;
+}
+
+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 TdcpParams& sd_params)
+{
+ // Checks
+ assert(snapshot_r->satellitesPositionsComputed() && "this TDCP assumes satellites already computed");
+ assert(snapshot_k->satellitesPositionsComputed() && "this TDCP assumes satellites already computed");
+ assert(snapshot_r->getSatellitesPositions().size() > snapshot_r->getObservations()->size() && "Satellites without position not filtered");
+ assert(snapshot_k->getSatellitesPositions().size() > snapshot_k->getObservations()->size() && "Satellites without position not filtered");
+ assert(snapshot_r->getSatellitesPositions().size() > common_sats.size() && "Too many common sats");
+ assert(snapshot_k->getSatellitesPositions().size() > common_sats.size() && "Too many common sats");
+ assert(raim_discarded_sats.empty() && "just output param");
+
+ for (auto sat : common_sats)
{
-#if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "\tdiscarded sat" << std::endl;
-#endif
- continue;
+ assert(snapshot_r->getObservations()->hasSatellite(sat) && "wrong common sat");
+ assert(snapshot_k->getObservations()->hasSatellite(sat) && "wrong common sat");
+ assert(snapshot_r->getSatellitesPositions().count(sat) && "common sat without position");
+ assert(snapshot_k->getSatellitesPositions().count(sat) && "common sat without position");
}
- // Satellite's positions
- s_r.col(row) = common_sats_pos_r.at(sat_number);
- s_k.col(row) = common_sats_pos_k.at(sat_number);
- nav_k.getNavigation().ion_gps;
-
- if (sd_params.use_carrier_phase) // TODO: add iono and tropo corrections (optionally)
- drho_m(row) = (obs_k.L[freq] * nav_k.getNavigation().lam[sat_number - 1][freq]) -
- (obs_r.L[freq] * nav_r.getNavigation().lam[sat_number - 1][freq]);
- else
- drho_m(row) = obs_k.P[freq] - obs_r.P[freq];
-
-#if GNSS_UTILS_TDCP_DEBUG == 1
- // std::cout << "\tsat " << sat_number << " frequency " << freq << " wavelength = " <<
- // nav_r.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\tpositions:\n\t\tt_r: " <<
- // s_r.col(row).transpose() << "\n\t\tt_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.getNavigation().lam[sat_number-1][freq] <<
- // std::endl; std::cout << "\tnav_k.getNavigation().lam[sat_number-1][freq]: " <<
- // nav_k.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\trho_r: " << obs_r.L[freq] *
- // nav_r.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\trho_k: " << obs_k.L[freq] *
- // nav_k.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\tdrho_m: " << drho_m(row) <<
- // std::endl;
-#endif
-
- if (!sd_params.relinearize_jacobian)
+ int n_common_sats = common_sats.size();
+ int required_n_sats(std::max(sd_params.min_common_sats + sd_params.raim_n, 4 + sd_params.raim_n));
+ if (n_common_sats < required_n_sats)
{
- // Unit vectors from receivers to satellites
- Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
-
- // Fill Jacobian matrix
- A(row, 0) = u_k(0);
- A(row, 1) = u_k(1);
- A(row, 2) = u_k(2);
- A(row, 3) = -1.0;
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ 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;
}
- }
- // LEAST SQUARES SOLUTION =======================================================================
- for (int j = 0; j < sd_params.max_iterations; j++)
- {
- // fill A and r
- for (int row = 0; row < A.rows(); row++)
+ // Times
+ double tr(-1), tk(-1);
+
+ // MULTI-FREQUENCY
+ std::map<int, std::pair<int, int>> row_2_sat_freq;
+ int row = 0;
+ for (auto sat : common_sats)
{
- // 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);
-
- // Evaluate A
- if (sd_params.relinearize_jacobian)
- {
- // Unit vectors from rcvrs to sats
- Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
-
- // Fill Jacobian matrix
- A(row, 0) = u_k(0);
- A(row, 1) = u_k(1);
- A(row, 2) = u_k(2);
- A(row, 3) = -1.0;
- }
+ assert(snapshot_r->getObservations()->hasSatellite(sat));
+ assert(snapshot_k->getObservations()->hasSatellite(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;
+ }
+
+ // Carrier phase
+ if (sd_params.use_carrier_phase)
+ {
+ // L1/E1
+ if (std::abs(obs_r.L[0]) > 1e-12 and std::abs(obs_k.L[0]) > 1e-12)
+ row_2_sat_freq[row++] = std::make_pair(sat, 0);
+
+ if (!sd_params.use_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)
+ 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);
+ }
+ // Code
+ else
+ // L1/E1
+ if (std::abs(obs_r.P[0]) > 1e-12 and std::abs(obs_k.P[0]) > 1e-12)
+ row_2_sat_freq[row++] = std::make_pair(sat, 0);
}
+ int n_differences = row_2_sat_freq.size();
- // Solve
- cov_d = (A.transpose() * A).inverse();
- delta_d = -cov_d * A.transpose() * r;
- d += delta_d;
+ // 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 << "common sats: ";
+ for (auto row_sat_freq_it : row_2_sat_freq)
+ std::cout << row_sat_freq_it.second.first << " ";
+ std::cout << std::endl;
+ #endif
- // wrong solution
- if (d.array().isNaN().any())
+ // 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;
+
+ for (auto row_sat_freq_it : row_2_sat_freq)
{
- std::cout << "Tdcp: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
- return false;
+ const int& row = row_sat_freq_it.first;
+ 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();
+
+ // Satellite's positions
+ s_r.col(row) = snapshot_r->getSatellitesPositions().at(sat_number);
+ s_k.col(row) = snapshot_r->getSatellitesPositions().at(sat_number);
+ nav_k.ion_gps;
+
+ if (sd_params.use_carrier_phase) // TODO: add iono and tropo corrections (optionally)
+ 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]);
+ else
+ drho_m(row) = obs_k.P[freq] - obs_r.P[freq];
+
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ // std::cout << "\tsat " << sat_number << " frequency " << freq << " wavelength = " <<
+ // nav_r.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\tpositions:\n\t\tt_r: " <<
+ // s_r.col(row).transpose() << "\n\t\tt_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.getNavigation().lam[sat_number-1][freq] <<
+ // std::endl; std::cout << "\tnav_k.getNavigation().lam[sat_number-1][freq]: " <<
+ // nav_k.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\trho_r: " << obs_r.L[freq] *
+ // nav_r.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\trho_k: " << obs_k.L[freq] *
+ // nav_k.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\tdrho_m: " << drho_m(row) <<
+ // std::endl;
+ #endif
+
+ if (!sd_params.relinearize_jacobian)
+ {
+ // Unit vectors from receivers to satellites
+ Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
+
+ // Fill Jacobian matrix
+ A(row, 0) = u_k(0);
+ A(row, 1) = u_k(1);
+ A(row, 2) = u_k(2);
+ A(row, 3) = -1.0;
+ }
}
- // residual
- // residual = sqrt((r - A * delta_d).squaredNorm() / A.rows());
- residual = (r + A * delta_d).norm();
- // std::cout << "residual = " << residual << std::endl;
- // std::cout << "Tdcp2: r-A*delta_d = " << (r + A * delta_d).transpose() << std::endl;
-
-#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: residual = %13.10f\n", residual);
- printf("Tdcp: row = %i\n", r.rows());
- std::cout << "Tdcp: drho = " << r.transpose() << std::endl;
- std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
- std::cout << "Tdcp: H = \n" << A << std::endl;
- printf("Tdcp: dT = %8.3f secs\n", d(3));
-#endif
-
- // RAIM ====================================== (only at first iteration) RAIM WITH SATELLITES, not ROWS
- int input_discarded_sats = discarded_sats.size();
- if (j == 0 and sd_params.raim_n > 0 and residual > sd_params.raim_min_residual)
+ // LEAST SQUARES SOLUTION =======================================================================
+ for (int j = 0; j < sd_params.max_iterations; j++)
{
- int worst_sat_row = -1;
- double best_residual = 1e12;
- Eigen::Vector4d best_d;
- int n_removed_rows = 1;
-
- // remove some satellites
- while (discarded_sats.size() - input_discarded_sats < sd_params.raim_n)
- {
- auto A_raim = A;
- auto r_raim = r;
-
- // solve removing each satellite
- for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
+ // fill A and r
+ for (int row = 0; row < A.rows(); row++)
{
- int sat_removed = row_2_sat_freq.at(row_removed).first;
-
- // Multi-freq: some rows for the same satellite
- n_removed_rows = 1;
- if (sd_params.use_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++;
-
- // 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
- }
-
- // solve
- Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
- Eigen::Vector4d d_raim = d_0 + delta_d_raim;
-
- // no NaN
- if (!d_raim.array().isNaN().any())
- {
- // residual
- residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
- // 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 << "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: H = \n" << A_raim << std::endl;
- printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
-#endif
- // store if best residual
- if (residual < best_residual)
+ // 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);
+
+ // Evaluate A
+ if (sd_params.relinearize_jacobian)
{
- worst_sat_row = row_removed;
- best_residual = residual;
- best_d = d_raim;
+ // Unit vectors from rcvrs to sats
+ Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
+
+ // Fill Jacobian matrix
+ A(row, 0) = u_k(0);
+ A(row, 1) = u_k(1);
+ A(row, 2) = u_k(2);
+ A(row, 3) = -1.0;
}
- }
- // 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);
}
- // No successful RAIM solution
- if (worst_sat_row == -1)
+ // Solve
+ cov_d = (A.transpose() * A).inverse();
+ delta_d = -cov_d * A.transpose() * r;
+ d += delta_d;
+
+ // wrong solution
+ if (d.array().isNaN().any())
{
- printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
- return false;
+ std::cout << "Tdcp: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
+ return false;
}
- // store removed sat
- discarded_sats.insert(row_2_sat_freq[worst_sat_row].first);
-
- // decrease n_common_sats
- n_differences -= n_removed_rows;
- n_common_sats--;
-
- // Remove selected satellite from problem
- std::cout << "resizing problem..." << std::endl;
- auto A_aux = A;
- auto r_aux = r;
- auto drho_m_aux = drho_m;
- auto s_r_aux = s_r;
- auto s_k_aux = s_k;
- A.conservativeResize(n_differences, Eigen::NoChange);
- r.conservativeResize(n_differences);
- drho_m.conservativeResize(n_differences);
- s_r.conservativeResize(Eigen::NoChange, n_differences);
- s_k.conservativeResize(Eigen::NoChange, n_differences);
-
- int back_elements_changing = A_aux.rows() - worst_sat_row - n_removed_rows;
- if (back_elements_changing != 0) // if last satelite removed, conservative resize did the job
+ // residual
+ // residual = sqrt((r - A * delta_d).squaredNorm() / A.rows());
+ residual = (r + A * delta_d).norm();
+ // std::cout << "residual = " << residual << std::endl;
+ // std::cout << "Tdcp2: r-A*delta_d = " << (r + A * delta_d).transpose() << std::endl;
+
+ #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: residual = %13.10f\n", residual);
+ printf("Tdcp: row = %lu\n", r.rows());
+ std::cout << "Tdcp: drho = " << r.transpose() << std::endl;
+ std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
+ std::cout << "Tdcp: H = \n" << A << std::endl;
+ printf("Tdcp: dT = %8.3f secs\n", d(3));
+ #endif
+
+ // RAIM ====================================== (only at first iteration) RAIM WITH SATELLITES, not ROWS
+ if (j == 0 and sd_params.raim_n > 0 and residual > sd_params.raim_min_residual)
{
- A.bottomRows(back_elements_changing) = A_aux.bottomRows(back_elements_changing);
- s_r.rightCols(back_elements_changing) = s_r_aux.rightCols(back_elements_changing);
- 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);
- }
+ 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() < sd_params.raim_n)
+ {
+ auto A_raim = A;
+ auto r_raim = 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 (sd_params.use_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++;
+
+ // 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
+ }
+
+ // solve
+ Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
+ Eigen::Vector4d d_raim = d_0 + delta_d_raim;
+
+ // no NaN
+ if (!d_raim.array().isNaN().any())
+ {
+ // residual
+ residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
+ // 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 << "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: H = \n" << A_raim << std::endl;
+ printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
+ #endif
+
+ // store if best residual
+ if (residual < best_residual)
+ {
+ worst_sat_row = row_removed;
+ best_residual = residual;
+ best_d = d_raim;
+ }
+ }
+ // 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);
+ }
+
+ // No successful RAIM solution
+ if (worst_sat_row == -1)
+ {
+ printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
+ return false;
+ }
+
+ // store removed sat
+ raim_discarded_sats.insert(row_2_sat_freq[worst_sat_row].first);
+
+ // decrease n_common_sats
+ n_differences -= n_removed_rows;
+ n_common_sats--;
+
+ // Remove selected satellite from problem
+ std::cout << "resizing problem..." << std::endl;
+ auto A_aux = A;
+ auto r_aux = r;
+ auto drho_m_aux = drho_m;
+ auto s_r_aux = s_r;
+ auto s_k_aux = s_k;
+ A.conservativeResize(n_differences, Eigen::NoChange);
+ r.conservativeResize(n_differences);
+ drho_m.conservativeResize(n_differences);
+ s_r.conservativeResize(Eigen::NoChange, n_differences);
+ s_k.conservativeResize(Eigen::NoChange, n_differences);
+
+ int back_elements_changing = A_aux.rows() - worst_sat_row - n_removed_rows;
+ if (back_elements_changing != 0) // if last satelite removed, conservative resize did the job
+ {
+ A.bottomRows(back_elements_changing) = A_aux.bottomRows(back_elements_changing);
+ s_r.rightCols(back_elements_changing) = s_r_aux.rightCols(back_elements_changing);
+ 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;
+ cov_d = (A.transpose() * A).inverse();
+
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "Tdcp After RAIM iteration" << std::endl;
+ std::cout << "\tExcluded sats : ";
+ std::cout << "\tCommon sats : " << n_common_sats << std::endl;
+ std::cout << "\tRows : " << n_differences << 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;
+ std::cout << "\tResidual = " << best_residual << std::endl;
+ #endif
+ }
- // apply the RAIM solution
- d = best_d;
- cov_d = (A.transpose() * A).inverse();
+ #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 << "\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;
+ std::cout << "\tResidual = " << best_residual << std::endl;
+ #endif
+ }
-#if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "Tdcp After RAIM iteration" << std::endl;
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "Tdcp iteration " << j << std::endl;
std::cout << "\tExcluded sats : ";
- for (auto dsat : discarded_sats)
- std::cout << (int)dsat << " ";
- std::cout << std::endl;
- std::cout << "\tCommon sats : " << n_common_sats << std::endl;
+ std::cout << "\tCommon sats : " << common_sats.size() << std::endl;
std::cout << "\tRows : " << n_differences << 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;
- std::cout << "\tResidual = " << best_residual << std::endl;
-#endif
- }
-
-#if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "Tdcp After RAIM " << std::endl;
- std::cout << "\tExcluded sats : ";
- for (auto dsat : discarded_sats)
- std::cout << (int)dsat << " ";
- std::cout << std::endl;
- std::cout << "\tCommon sats : " << n_common_sats << std::endl;
- std::cout << "\tRows : " << n_differences << 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;
- std::cout << "\tResidual = " << best_residual << std::endl;
-#endif
+ std::cout << "\tResidual = " << residual << std::endl;
+ #endif
}
-#if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "Tdcp iteration " << j << std::endl;
- std::cout << "\tExcluded sats : ";
- for (auto dsat : discarded_sats)
- std::cout << (int)dsat << " ";
- std::cout << std::endl;
- std::cout << "\tCommon sats : " << n_common_sats << std::endl;
- std::cout << "\tRows : " << n_differences << 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;
- std::cout << "\tResidual = " << residual << std::endl;
-#endif
- }
-
- // weight covariance with the measurement noise (proportional to time)
- double sq_sigma_Tdcp = (tk - tr) * sd_params.sigma_atm * sd_params.sigma_atm +
- 2 * (sd_params.use_carrier_phase ? sd_params.sigma_carrier * sd_params.sigma_carrier :
- sd_params.sigma_code * sd_params.sigma_code);
- 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);
- }
- residual = sqrt(r.squaredNorm() / r.size());
-
- return true;
-}
+ // weight covariance with the measurement noise (proportional to time)
+ double sq_sigma_Tdcp = (tk - tr) * sd_params.sigma_atm * sd_params.sigma_atm +
+ 2 * (sd_params.use_carrier_phase ? sd_params.sigma_carrier * sd_params.sigma_carrier :
+ sd_params.sigma_code * sd_params.sigma_code);
+ cov_d *= sq_sigma_Tdcp;
+ // residual = (r - A * d).norm() / A.rows();
-void filterCommonSatellites(Observations& common_obs_r,
- std::map<int, Eigen::Vector3d>& common_sats_pos_r,
- Observations& common_obs_k,
- std::map<int, Eigen::Vector3d>& common_sats_pos_k,
- std::set<int>& discarded_sats,
- const Eigen::Vector3d& x_r,
- const TdcpParams& sd_params,
- const prcopt_t& opt)
-{
- assert(&common_obs_r != &common_obs_k);
- assert(&common_sats_pos_r != &common_sats_pos_k);
- assert(common_obs_r.size() == common_obs_k.size());
- assert(common_obs_r.size() == common_sats_pos_r.size());
- assert(common_obs_r.size() == common_sats_pos_k.size());
+ // 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);
- // std::cout << "filterCommonSatellites: initial size: " << common_obs_k.size() << std::endl;
+ residual = sqrt(r.squaredNorm() / r.size());
- std::set<int> remove_sats;
-
- for (int obs_i = 0; obs_i < common_obs_r.size(); obs_i++)
- {
- auto&& obs_r = common_obs_r.getObservationByIdx(obs_i);
- auto&& obs_k = common_obs_k.getObservationByIdx(obs_i);
- assert(obs_r.sat == obs_k.sat && "common satellites observations have to be ordered equally");
- const int& sat_number = obs_r.sat;
-
- // already discarded sats
- if (discarded_sats.count(sat_number) != 0)
- {
- remove_sats.insert(sat_number);
- continue;
- }
-
- // wrong data (satellite is not included in the discarded list)
- if (sd_params.use_carrier_phase and (std::abs(obs_r.L[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
- {
- // std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.L[0] << " " <<
- // obs_k.L[0] << std::endl;
- remove_sats.insert(sat_number);
- continue;
- }
- if (!sd_params.use_carrier_phase and (std::abs(obs_r.P[0]) < 1e-12 or std::abs(obs_k.L[0]) < 1e-12))
- {
- // std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_r.P[0] << " " <<
- // obs_k.P[0] << std::endl;
- remove_sats.insert(sat_number);
- continue;
- }
-
- // bad satellite position (satellite is not included in the discarded list)
- if (common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3) or
- common_sats_pos_r[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3))
- {
- // std::cout << "Discarding sat " << sat_number << ": bad satellite position: \n\t" <<
- // common_sats_pos_r[sat_number].transpose() << "\n\t" << common_sats_pos_k[sat_number].transpose() << std::endl;
- remove_sats.insert(sat_number);
- continue;
- }
-
- // constellation
- int sys = satsys(obs_r.sat, NULL);
- if (!(sys & opt.navsys))
- {
- // std::cout << "Discarding sat " << sat_number << ": not selected constellation " << sys << " - mask: " <<
- // opt.navsys << std::endl;
- discarded_sats.insert(sat_number);
- remove_sats.insert(sat_number);
- continue;
- }
-
- // check both elevations
- double elevation = std::min(computeSatElevation(x_r, common_sats_pos_r[sat_number]),
- computeSatElevation(x_r, common_sats_pos_k[sat_number]));
- if (elevation < opt.elmin)
- {
- // std::cout << "Discarding sat " << sat_number << ": low elevation " << elevation << " - min: " << opt.elmin <<
- // std::endl;
- discarded_sats.insert(sat_number);
- remove_sats.insert(sat_number);
- continue;
- }
-
- // snr TODO: multifrequency (2nd param and 3rd idx)
- if (testsnr(0, 0, elevation, obs_r.SNR[0] * 0.25, &opt.snrmask) == 1)
- {
- // std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl;
- discarded_sats.insert(sat_number);
- remove_sats.insert(sat_number);
- continue;
- }
- }
-
- // remove sats
- // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl;
- for (auto sat : remove_sats)
- {
- assert(common_obs_r.hasSatellite(sat));
- assert(common_obs_k.hasSatellite(sat));
- assert(common_sats_pos_r.count(sat));
- assert(common_sats_pos_k.count(sat));
- common_obs_r.removeObservationBySat(sat);
- common_obs_k.removeObservationBySat(sat);
- common_sats_pos_r.erase(sat);
- common_sats_pos_k.erase(sat);
- }
-
- assert(common_obs_r.size() == common_obs_k.size());
- assert(common_obs_r.size() == common_sats_pos_r.size());
- assert(common_obs_r.size() == common_sats_pos_k.size());
-
- // std::cout << "final size: " << common_obs_k.size() << std::endl;
+ return true;
}
} // namespace GnssUtils
diff --git a/src/utils/sat_position.cpp b/src/utils/satellite.cpp
similarity index 63%
rename from src/utils/sat_position.cpp
rename to src/utils/satellite.cpp
index 750441eb04f8e94d998664dda1c121985248a678..878719e31a574b338217438e8b5549217b87b64e 100644
--- a/src/utils/sat_position.cpp
+++ b/src/utils/satellite.cpp
@@ -5,9 +5,12 @@
* Author: Joan Vallvé, Pep MartÃ-Saumell
*/
-#include "gnss_utils/utils/sat_position.h"
+#include "gnss_utils/utils/satellite.h"
-using namespace GnssUtils;
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/transformations.h"
+#include "gnss_utils/gnss_utils.h"
namespace GnssUtils
{
@@ -33,31 +36,31 @@ double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Ve
void computeSatellitesPositions(const Observations& obs,
const Navigation& nav,
- const prcopt_t& opt,
+ const int& eph_opt,
std::map<int, Eigen::Vector3d>& sats_pos)
{
double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()];
int svh[obs.size()];
- // std::cout << "computing sats position from sats: ";
- // for (auto&& obs_ref : obs.getObservations())
- // std::cout << (int)obs_ref.sat << " ";
- // std::cout << std::endl;
+// std::cout << "computing position of sats: ";
+// for (auto&& obs_ref : obs.getObservations())
+// std::cout << (int)obs_ref.sat << " ";
+// std::cout << std::endl;
// compute positions
satposs(
- obs.getObservations().front().time, obs.data(), obs.size(), &nav.getNavigation(), opt.sateph, rs, dts, var, svh);
+ obs.getObservations().front().time, obs.data(), obs.size(), &nav.getNavigation(), eph_opt, rs, dts, var, svh);
// store positions
// std::cout << "filling sats positions: \n";
for (int i = 0; i < obs.size(); i++)
{
- if (svh[i] < 0) // ephemeris unavailable
- sats_pos[obs.getObservationByIdx(i).sat] = Eigen::Vector3d::Zero();
- else
sats_pos[obs.getObservationByIdx(i).sat] << rs[6 * i], rs[6 * i + 1], rs[6 * i + 2];
- // std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << ": " <<
- // sats_pos[obs.getObservationByIdx(i).sat].transpose() << std::endl;
+ // std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << ": " <<
+ // sats_pos[obs.getObservationByIdx(i).sat].transpose() << std::endl;
+ // if (sats_pos[obs.getObservationByIdx(i).sat] == Eigen::Vector3d::Zero())
+ // std::cout << "ephemeris not available for sat " << int(obs.getObservationByIdx(i).sat) << std::endl;
}
}
-} // namespace GnssUtils
\ No newline at end of file
+
+} // namespace GnssUtils
diff --git a/src/utils/transformations.cpp b/src/utils/transformations.cpp
index 38097f55678579cb819ccf01ab0fc1a0cbbd6bfe..81639674696ff9fc38c742cb8f433164529d0814 100644
--- a/src/utils/transformations.cpp
+++ b/src/utils/transformations.cpp
@@ -21,7 +21,7 @@ Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon)
return ecef;
}
-Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef)
+Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d& _cov_ecef)
{
Eigen::Matrix3d cov_enu;
@@ -38,7 +38,7 @@ Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix
return cov_enu;
}
-Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu)
+Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d& _cov_enu)
{
Eigen::Matrix3d cov_ecef;
@@ -149,4 +149,4 @@ void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU;
}
-} // namespace GnssUtils
\ No newline at end of file
+} // namespace GnssUtils
diff --git a/test/gtest_observations.cpp b/test/gtest_observations.cpp
index 422a28c68d9be9fbf3798422328a058db8219a24..cbb25d72d1863ea094dd2bd83cc28c3a8f60a5d6 100644
--- a/test/gtest_observations.cpp
+++ b/test/gtest_observations.cpp
@@ -134,14 +134,15 @@ TEST(ObservationsTest, FindCommonObservations)
ASSERT_TRUE(observations1 == observations2);
- Observations common1;
- Observations common2;
+ std::set<int> common_sats = Observations::findCommonObservations(observations1, observations2);
- Observations::findCommonObservations(observations1, observations2, common1, common2);
-
- ASSERT_TRUE(common1 == common2);
- ASSERT_TRUE(observations1 == common1);
- ASSERT_TRUE(observations2 == common2);
+ ASSERT_TRUE(common_sats.size() == observations1.size());
+ ASSERT_TRUE(common_sats.size() == observations2.size());
+ for (auto sat : common_sats)
+ {
+ ASSERT_TRUE(observations1.hasSatellite(sat));
+ ASSERT_TRUE(observations2.hasSatellite(sat));
+ }
}
TEST(ObservationsTest, FindCommonObservationsRemoved)
@@ -151,16 +152,18 @@ TEST(ObservationsTest, FindCommonObservationsRemoved)
Observations observations2(observations1);
// Remove first observation of observations2
- observations2.removeObservationByIdx(0);
-
- Observations common1;
- Observations common2;
+ int sat_removed = observations2.getObservationByIdx(0).sat;
+ observations2.removeObservationBySat(sat_removed);
- Observations::findCommonObservations(observations1, observations2, common1, common2);
+ std::set<int> common_sats = Observations::findCommonObservations(observations1, observations2);
- ASSERT_TRUE(common1 == common2);
- ASSERT_FALSE(observations1 == common1);
- ASSERT_TRUE(observations2 == common2);
+ ASSERT_FALSE(common_sats.size() == observations1.size());
+ ASSERT_TRUE(common_sats.size() == observations2.size());
+ for (auto sat : common_sats)
+ {
+ ASSERT_TRUE(observations1.hasSatellite(sat));
+ ASSERT_TRUE(observations2.hasSatellite(sat));
+ }
}
TEST(ObservationsTest, FindCommonObservationsChangeTime)
@@ -175,37 +178,14 @@ TEST(ObservationsTest, FindCommonObservationsChangeTime)
obs.time.sec += 10;
}
- Observations common1;
- Observations common2;
-
- Observations::findCommonObservations(observations1, observations2, common1, common2);
-
- ASSERT_FALSE(common1 == common2);
- ASSERT_TRUE(observations1 == common1);
- ASSERT_TRUE(observations2 == common2);
+ std::set<int> common_sats = Observations::findCommonObservations(observations1, observations2);
- for (auto obs = common1.getObservations().begin(); obs != common1.getObservations().end(); ++obs)
+ ASSERT_TRUE(common_sats.size() == observations1.size());
+ ASSERT_TRUE(common_sats.size() == observations2.size());
+ for (auto sat : common_sats)
{
- const obsd_t& obs1 = common1.getObservationBySat(obs->sat);
- const obsd_t& obs2 = common2.getObservationBySat(obs->sat);
-
- ASSERT_FALSE(obs1.time == obs2.time);
- ASSERT_TRUE(obs1.eventime == obs2.eventime);
- ASSERT_TRUE(obs1.timevalid == obs2.timevalid);
- ASSERT_TRUE(obs1.sat == obs2.sat);
- ASSERT_TRUE(obs1.rcv == obs2.rcv);
- ASSERT_TRUE(memcmp(obs1.SNR, obs2.SNR, sizeof(obs1.SNR)) == 0);
- ASSERT_TRUE(memcmp(obs1.LLI, obs2.LLI, sizeof(obs1.LLI)) == 0);
- ASSERT_TRUE(memcmp(obs1.code, obs2.code, sizeof(obs1.code)) == 0);
- ASSERT_TRUE(memcmp(obs1.qualL, obs2.qualL, sizeof(obs1.qualL)) == 0);
- ASSERT_TRUE(memcmp(obs1.qualP, obs2.qualP, sizeof(obs1.qualP)) == 0);
- ASSERT_TRUE(obs1.freq == obs2.freq);
- ASSERT_TRUE(
- equalArray<double>(obs1.L, obs2.L, sizeof(obs1.L) / sizeof(obs1.L[0]), sizeof(obs2.L) / sizeof(obs2.L[0])));
- ASSERT_TRUE(
- equalArray<double>(obs1.P, obs2.P, sizeof(obs1.P) / sizeof(obs1.P[0]), sizeof(obs2.P) / sizeof(obs2.P[0])));
- ASSERT_TRUE(
- equalArray<float>(obs1.D, obs2.D, sizeof(obs1.D) / sizeof(obs1.D[0]), sizeof(obs2.D) / sizeof(obs2.D[0])));
+ ASSERT_TRUE(observations1.hasSatellite(sat));
+ ASSERT_TRUE(observations2.hasSatellite(sat));
}
}
diff --git a/test/gtest_transformations.cpp b/test/gtest_transformations.cpp
index 22f7831647c1b7ebd043de0e737459556f60a284..cf5a3b05838cc18dff9fe057688c74bd2338104c 100644
--- a/test/gtest_transformations.cpp
+++ b/test/gtest_transformations.cpp
@@ -1,5 +1,6 @@
#include "gtest/utils_gtest.h"
-#include "gnss_utils/utils/sat_position.h"
+#include "gnss_utils/utils/satellite.h"
+#include "gnss_utils/utils/transformations.h"
// Geodetic system parameters
static double kSemimajorAxis = 6378137;