diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h index a4056d03b3a3fee72bb4424fbf0f4cee6c93a7e3..caba378906a01b7c20a30721a809a308dc5002ba 100644 --- a/include/gnss_utils/observations.h +++ b/include/gnss_utils/observations.h @@ -11,7 +11,6 @@ #include <eigen3/Eigen/Dense> #include "gnss_utils/gnss_utils.h" -#include "gnss_utils/utils/utils.h" namespace GnssUtils { @@ -19,6 +18,8 @@ class Observations; typedef std::shared_ptr<Observations> ObservationsPtr; typedef std::shared_ptr<const Observations> ObservationsConstPtr; +typedef std::map<int,Eigen::Vector3d> SatellitesPositions; + class Observations { public: @@ -63,25 +64,25 @@ public: void printByIdx(const int& _idx); void print(); - // TODO: -// std::set<int> filterEphemeris() const; -// std::set<int> removeSatellites() const; -// std::set<int> filterWrongCode() const; -// std::set<int> filterWrongCarrierPhase() const; -// std::set<int> filterConstellations() const; -// std::set<int> filterElevation() const; -// std::set<int> filterSnr() const; - std::set<int> filter(std::map<int, Eigen::Vector3d>& sats_pos, - std::set<int>& discarded_sats, - const Eigen::Vector3d& x_r, - const bool& check_code, - const bool& check_carrier_phase, - const prcopt_t& opt); - - 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; @@ -95,6 +96,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 index 41c8dfcd681ffdd9a0168a1f011442869b38090d..8ba30db64797bfb19bd5a54263e91e670d997db6 100644 --- a/include/gnss_utils/snapshot.h +++ b/include/gnss_utils/snapshot.h @@ -9,16 +9,22 @@ #include <eigen3/Eigen/Dense> -#include "gnss_utils/observations.h" -#include "gnss_utils/navigation.h" +#include "gnss_utils/gnss_utils.h" namespace GnssUtils { + +// forward declarations +class Observations; +class Navigation; +typedef std::map<int,Eigen::Vector3d> SatellitesPositions; +typedef std::shared_ptr<Observations> ObservationsPtr; +typedef std::shared_ptr<Navigation> NavigationPtr; + class Snapshot; typedef std::shared_ptr<Snapshot> SnapshotPtr; typedef std::shared_ptr<const Snapshot> SnapshotConstPtr; -typedef std::map<int,Eigen::Vector3d> SatellitesPositions; class Snapshot { @@ -60,6 +66,17 @@ private: // 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_; diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h index 0a0b2ffc5187d189d826eb3d531de9b1bbfa0f5c..10e280d03e9dca9de92954ac6d10b611376457b1 100644 --- a/include/gnss_utils/tdcp.h +++ b/include/gnss_utils/tdcp.h @@ -13,8 +13,7 @@ #include <set> #include "gnss_utils/utils/rcv_position.h" #include "utils/satellite.h" -#include "gnss_utils/observations.h" -#include "gnss_utils/navigation.h" +#include "gnss_utils/snapshot.h" namespace GnssUtils { @@ -36,52 +35,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(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(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 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 filterCommonObservations(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 bool& check_code, - const bool& check_carrier_phase, - 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/satellite.h b/include/gnss_utils/utils/satellite.h index 665443d85d06af9fbf357b539bc15eda1a16f822..8ca85f3aaf937a36146e3d8dd02cfe65b6a641b6 100644 --- a/include/gnss_utils/utils/satellite.h +++ b/include/gnss_utils/utils/satellite.h @@ -12,14 +12,17 @@ #include <eigen3/Eigen/Geometry> #include <set> +#include <map> -#include "gnss_utils/observations.h" -#include "gnss_utils/navigation.h" -#include "gnss_utils/utils/transformations.h" -#include "gnss_utils/gnss_utils.h" 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, diff --git a/src/observations.cpp b/src/observations.cpp index 4c455d0a43f459b3923ca9fd6e4335a100f78dfe..2cedfe367af1266e0b6bf8651c9b6b889dedb374 100644 --- a/src/observations.cpp +++ b/src/observations.cpp @@ -188,108 +188,216 @@ void Observations::print() } } -std::set<int> Observations::filter(std::map<int, Eigen::Vector3d>& sats_pos, - std::set<int>& discarded_sats, - const Eigen::Vector3d& x_r, - const bool& check_code, - const bool& check_carrier_phase, - const prcopt_t& opt) +std::set<int> Observations::filterByEphemeris(const SatellitesPositions& sats_pos) { - std::cout << "filterSatellites: initial size: " << obs_.size() << std::endl; + std::set<int> remove_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; + + // 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); + } + } - for (int obs_i = 0; obs_i < obs_.size(); obs_i++) - { - auto&& obs_sat = getObservationByIdx(obs_i); - const int& sat_number = obs_sat.sat; + // 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; - // already discarded sats - if (discarded_sats.count(sat_number) != 0) + for (int obs_i = 0; obs_i < obs_.size(); obs_i++) { - remove_sats.insert(sat_number); - continue; + 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); } - // wrong data (satellite is not included in the discarded list) - if (check_carrier_phase and std::abs(obs_sat.L[0]) < 1e-12) + // remove sats + // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl; + for (auto sat : remove_sats) { - std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_sat.L[0] << std::endl; - remove_sats.insert(sat_number); - continue; + assert(hasSatellite(sat)); + removeObservationBySat(sat); } - if (check_code and std::abs(obs_sat.P[0]) < 1e-12) + + 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++) { - std::cout << "Discarding sat " << sat_number << ": wrong carrier phase data: " << obs_sat.P[0] << std::endl; - remove_sats.insert(sat_number); - continue; + 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); + } } - // bad or inexistent satellite position (satellite is not included in the discarded list) - if (sats_pos.count(sat_number) == 0 or - sats_pos[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3) or - sats_pos[sat_number].isApprox(Eigen::Vector3d::Zero(), 1e-3)) + // remove sats + // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl; + for (auto sat : remove_sats) { - std::cout << "Discarding sat " << sat_number << ": wrong satellite position: \n\t" << sats_pos[sat_number].transpose() << std::endl; - remove_sats.insert(sat_number); - continue; + assert(hasSatellite(sat)); + removeObservationBySat(sat); } - // constellation - int sys = satsys(obs_sat.sat, NULL); - if (!(sys & opt.navsys)) + 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++) { - 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; + 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; + } } - // check both elevations - double elevation = computeSatElevation(x_r, sats_pos[sat_number]); - if (elevation < opt.elmin) + // remove sats + // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl; + for (auto sat : remove_sats) { - 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; + assert(hasSatellite(sat)); + removeObservationBySat(sat); } - // snr TODO: multifrequency (2nd param and 3rd idx) - if (testsnr(0, 0, elevation, obs_sat.SNR[0] * 0.25, &opt.snrmask) == 1) + 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++) { - std::cout << "Discarding sat " << sat_number << ": snr test " << std::endl; - discarded_sats.insert(sat_number); - remove_sats.insert(sat_number); - continue; + 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)); - assert(sats_pos.count(sat)); - removeObservationBySat(sat); - sats_pos.erase(sat); - } + // remove sats + // std::cout << "removing: " << remove_sats.size() << " satellites" << std::endl; + for (auto sat : remove_sats) + { + assert(hasSatellite(sat)); + removeObservationBySat(sat); + } - assert(obs_.size() == sats_pos.size()); + return remove_sats; +} - 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; } -void Observations::findCommonObservations(const Observations& obs_1, - const Observations& obs_2, - Observations& common_obs_1, - Observations& common_obs_2) +std::set<int> Observations::findCommonObservations(const Observations& obs_1, + const Observations& obs_2) { - // clear and reserve - common_obs_1.clearObservations(); - common_obs_2.clearObservations(); - // std::cout << "obs 1 sats: "; // for (auto&& obs_1_ref : obs_1.getObservations()) // std::cout << (int)obs_1_ref.sat << " "; @@ -299,18 +407,19 @@ 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; } diff --git a/src/tdcp.cpp b/src/tdcp.cpp index a3d6d357cb2d6f33def9215c7c004aa88e963a01..cfe42cca3d4e9c73bdf3543300094b093e33525d 100644 --- a/src/tdcp.cpp +++ b/src/tdcp.cpp @@ -9,570 +9,466 @@ 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.sateph, common_sats_pos_r); - computeSatellitesPositions(common_obs_k, sd_params.use_old_nav ? nav_r : nav_k, opt.sateph, common_sats_pos_k); - - // FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR) - filterCommonObservations( - common_obs_r, common_sats_pos_r, common_obs_k, common_sats_pos_k, discarded_sats, x_r, !sd_params.use_carrier_phase, sd_params.use_carrier_phase, 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(); + + // 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 - // Solve - cov_d = (A.transpose() * A).inverse(); - delta_d = -cov_d * A.transpose() * r; - d += delta_d; + // 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; - // wrong solution - if (d.array().isNaN().any()) + 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 = %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 + 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; - // apply the RAIM solution - d = best_d; - cov_d = (A.transpose() * A).inverse(); + // 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 + } + + #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; -} - -void filterCommonObservations(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 bool& check_code, - const bool& check_carrier_phase, - 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()); + // 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(); - // TODO: 2 calls to filterObservations and take the common again + // 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 << "filterSatellites: 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 (check_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 (check_code 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 << ": wrong 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()); - - return remove_sats; - // std::cout << "final size: " << common_obs_k.size() << std::endl; + return true; } } // namespace GnssUtils diff --git a/src/utils/satellite.cpp b/src/utils/satellite.cpp index 0a9193044f11fcebea129e16cf21cae6a07032cf..878719e31a574b338217438e8b5549217b87b64e 100644 --- a/src/utils/satellite.cpp +++ b/src/utils/satellite.cpp @@ -7,7 +7,10 @@ #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 { diff --git a/test/gtest_observations.cpp b/test/gtest_observations.cpp index 5d2de7aab2437b85c303c968fd696844ccf058eb..d64de85415cc59e7c5d33d559cc842d334ab46f6 100644 --- a/test/gtest_observations.cpp +++ b/test/gtest_observations.cpp @@ -130,17 +130,17 @@ 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) { Observations observations1; @@ -148,17 +148,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); - - ASSERT_TRUE(common1 == common2); - ASSERT_FALSE(observations1 == common1); - ASSERT_TRUE(observations2 == common2); + std::set<int> common_sats = Observations::findCommonObservations(observations1, observations2); + 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) @@ -173,34 +174,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 f142ba3e57e01109957d2506be7a133fa706bf06..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/satellite.h" +#include "gnss_utils/utils/transformations.h" // Geodetic system parameters static double kSemimajorAxis = 6378137;