From 1d592939117f355148034c22ddd723a2ef2b93c9 Mon Sep 17 00:00:00 2001 From: joanvallve <jvallve@iri.upc.edu> Date: Tue, 2 Mar 2021 08:17:52 +0100 Subject: [PATCH] WIP --- include/gnss_utils/range.h | 2 ++ include/gnss_utils/snapshot.h | 24 +++++++++++++++++------- src/tdcp.cpp | 13 +++---------- test/gtest_tdcp.cpp | 27 ++++++++++++++++++++++----- 4 files changed, 44 insertions(+), 22 deletions(-) diff --git a/include/gnss_utils/range.h b/include/gnss_utils/range.h index caf9e9d..7c893a3 100644 --- a/include/gnss_utils/range.h +++ b/include/gnss_utils/range.h @@ -18,6 +18,7 @@ namespace GnssUtils class Range { public: + int sys = SYS_GPS; int sat = 0; double P = -1; double P_var = 1; @@ -28,6 +29,7 @@ class Range double L = -1; double L_corrected = -1; double L_var = 1; + int freq = 0; public: Range(); diff --git a/include/gnss_utils/snapshot.h b/include/gnss_utils/snapshot.h index e2ef0ce..f1ebc63 100644 --- a/include/gnss_utils/snapshot.h +++ b/include/gnss_utils/snapshot.h @@ -27,16 +27,15 @@ public: // Public methods void loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = ""); + double getGPST() const; - ObservationsPtr getObservations() const; + // Navigation NavigationPtr getNavigation() const; - const Satellites& getSatellites() const; - Satellites& getSatellites(); - void setObservations(ObservationsPtr obs); void setNavigation(NavigationPtr nav); - void computeSatellites(const int& eph_opt); // see rtklib.h L396); - bool satellitesComputed() const; + // Observations + ObservationsPtr getObservations() const; + void setObservations(ObservationsPtr obs); std::set<int> filterObservations(const std::set<int>& discarded_sats, const Eigen::Vector3d& x_r, @@ -49,7 +48,13 @@ public: const bool& check_carrier_phase, const Options& opt); - // Pseudo-ranges + // Satellites + const Satellites& getSatellites() const; + Satellites& getSatellites(); + void computeSatellites(const int& eph_opt); // see rtklib.h L396); + bool satellitesComputed() const; + + // Ranges Ranges& getRanges(); const Ranges& getRanges() const; bool rangesComputed() const; @@ -81,6 +86,11 @@ private: namespace GnssUtils { +inline double Snapshot::getGPST() const +{ + return obs_->getObservations()[0].time.time + obs_->getObservations()[0].time.sec; +} + inline GnssUtils::ObservationsPtr Snapshot::getObservations() const { return obs_; diff --git a/src/tdcp.cpp b/src/tdcp.cpp index c67052f..25a1726 100644 --- a/src/tdcp.cpp +++ b/src/tdcp.cpp @@ -102,6 +102,7 @@ bool Tdcp(SnapshotPtr snapshot_r, std::set<int> raim_discarded_sats, const TdcpBatchParams& tdcp_params) { + // TODO: change obs -> ranges // Checks assert(snapshot_r->satellitesComputed() && "this TDCP assumes satellites already computed"); assert(snapshot_k->satellitesComputed() && "this TDCP assumes satellites already computed"); @@ -131,7 +132,8 @@ bool Tdcp(SnapshotPtr snapshot_r, } // Times - double tr(-1), tk(-1); + double tr = snapshot_r->getGPST(); + double tk = snapshot_k->getGPST(); // MULTI-FREQUENCY std::map<int, std::pair<int, int>> row_2_sat_freq; @@ -144,15 +146,6 @@ bool Tdcp(SnapshotPtr snapshot_r, 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; - std::cout << "tr: " << tr << std::endl; - std::cout << "tk: " << tk << std::endl; - } - // Carrier phase // L1/E1 if (std::abs(obs_r.L[0]) > 1e-12 and std::abs(obs_k.L[0]) > 1e-12) diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp index 447c273..3b5d0dc 100644 --- a/test/gtest_tdcp.cpp +++ b/test/gtest_tdcp.cpp @@ -52,8 +52,8 @@ void computeRandomVisibleSatellite(const Eigen::Vector3d& receiver_latlonalt, TEST(Tdcp, Tdcp) { - Eigen::Vector3d t_ENU_ECEF,t_ECEF_ENU,t_ENU_latlonalt, sat_ENU, sat_ECEF; - Eigen::Vector3d r1_LLA, r1_ECEF, r2_ECEF, d_ECEF; + Eigen::Vector3d sat_ENU, sat_ECEF; + Eigen::Vector3d r1_LLA, r1_ECEF, r2_LLA, r2_ECEF, d_ECEF; Eigen::Matrix3d R_ENU_ECEF; Eigen::Vector2d azel, azel2; double range; @@ -61,6 +61,9 @@ TEST(Tdcp, Tdcp) Satellite sat1({0,0,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),1.0,0,0,1}); Satellite sat2(sat1); + auto shapshot1 = std::make_shared<Snapshot>(); + auto shapshot2 = std::make_shared<Snapshot>(); + // Random receiver position for (auto i = 0; i<100; i++) { @@ -68,22 +71,36 @@ TEST(Tdcp, Tdcp) r1_ECEF = latLonAltToEcef(r1_LLA); d_ECEF = Eigen::Vector3d::Random() * 10; r2_ECEF = r1_ECEF + d_ECEF; - - Satellites sats1, sats2; + r2_LLA = ecefToLatLonAlt(r2_ECEF); // random visible satellites for (auto j = 0; j<10; j++) { - sat1.pos; + sat1.sat = j; + sat2.sat = j; + computeRandomVisibleSatellite(r1_LLA, sat_ENU, sat_ECEF, azel, range); + sat1.pos = sat_ECEF; computeRandomVisibleSatellite(r2_LLA, sat_ENU, sat_ECEF, azel, range); azel2 = computeAzel(sat_ECEF, t_ECEF_ENU); ASSERT_MATRIX_APPROX(azel,azel2,1e-6); } + + bool Tdcp(SnapshotPtr snapshot_r, + SnapshotPtr snapshot_k, + r1_ECEF, + const std::set<int> common_sats, + Eigen::Vector4d& d, + Eigen::Matrix4d& cov_d, + double& residual, + std::set<int> raim_discarded_sats, + const TdcpBatchParams& tdcp_params) } + + } int main(int argc, char **argv) -- GitLab