diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index dadb9384f8786ac3d2f135e5016eceec55cfd0d2..7005a84596a41a9fb6a4cd491b8d7b6f546f1111 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -44,6 +44,15 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
const TdcpBatchParams& tdcp_params,
const Options& opt);
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const std::set<int>& tracked_sats,
+ const TdcpBatchParams& tdcp_params,
+ const Options& opt);
+
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index ad76131b3d47f4ce0db4e67a8a3e898d05ec3cd2..995489cf4b66e4d75ea5f19127e7ec269133cd13 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -31,12 +31,30 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
tdcp_params,
opt);
}
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const TdcpBatchParams& tdcp_params,
+ const Options& opt)
+{
+ return Tdcp(snapshot_r,
+ snapshot_k,
+ x_r,
+ d_0,
+ discarded_sats,
+ std::set<int>({}),
+ tdcp_params,
+ opt);
+}
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats,
+ const std::set<int>& tracked_sats,
const TdcpBatchParams& tdcp_params,
const Options& opt)
{
@@ -63,12 +81,23 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
if (not snapshot_r->rangesComputed())
snapshot_r->computeRanges(x_r, opt);
if (not snapshot_k->rangesComputed())
- snapshot_k->computeRanges(x_r, opt); // the x_r is only used to compute Azels -> corrections
+ snapshot_k->computeRanges(x_r, opt); // the x_r is only used to compute Azels -> iono&tropo corrections
// FIND COMMON SATELLITES
std::set<int> common_sats = Range::findCommonSatellites(snapshot_r->getRanges(),
snapshot_k->getRanges());
+ // FILTER OUT SATELLITES NOT IN tracked_sats (if not empty)
+ if (not tracked_sats.empty())
+ {
+ auto com_sat_it = common_sats.begin();
+ while (com_sat_it != common_sats.end())
+ if (tracked_sats.count(*com_sat_it) == 0)
+ com_sat_it = common_sats.erase(com_sat_it);
+ else
+ com_sat_it++;
+ }
+
if (common_sats.empty())
{
TdcpOutput output;
@@ -141,16 +170,34 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
auto&& range_k = snapshot_k->getRanges().at(sat);
// Carrier phase
- // L1/E1
- if (std::abs(range_r.L_corrected) > 1e-12 and std::abs(range_k.L_corrected) > 1e-12)
- row_2_sat_freq[row++] = std::make_pair(sat, 0);
+ // corrected
+ if (tdcp_params.tdcp.corr_iono or tdcp_params.tdcp.corr_tropo)
+ {
+ // L1/E1
+ if (std::abs(range_r.L_corrected) > 1e-12 and std::abs(range_k.L_corrected) > 1e-12)
+ row_2_sat_freq[row++] = std::make_pair(sat, 0);
+
+ if (!tdcp_params.tdcp.multi_freq)
+ continue;
+
+ // L2 (GPS/GLO/QZS) / E5 (GAL)
+ if (std::abs(range_r.L2_corrected) > 1e-12 and std::abs(range_k.L2_corrected) > 1e-12)
+ row_2_sat_freq[row++] = std::make_pair(sat, 1);
+ }
+ // not corrected
+ else
+ {
+ // L1/E1
+ if (std::abs(range_r.L) > 1e-12 and std::abs(range_k.L) > 1e-12)
+ row_2_sat_freq[row++] = std::make_pair(sat, 0);
- if (!tdcp_params.tdcp.multi_freq)
- continue;
+ if (!tdcp_params.tdcp.multi_freq)
+ continue;
- // L2 (GPS/GLO/QZS) / E5 (GAL)
- if (std::abs(range_r.L2_corrected) > 1e-12 and std::abs(range_k.L2_corrected) > 1e-12)
- row_2_sat_freq[row++] = std::make_pair(sat, 1);
+ // L2 (GPS/GLO/QZS) / E5 (GAL)
+ if (std::abs(range_r.L2) > 1e-12 and std::abs(range_k.L2) > 1e-12)
+ row_2_sat_freq[row++] = std::make_pair(sat, 1);
+ }
}
int n_differences = row_2_sat_freq.size();
@@ -183,10 +230,21 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
s_r.col(row) = snapshot_r->getSatellites().at(sat_number).pos;
s_k.col(row) = snapshot_k->getSatellites().at(sat_number).pos;
- if (freq == 0)
- drho_m(row) = range_k.L_corrected - range_r.L_corrected;
+ // time differenced carrier phase measurements
+ if (tdcp_params.tdcp.corr_iono or tdcp_params.tdcp.corr_tropo)
+ {
+ if (freq == 0)
+ drho_m(row) = range_k.L_corrected - range_r.L_corrected;
+ else
+ drho_m(row) = range_k.L2_corrected - range_r.L2_corrected;
+ }
else
- drho_m(row) = range_k.L2_corrected - range_r.L2_corrected;
+ {
+ if (freq == 0)
+ drho_m(row) = range_k.L - range_r.L;
+ else
+ drho_m(row) = range_k.L2 - range_r.L2;
+ }
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "\tsat " << sat_number;
@@ -194,8 +252,16 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
<< "\tpositions:" << std::endl
<< "\t\ts_r: " << s_r.col(row).transpose() << std::endl
<< "\t\ts_k: " << s_k.col(row).transpose() << std::endl;
- std::cout << "\trange_r.L: " << (freq == 0 ? range_r.L_corrected : range_r.L2_corrected) << std::endl;
- std::cout << "\trange_k.L: " << (freq == 0 ? range_k.L_corrected : range_k.L2_corrected) << std::endl;
+ if (tdcp_params.tdcp.corr_iono or tdcp_params.tdcp.corr_tropo)
+ {
+ std::cout << "\trange_r.L: " << (freq == 0 ? range_r.L_corrected : range_r.L2_corrected) << std::endl;
+ std::cout << "\trange_k.L: " << (freq == 0 ? range_k.L_corrected : range_k.L2_corrected) << std::endl;
+ }
+ else
+ {
+ std::cout << "\trange_r.L: " << (freq == 0 ? range_r.L : range_r.L2) << std::endl;
+ std::cout << "\trange_k.L: " << (freq == 0 ? range_k.L : range_k.L2) << std::endl;
+ }
std::cout << "\tdrho_m: " << drho_m(row) << std::endl;
#endif
diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp
index 1338b7e09879b6d6ae93c551a52f6165e34f0112..cdb90c91ac14a642e40a7a3f75cd8c549cc1de2e 100644
--- a/test/gtest_tdcp.cpp
+++ b/test/gtest_tdcp.cpp
@@ -133,9 +133,11 @@ TEST(Tdcp, Tdcp)
// Range r
Range range_r;
range_r.sat = j;
- range_r.L_corrected = range + CLIGHT * clock_r;
+ range_r.L = range + CLIGHT * clock_r;
+ range_r.L_corrected = range_r.L;
range_r.L_var = 1;
- range_r.L2_corrected = range_r.L_corrected;
+ range_r.L2 = range_r.L;
+ range_r.L2_corrected = range_r.L;
range_r.L2_var = range_r.L_var;
snapshot_r->getRanges().emplace(j, range_r);
@@ -154,9 +156,11 @@ TEST(Tdcp, Tdcp)
// Range k
Range range_k;
range_k.sat = j;
- range_k.L_corrected = range + CLIGHT * clock_k;
+ range_k.L = range + CLIGHT * clock_k;
+ range_k.L_corrected = range_k.L;
range_k.L_var = 1;
- range_k.L2_corrected = range_k.L_corrected;
+ range_k.L2 = range_k.L;
+ range_k.L2_corrected = range_k.L;
range_k.L2_var = range_k.L_var;
snapshot_k->getRanges().emplace(j, range_k);
@@ -265,9 +269,11 @@ TEST(Tdcp, Tdcp_raim)
// Range r
Range range_r;
range_r.sat = j;
- range_r.L_corrected = range + CLIGHT * clock_r;
+ range_r.L = range + CLIGHT * clock_r;
+ range_r.L_corrected = range_r.L;
range_r.L_var = 1;
- range_r.L2_corrected = range_r.L_corrected;
+ range_r.L2 = range_r.L;
+ range_r.L2_corrected = range_r.L;
range_r.L2_var = range_r.L_var;
snapshot_r->getRanges().emplace(j, range_r);
@@ -286,9 +292,11 @@ TEST(Tdcp, Tdcp_raim)
// Range k
Range range_k;
range_k.sat = j;
- range_k.L_corrected = range + CLIGHT * clock_k;
+ range_k.L = range + CLIGHT * clock_k;
+ range_k.L_corrected = range_k.L;
range_k.L_var = 1;
- range_k.L2_corrected = range_k.L_corrected;
+ range_k.L2 = range_k.L;
+ range_k.L2_corrected = range_k.L;
range_k.L2_var = range_k.L_var;
snapshot_k->getRanges().emplace(j, range_k);
@@ -300,8 +308,10 @@ TEST(Tdcp, Tdcp_raim)
// Distort 2 ranges -> to be detected by RAIM
int wrong_sat1 = i % N_sats;
snapshot_k->getRanges().at(wrong_sat1).L_corrected += 20;
+ snapshot_k->getRanges().at(wrong_sat1).L += 20;
int wrong_sat2 = (i + N_sats / 2) % N_sats;
snapshot_r->getRanges().at(wrong_sat2).L_corrected -= 10;
+ snapshot_r->getRanges().at(wrong_sat2).L -= 10;
// TDCP