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
This commit is part of merge request !20. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
include/gnss_utils/tdcp.h
+ 9
− 0
Edit View file @ ca9f24c3
......@@ -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,
......
src/tdcp.cpp
+ 80
− 14
Edit View file @ ca9f24c3
......@@ -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
......
test/gtest_tdcp.cpp
+ 18
− 8
Edit View file @ ca9f24c3
......@@ -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
......
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