From c31b219dd77ea200fcf2cb72d7f6c2b6724b1981 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Mon, 29 Mar 2021 15:07:14 +0200
Subject: [PATCH] struct for tdcp output
---
include/gnss_utils/tdcp.h | 79 ++++++++++----------
src/tdcp.cpp | 152 +++++++++++++++++++-------------------
test/gtest_tdcp.cpp | 53 ++++++-------
3 files changed, 140 insertions(+), 144 deletions(-)
diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index c582301..dadb938 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -17,48 +17,49 @@ struct TdcpBatchParams
int max_iterations;
};
-bool Tdcp(SnapshotPtr snapshot_r,
- SnapshotPtr snapshot_k,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- const std::set<int>& discarded_sats,
- std::set<int>& raim_discarded_sats,
- const TdcpBatchParams& tdcp_params,
- const Options& opt);
+struct TdcpOutput
+{
+ bool success = false;
+ std::string msg = "";
+ std::set<int> raim_discarded_sats;
+ std::set<int> used_sats;
+ Eigen::Vector4d d = Eigen::Vector4d::Zero();
+ Eigen::Matrix4d cov_d = Eigen::Matrix4d::Zero();
+ double dt = 0;
+ double residual = 0;
+};
+
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const TdcpBatchParams& tdcp_params,
+ const Options& opt);
-bool Tdcp(SnapshotPtr snapshot_r,
- SnapshotPtr snapshot_k,
- const Eigen::Vector3d& x_r,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- const std::set<int>& discarded_sats,
- std::set<int>& raim_discarded_sats,
- 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 TdcpBatchParams& tdcp_params,
+ const Options& opt);
-bool Tdcp(SnapshotPtr snapshot_r,
- SnapshotPtr snapshot_k,
- const Eigen::Vector3d& x_r,
- 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);
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const std::set<int>& common_sats,
+ const Eigen::Vector4d& d_0,
+ const TdcpBatchParams& tdcp_params);
-bool Tdcp(const Eigen::Vector3d& x_r,
- Eigen::Matrix<double, Eigen::Dynamic, 4>& A,
- Eigen::VectorXd& r,
- Eigen::VectorXd& drho_m,
- Eigen::Matrix<double, 3, Eigen::Dynamic>& s_k,
- Eigen::Matrix<double, 3, Eigen::Dynamic>& s_r,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- std::set<int>& raim_discarded_sats,
- const TdcpBatchParams& tdcp_params);
+TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
+ Eigen::MatrixXd& A,
+ Eigen::VectorXd& r,
+ Eigen::VectorXd& drho_m,
+ Eigen::MatrixXd& s_k,
+ Eigen::MatrixXd& s_r,
+ const Eigen::Vector4d& d_0,
+ std::set<int>& raim_discarded_rows,
+ const TdcpBatchParams& tdcp_params);
} // namespace GnssUtils
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index 75788b0..3c91462 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -6,38 +6,29 @@
namespace GnssUtils
{
-bool Tdcp(SnapshotPtr snapshot_r,
- SnapshotPtr snapshot_k,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- const std::set<int>& discarded_sats,
- std::set<int>& raim_discarded_sats,
- const TdcpBatchParams& tdcp_params,
- const Options& opt)
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector4d& d_0,
+ const std::set<int>& discarded_sats,
+ const TdcpBatchParams& tdcp_params,
+ const Options& opt)
{
return Tdcp(snapshot_r,
snapshot_k,
computePos(*snapshot_r->getObservations(), *snapshot_r->getNavigation(), opt).pos,
- d,
- cov_d,
- residual,
+ d_0,
discarded_sats,
- raim_discarded_sats,
tdcp_params,
opt);
}
-bool Tdcp(SnapshotPtr snapshot_r,
- SnapshotPtr snapshot_k,
- const Eigen::Vector3d& x_r,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- const std::set<int>& discarded_sats,
- std::set<int>& raim_discarded_sats,
- 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 TdcpBatchParams& tdcp_params,
+ const Options& opt)
{
// If use old nav temporary change navigation to (re)compute satellites positions
auto nav_k = snapshot_k->getNavigation();
@@ -68,15 +59,12 @@ bool Tdcp(SnapshotPtr snapshot_r,
snapshot_k->getRanges());
// COMPUTE TDCP
- bool tdcp_ok = Tdcp(snapshot_r,
- snapshot_k,
- x_r,
- common_sats,
- d,
- cov_d,
- residual,
- raim_discarded_sats,
- tdcp_params);
+ TdcpOutput output = Tdcp(snapshot_r,
+ snapshot_k,
+ x_r,
+ common_sats,
+ d_0,
+ tdcp_params);
// UNDO temporary change navigation
if (tdcp_params.tdcp.use_old_nav)
@@ -85,25 +73,19 @@ bool Tdcp(SnapshotPtr snapshot_r,
snapshot_k->computeSatellites(opt.sateph);
}
- return tdcp_ok;
+ return output;
}
-bool Tdcp(SnapshotPtr snapshot_r,
- SnapshotPtr snapshot_k,
- const Eigen::Vector3d& x_r,
- 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)
+TdcpOutput Tdcp(SnapshotPtr snapshot_r,
+ SnapshotPtr snapshot_k,
+ const Eigen::Vector3d& x_r,
+ const std::set<int>& common_sats,
+ const Eigen::Vector4d& d_0,
+ const TdcpBatchParams& tdcp_params)
{
- // CHECKS
- if (not raim_discarded_sats.empty())
- {
- std::cout << "TDCP: 'raim_discarded_sats' is just an output param, it will be cleared.\n";
- raim_discarded_sats.clear();
- }
+
+ TdcpOutput output;
+
assert(snapshot_r->satellitesComputed() && "this TDCP assumes satellites already computed");
assert(snapshot_k->satellitesComputed() && "this TDCP assumes satellites already computed");
assert(snapshot_r->rangesComputed() && "this TDCP assumes ranges already computed");
@@ -120,7 +102,6 @@ bool Tdcp(SnapshotPtr snapshot_r,
assert(snapshot_k->getRanges().count(sat) && "common sat not stored in ranges");
}
-
int n_common_sats = common_sats.size();
int required_n_sats(std::max(tdcp_params.min_common_sats + tdcp_params.raim_n, 4 + tdcp_params.raim_n));
if (n_common_sats < required_n_sats)
@@ -129,7 +110,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
printf("Tdcp: NOT ENOUGH COMMON SATS");
printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]", n_common_sats, required_n_sats);
#endif
- return false;
+ output.msg = "Not enough common sats";
+ output.success = false;
+ return output;
}
// Times
@@ -171,11 +154,11 @@ bool Tdcp(SnapshotPtr snapshot_r,
#endif
// FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
- Eigen::Matrix<double, Eigen::Dynamic, 4> A(Eigen::Matrix<double, Eigen::Dynamic, 4>::Zero(n_differences, 4));
- Eigen::VectorXd r(Eigen::VectorXd::Zero(n_differences));
- Eigen::VectorXd drho_m(Eigen::VectorXd::Zero(n_differences));
- Eigen::Matrix<double, 3, Eigen::Dynamic> s_k(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
- Eigen::Matrix<double, 3, Eigen::Dynamic> s_r(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
+ Eigen::MatrixXd A(Eigen::MatrixXd::Zero(n_differences, 4));
+ Eigen::VectorXd r(Eigen::VectorXd::Zero(n_differences));
+ Eigen::VectorXd drho_m(Eigen::VectorXd::Zero(n_differences));
+ Eigen::MatrixXd s_k(Eigen::MatrixXd::Zero(3, n_differences));
+ Eigen::MatrixXd s_r(Eigen::MatrixXd::Zero(3, n_differences));
for (auto row_sat_freq_it : row_2_sat_freq)
{
@@ -209,7 +192,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
if (!tdcp_params.relinearize_jacobian)
{
// Unit vectors from receivers to satellites
- Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
+ Eigen::Vector3d u_k = (s_k.col(row) - x_r - d_0.head(3)).normalized();
// Fill Jacobian matrix
A(row, 0) = u_k(0);
@@ -221,34 +204,47 @@ bool Tdcp(SnapshotPtr snapshot_r,
// LEAST SQUARES SOLUTION =======================================================================
std::set<int> raim_discarded_rows;
- bool result = Tdcp(x_r, A, r, drho_m, s_k, s_r, d, cov_d, residual, raim_discarded_rows, tdcp_params);
+ output = Tdcp(x_r, A, r, drho_m, s_k, s_r, d_0, raim_discarded_rows, tdcp_params);
+ // FILL OUTPUT
+ output.used_sats = common_sats;
for (auto disc_row : raim_discarded_rows)
- raim_discarded_sats.insert(row_2_sat_freq[disc_row].first);
+ {
+ output.raim_discarded_sats.insert(row_2_sat_freq[disc_row].first);
+ output.used_sats.erase(row_2_sat_freq[disc_row].first);
+ }
+ output.dt = tk - tr;
// weight covariance with the measurement noise (proportional to time)
double sq_sigma_Tdcp = (tk - tr) * tdcp_params.tdcp.sigma_atm * tdcp_params.tdcp.sigma_atm +
2 * tdcp_params.tdcp.sigma_carrier * tdcp_params.tdcp.sigma_carrier;
- cov_d *= sq_sigma_Tdcp;
+ output.cov_d *= sq_sigma_Tdcp;
- return result;
+ return output;
}
-bool Tdcp(const Eigen::Vector3d& x_r,
- Eigen::Matrix<double, Eigen::Dynamic, 4>& A,
- Eigen::VectorXd& r,
- Eigen::VectorXd& drho_m,
- Eigen::Matrix<double, 3, Eigen::Dynamic>& s_k,
- Eigen::Matrix<double, 3, Eigen::Dynamic>& s_r,
- Eigen::Vector4d& d,
- Eigen::Matrix4d& cov_d,
- double& residual,
- std::set<int>& raim_discarded_rows,
- const TdcpBatchParams& tdcp_params)
+TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
+ Eigen::MatrixXd& A,
+ Eigen::VectorXd& r,
+ Eigen::VectorXd& drho_m,
+ Eigen::MatrixXd& s_k,
+ Eigen::MatrixXd& s_r,
+ const Eigen::Vector4d& d_0,
+ std::set<int>& raim_discarded_rows,
+ const TdcpBatchParams& tdcp_params)
{
+ assert(A.cols() == 4);
+ assert(s_k.rows() == 3);
+ assert(s_r.rows() == 3);
+
+ TdcpOutput output;
+ Eigen::Vector4d& d(output.d);
+ Eigen::Matrix4d& cov_d(output.cov_d);
+ double& residual(output.residual);
// Initial guess
- Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
+ Eigen::Vector4d delta_d(Eigen::Vector4d::Zero());
+ d = d_0;
for (int j = 0; j < tdcp_params.max_iterations; j++)
{
@@ -298,7 +294,9 @@ bool Tdcp(const Eigen::Vector3d& x_r,
std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl;
printf("Tdcp: dT = %8.3f secs\n", d(3));
#endif
- return false;
+ output.msg = "NaN values in NLS";
+ output.success = false;
+ return output;
}
// residual
@@ -387,7 +385,9 @@ bool Tdcp(const Eigen::Vector3d& x_r,
if (worst_sat_row == -1)
{
printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
- return false;
+ output.msg = "NaN values in NLS after RAIM";
+ output.success = false;
+ return output;
}
// store removed sat
@@ -466,7 +466,9 @@ bool Tdcp(const Eigen::Vector3d& x_r,
// residual = (r - A * d).norm() / A.rows();
residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
- return true;
+ output.success = true;
+
+ return output;
}
diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp
index 7120539..1338b7e 100644
--- a/test/gtest_tdcp.cpp
+++ b/test/gtest_tdcp.cpp
@@ -65,7 +65,7 @@ TEST(Tdcp, Tdcp)
Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
Matrix3d R_ENU_ECEF;
Vector2d azel, azel2;
- Vector4d d, d_gt;
+ Vector4d d_gt;
Matrix4d cov_d;
double range, residual;
@@ -166,22 +166,18 @@ TEST(Tdcp, Tdcp)
}
// TDCP
- d = Vector4d::Zero(); // initialize
- bool tdcp_ok = Tdcp(snapshot_r,
- snapshot_k,
- x_r_ECEF,
- common_sats,
- d,
- cov_d,
- residual,
- raim_discarded_sats,
- tdcp_params);
+ auto tdcp_out = Tdcp(snapshot_r,
+ snapshot_k,
+ x_r_ECEF,
+ common_sats,
+ Vector4d::Zero(),
+ tdcp_params);
// CHECKS
std::cout << "CHECKS iteration " << i << std::endl;
- ASSERT_TRUE(tdcp_ok);
- ASSERT_LE(residual, 1e-9);
- ASSERT_MATRIX_APPROX(d, d_gt, 1e-6);
+ ASSERT_TRUE(tdcp_out.success);
+ ASSERT_LE(tdcp_out.residual, 1e-9);
+ ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6);
}
}
@@ -307,26 +303,23 @@ TEST(Tdcp, Tdcp_raim)
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,
- common_sats,
- d,
- cov_d,
- residual,
- raim_discarded_sats,
- tdcp_params);
+ auto tdcp_out = Tdcp(snapshot_r,
+ snapshot_k,
+ x_r_ECEF,
+ common_sats,
+ Vector4d::Zero(),
+ tdcp_params);
// 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);
+ ASSERT_EQ(tdcp_out.raim_discarded_sats.size(), 2);
+ ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat1));
+ ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat2));
+ ASSERT_TRUE(tdcp_out.success);
+ ASSERT_LE(tdcp_out.residual, 1e-9);
+ ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6);
}
}
--
GitLab