Skip to content
Snippets Groups Projects
Commit ca9f24c3 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

improvements in tdcp

parent ea69f6c0
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
Hide whitespace changes
Inline Side-by-side
include/gnss_utils/tdcp.h
+ 9
− 0
View file @ ca9f24c3
...@@ -44,6 +44,15 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, ...@@ -44,6 +44,15 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
const TdcpBatchParams& tdcp_params, const TdcpBatchParams& tdcp_params,
const Options& opt); 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, TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k, SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r, const Eigen::Vector3d& x_r,
......
src/tdcp.cpp
+ 80
− 14
View file @ ca9f24c3
...@@ -31,12 +31,30 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, ...@@ -31,12 +31,30 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
tdcp_params, tdcp_params,
opt); 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, TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k, SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r, const Eigen::Vector3d& x_r,
const Eigen::Vector4d& d_0, const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats, const std::set<int>& discarded_sats,
const std::set<int>& tracked_sats,
const TdcpBatchParams& tdcp_params, const TdcpBatchParams& tdcp_params,
const Options& opt) const Options& opt)
{ {
...@@ -63,12 +81,23 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, ...@@ -63,12 +81,23 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
if (not snapshot_r->rangesComputed()) if (not snapshot_r->rangesComputed())
snapshot_r->computeRanges(x_r, opt); snapshot_r->computeRanges(x_r, opt);
if (not snapshot_k->rangesComputed()) 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 // FIND COMMON SATELLITES
std::set<int> common_sats = Range::findCommonSatellites(snapshot_r->getRanges(), std::set<int> common_sats = Range::findCommonSatellites(snapshot_r->getRanges(),
snapshot_k->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()) if (common_sats.empty())
{ {
TdcpOutput output; TdcpOutput output;
...@@ -141,16 +170,34 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, ...@@ -141,16 +170,34 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
auto&& range_k = snapshot_k->getRanges().at(sat); auto&& range_k = snapshot_k->getRanges().at(sat);
// Carrier phase // Carrier phase
// L1/E1 // corrected
if (std::abs(range_r.L_corrected) > 1e-12 and std::abs(range_k.L_corrected) > 1e-12) if (tdcp_params.tdcp.corr_iono or tdcp_params.tdcp.corr_tropo)
row_2_sat_freq[row++] = std::make_pair(sat, 0); {
// 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) if (!tdcp_params.tdcp.multi_freq)
continue; continue;
// L2 (GPS/GLO/QZS) / E5 (GAL) // L2 (GPS/GLO/QZS) / E5 (GAL)
if (std::abs(range_r.L2_corrected) > 1e-12 and std::abs(range_k.L2_corrected) > 1e-12) 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); row_2_sat_freq[row++] = std::make_pair(sat, 1);
}
} }
int n_differences = row_2_sat_freq.size(); int n_differences = row_2_sat_freq.size();
...@@ -183,10 +230,21 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, ...@@ -183,10 +230,21 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
s_r.col(row) = snapshot_r->getSatellites().at(sat_number).pos; s_r.col(row) = snapshot_r->getSatellites().at(sat_number).pos;
s_k.col(row) = snapshot_k->getSatellites().at(sat_number).pos; s_k.col(row) = snapshot_k->getSatellites().at(sat_number).pos;
if (freq == 0) // time differenced carrier phase measurements
drho_m(row) = range_k.L_corrected - range_r.L_corrected; 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 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 #if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "\tsat " << sat_number; std::cout << "\tsat " << sat_number;
...@@ -194,8 +252,16 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, ...@@ -194,8 +252,16 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
<< "\tpositions:" << std::endl << "\tpositions:" << std::endl
<< "\t\ts_r: " << s_r.col(row).transpose() << std::endl << "\t\ts_r: " << s_r.col(row).transpose() << std::endl
<< "\t\ts_k: " << s_k.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; if (tdcp_params.tdcp.corr_iono or tdcp_params.tdcp.corr_tropo)
std::cout << "\trange_k.L: " << (freq == 0 ? range_k.L_corrected : range_k.L2_corrected) << 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;
}
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; std::cout << "\tdrho_m: " << drho_m(row) << std::endl;
#endif #endif
......
test/gtest_tdcp.cpp
+ 18
− 8
View file @ ca9f24c3
...@@ -133,9 +133,11 @@ TEST(Tdcp, Tdcp) ...@@ -133,9 +133,11 @@ TEST(Tdcp, Tdcp)
// Range r // Range r
Range range_r; Range range_r;
range_r.sat = j; 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.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; range_r.L2_var = range_r.L_var;
snapshot_r->getRanges().emplace(j, range_r); snapshot_r->getRanges().emplace(j, range_r);
...@@ -154,9 +156,11 @@ TEST(Tdcp, Tdcp) ...@@ -154,9 +156,11 @@ TEST(Tdcp, Tdcp)
// Range k // Range k
Range range_k; Range range_k;
range_k.sat = j; 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.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; range_k.L2_var = range_k.L_var;
snapshot_k->getRanges().emplace(j, range_k); snapshot_k->getRanges().emplace(j, range_k);
...@@ -265,9 +269,11 @@ TEST(Tdcp, Tdcp_raim) ...@@ -265,9 +269,11 @@ TEST(Tdcp, Tdcp_raim)
// Range r // Range r
Range range_r; Range range_r;
range_r.sat = j; 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.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; range_r.L2_var = range_r.L_var;
snapshot_r->getRanges().emplace(j, range_r); snapshot_r->getRanges().emplace(j, range_r);
...@@ -286,9 +292,11 @@ TEST(Tdcp, Tdcp_raim) ...@@ -286,9 +292,11 @@ TEST(Tdcp, Tdcp_raim)
// Range k // Range k
Range range_k; Range range_k;
range_k.sat = j; 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.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; range_k.L2_var = range_k.L_var;
snapshot_k->getRanges().emplace(j, range_k); snapshot_k->getRanges().emplace(j, range_k);
...@@ -300,8 +308,10 @@ TEST(Tdcp, Tdcp_raim) ...@@ -300,8 +308,10 @@ TEST(Tdcp, Tdcp_raim)
// Distort 2 ranges -> to be detected by RAIM // Distort 2 ranges -> to be detected by RAIM
int wrong_sat1 = i % N_sats; int wrong_sat1 = i % N_sats;
snapshot_k->getRanges().at(wrong_sat1).L_corrected += 20; 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; 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_corrected -= 10;
snapshot_r->getRanges().at(wrong_sat2).L -= 10;
// TDCP // TDCP
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment