From b7f35fa4017a55655eb8780b28991607f6c2084b Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 19 Mar 2021 17:11:32 +0100
Subject: [PATCH] working!
---
include/gnss_utils/tdcp.h | 4 +-
src/tdcp.cpp | 37 ++++++++++----
test/gtest_tdcp.cpp | 105 +++++++++++++++++++++++++++++++-------
3 files changed, 117 insertions(+), 29 deletions(-)
diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 5cc3919..deb5aed 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -46,7 +46,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
- std::set<int> raim_discarded_sats,
+ std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params);
bool Tdcp(const Eigen::Vector3d& x_r,
@@ -58,7 +58,7 @@ bool Tdcp(const Eigen::Vector3d& x_r,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
- std::set<int> raim_discarded_sats,
+ std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params);
} // namespace GnssUtils
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index bb36f5b..8e0b0e0 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -95,7 +95,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
- std::set<int> raim_discarded_sats,
+ std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params)
{
// CHECKS
@@ -137,7 +137,6 @@ bool Tdcp(SnapshotPtr snapshot_r,
double tk = snapshot_k->getGPST();
// MULTI-FREQUENCY
- // TODO: change obs -> ranges
std::map<int, std::pair<int, int>> row_2_sat_freq;
int row = 0;
for (auto sat : common_sats)
@@ -197,7 +196,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
drho_m(row) = range_k.L2_corrected - range_r.L2_corrected;
#if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "\tsat " << sat_number
+ std::cout << "\tsat " << sat_number;
std::cout << std::setprecision(10)
<< "\tpositions:" << std::endl
<< "\t\ts_r: " << s_r.col(row).transpose() << std::endl
@@ -244,7 +243,7 @@ bool Tdcp(const Eigen::Vector3d& x_r,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
- std::set<int> raim_discarded_rows,
+ std::set<int>& raim_discarded_rows,
const TdcpBatchParams& tdcp_params)
{
@@ -256,6 +255,10 @@ bool Tdcp(const Eigen::Vector3d& x_r,
// fill A and r
for (int row = 0; row < A.rows(); row++)
{
+ // skip discarded rows
+ if (raim_discarded_rows.count(row) != 0)
+ continue;
+
// 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);
@@ -282,6 +285,19 @@ bool Tdcp(const Eigen::Vector3d& x_r,
if (d.array().isNaN().any())
{
std::cout << "Tdcp: NLS DID NOT WORK. NAN values appeared. ABORTING!!" << 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: rows = %lu\n", r.rows());
+ std::cout << "Tdcp: r = " << r.transpose() << std::endl;
+ std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
+ std::cout << "Tdcp: A = \n" << A << std::endl;
+ std::cout << "Tdcp: H = \n" << A.transpose() * A << std::endl;
+ std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl;
+ printf("Tdcp: dT = %8.3f secs\n", d(3));
+ #endif
return false;
}
@@ -297,7 +313,7 @@ bool Tdcp(const Eigen::Vector3d& x_r,
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 = %lu\n", r.rows());
+ printf("Tdcp: rows = %lu\n", r.rows());
std::cout << "Tdcp: r = " << r.transpose() << std::endl;
std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
std::cout << "Tdcp: A = \n" << A << std::endl;
@@ -342,16 +358,15 @@ bool Tdcp(const Eigen::Vector3d& x_r,
// residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
#if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "RAIM excluding row " << << row_removed << std::endl;
+ std::cout << "RAIM excluding row " << row_removed << 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: A = \n" << A << std::endl;
- std::cout << "Tdcp: H = \n" << A.transpose() * A << std::endl;
- std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl;
+ std::cout << "Tdcp: A = \n" << A_raim << std::endl;
+ std::cout << "Tdcp: H = \n" << A_raim.transpose() * A_raim << std::endl;
printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
#endif
@@ -378,6 +393,10 @@ bool Tdcp(const Eigen::Vector3d& x_r,
// store removed sat
raim_discarded_rows.insert(worst_sat_row);
+ // remove row
+ A.row(worst_sat_row) = Eigen::Vector4d::Zero().transpose();
+ r(worst_sat_row) = 0; // not necessary
+
/*// Remove selected satellite from problem
std::cout << "resizing problem..." << std::endl;
auto A_aux = A;
diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp
index 0b160f3..989c47e 100644
--- a/test/gtest_tdcp.cpp
+++ b/test/gtest_tdcp.cpp
@@ -22,7 +22,7 @@ void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
Vector3d& sat_ENU,
Vector3d& sat_ECEF,
Vector2d& sat_azel,
- double range)
+ double& range)
{
Vector3d t_ECEF_ENU, t_ENU_ECEF;
Matrix3d R_ENU_ECEF;
@@ -44,13 +44,23 @@ void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
// ECEF
sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * sat_ENU;
+
+ // Range
+ range = (sat_ECEF - t_ECEF_ENU).norm();
}
TEST(Tdcp, Tdcp)
{
+ int N_sats = 20;
+
TdcpBatchParams tdcp_params;
- // TODO: fill params
+ tdcp_params.max_iterations = 10;
+ tdcp_params.min_common_sats = 6;
+ tdcp_params.raim_n = 2;
+ tdcp_params.raim_min_residual = 0;
+ tdcp_params.relinearize_jacobian = true;
+ tdcp_params.tdcp.multi_freq = false;
Vector3d sat_ENU, sat_ECEF;
Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
@@ -58,58 +68,112 @@ TEST(Tdcp, Tdcp)
Vector2d azel, azel2;
Vector4d d, d_gt;
Matrix4d cov_d;
- double residual;
- double range;
-
- Satellite sat_def({0,0,Vector3d::Zero(),Vector3d::Zero(),1.0,0,0,1});
+ double range, residual;
+ // Snapshots
auto snapshot_r = std::make_shared<Snapshot>();
auto snapshot_k = std::make_shared<Snapshot>();
+ // Observations (just needed for the GPST)
+ snapshot_r->setObservations(std::make_shared<Observations>());
+ snapshot_k->setObservations(std::make_shared<Observations>());
+ obsd_t obs_r;
+ obs_r.time.sec = 0;
+ obs_r.time.time = 0;
+ snapshot_r->getObservations()->addObservation(obs_r);
+ obsd_t obs_k;
+ obs_k.time.sec = 0;
+ obs_k.time.time = 1;
+ snapshot_k->getObservations()->addObservation(obs_k);
+
// Random receiver position
for (auto i = 0; i<100; i++)
{
+ // clear satellites and ranges
+ snapshot_r->getSatellites().clear();
+ snapshot_k->getSatellites().clear();
+ snapshot_r->getRanges().clear();
+ snapshot_k->getRanges().clear();
+
+ // random setup
x_r_LLA = computeRandomReceiverLatLonAlt();
x_r_ECEF = latLonAltToEcef(x_r_LLA);
d_ECEF = Vector3d::Random() * 10;
x_k_ECEF = x_r_ECEF + d_ECEF;
x_k_LLA = ecefToLatLonAlt(x_k_ECEF);
- // TODO: random clock biases
- double clock_r, clock_k;
+
+ double clock_r = Vector2d::Random()(0) * 1e-6;
+ double clock_k = Vector2d::Random()(0) * 1e-6;
// groundtruth
d_gt.head<3>() = d_ECEF;
- d_gt(3) = clock_k - clock_r;
+ d_gt(3) = (clock_k - clock_r) * CLIGHT;
+
+ std::cout << "Iteration " << i << ":\n";
+ std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
+ std::cout << "\tclock_r " << clock_r << ":\n";
+ std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
+ std::cout << "\tclock_k " << clock_k << ":\n";
+ std::cout << "\td_gt " << d_gt.transpose() << ":\n";
std::set<int> common_sats;
std::set<int> raim_discarded_sats;
// random visible satellites
- for (auto j = 0; j<10; j++)
+ for (auto j = 0; j<N_sats; j++)
{
common_sats.insert(j);
// Satellite r (random)
computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range);
- EXPECT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-6);
+ ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
snapshot_r->getSatellites().emplace(j, sat_r);
- // TODO: compute range and add random clock bias
+ // Range r
+ Range range_r;
+ range_r.sat = j;
+ range_r.L_corrected = range + CLIGHT * clock_r;
+ range_r.L_var = 1;
+ range_r.L2_corrected = range_r.L_corrected;
+ range_r.L2_var = range_r.L_var;
+
+ snapshot_r->getRanges().emplace(j, range_r);
+
+ std::cout << "\tsat: " << j << "\n";
+ std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
+ std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
// Satellite k (random)
computeRandomVisibleSatellite(x_k_LLA, sat_ENU, sat_ECEF, azel, range);
- EXPECT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-6);
+ ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
snapshot_k->getSatellites().emplace(j, sat_k);
- // TODO: compute range and add random clock bias
+ // Range k
+ Range range_k;
+ range_k.sat = j;
+ range_k.L_corrected = range + CLIGHT * clock_k;
+ range_k.L_var = 1;
+ range_k.L2_corrected = range_k.L_corrected;
+ range_k.L2_var = range_k.L_var;
+
+ snapshot_k->getRanges().emplace(j, range_k);
+
+ std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
+ std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
}
- // TODO: randomly distort 1 satellite -> to be detected by RAIM
+ // Distort one range -> to be detected by RAIM
+ int wrong_sat1 = i % N_sats;
+ snapshot_k->getRanges().at(wrong_sat1).L_corrected += 20;
+ int wrong_sat2 = (i + N_sats / 2) % N_sats;
+ snapshot_r->getRanges().at(wrong_sat2).L_corrected -= 10;
+ // TDCP
+ d = Vector4d::Zero(); // initialize
bool tdcp_ok = Tdcp(snapshot_r,
snapshot_k,
x_r_ECEF,
@@ -120,9 +184,14 @@ TEST(Tdcp, Tdcp)
raim_discarded_sats,
tdcp_params);
- EXPECT_TRUE(tdcp_ok);
- EXPECT_MATRIX_APPROX(d, d_gt, 1e-9);
- EXPECT_LE(residual, 1e-9);
+ // CHECKS
+ std::cout << "CHECKS iteration " << i << std::endl;
+ ASSERT_EQ(raim_discarded_sats.size(), 2);
+ ASSERT_TRUE(raim_discarded_sats.count(wrong_sat1));
+ ASSERT_TRUE(raim_discarded_sats.count(wrong_sat2));
+ ASSERT_TRUE(tdcp_ok);
+ ASSERT_LE(residual, 1e-9);
+ ASSERT_MATRIX_APPROX(d, d_gt, 1e-6);
}
}
--
GitLab