From 9fb7e462e40af746803095793a969cc7a4513582 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 29 Apr 2021 12:48:17 +0200
Subject: [PATCH] 2 raim methods implemented and gtest working
---
include/gnss_utils/tdcp.h | 2 +-
src/tdcp.cpp | 195 +++++++++++++++++----------
test/gtest_tdcp.cpp | 269 +++++++++++++++++++++++++++++++-------
3 files changed, 349 insertions(+), 117 deletions(-)
diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 5bcd6ad..abcbc78 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -15,7 +15,7 @@ struct TdcpBatchParams
double max_residual; // max allowed residual to be considered good solution. RAIM applied if enabled in this case.
bool relinearize_jacobian;
int max_iterations;
- int residual_opt; // 0: RMS of residual vector. 1: Max residual in Mahalanobis squared distance
+ int residual_opt; // 0: Normalized RMS of residual vector. 1: Max residual in Mahalanobis squared distance
};
struct TdcpOutput
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index 23f981a..e6503da 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -145,8 +145,8 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
if (n_common_sats < std::max(tdcp_params.min_common_sats, 4))
{
#if GNSS_UTILS_TDCP_DEBUG == 1
- printf("Tdcp: NOT ENOUGH COMMON SATS");
- printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]", n_common_sats, std::max(tdcp_params.min_common_sats, 4));
+ printf("Tdcp: NOT ENOUGH COMMON SATS\n");
+ printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]\n", n_common_sats, std::max(tdcp_params.min_common_sats, 4));
#endif
output.msg = "Not enough common sats";
output.success = false;
@@ -202,7 +202,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
- std::cout << "d initial guess: " << d.transpose() << std::endl;
+ std::cout << "d initial guess: " << d_0.transpose() << std::endl;
std::cout << "common sats: ";
for (auto row_sat_freq_it : row_2_sat_freq)
std::cout << row_sat_freq_it.second.first << " ";
@@ -368,63 +368,130 @@ TdcpOutput 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
- output.msg = "NaN values in NLS";
+ output.msg = "Na"
+ "N values in NLS";
output.success = false;
return output;
}
// residual
- if (tdcp_params.residual_opt == 0) // RMSE
- residual = sqrt((r - A * delta_d).squaredNorm() / A.rows()) / sqrt(var_tdcp);
+ if (tdcp_params.residual_opt == 0) // RMSE normalized
+ residual = sqrt((r + A * delta_d).squaredNorm() / A.rows()) / sqrt(var_tdcp);
else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise)
- residual = (r - A * delta_d).cwiseAbs2().maxCoeff() / var_tdcp;
+ residual = (r + A * delta_d).cwiseAbs2().maxCoeff() / var_tdcp;
else
throw std::runtime_error("unknown value for residual_opt");
// residual = (r + A * delta_d).norm();
#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: residual = %13.10f\n", residual);
- 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));
+ if (j == 0)
+ {
+ std::cout << "\n//////////////// Initial solution at first iteration ////////////////\n";
+ 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: residual = %13.10f\n", residual);
+ printf("Tdcp: rows = %lu\n", r.rows());
+ std::cout << "Tdcp: r = " << r.transpose() << std::endl;
+ std::cout << "Tdcp: r+A*delta_d = " << (r + A * delta_d).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));
+ std::cout << "Check RAIM conditions...\n";
+ std::cout << "\titeration == 0: " << (j==0 ? "OK\n":"FAILED\n");
+ std::cout << "\tA.rows() > tdcp_params.min_common_sats: " << (A.rows() > tdcp_params.min_common_sats ? "OK\n":"FAILED\n");
+ std::cout << "\ttdcp_params.raim_n > 0: " << (tdcp_params.raim_n > 0 ? "OK\n":"FAILED\n");
+ std::cout << "\tresidual > tdcp_params.max_residual: " << (residual > tdcp_params.max_residual ? "OK\n":"FAILED\n");
+ }
#endif
- // RAIM ====================================== (at first iteration)
+ // RAIM ====================================== (after first iteration)
if (j == 0 and
- A.cols() > tdcp_params.min_common_sats and
+ A.rows() > tdcp_params.min_common_sats and
tdcp_params.raim_n > 0 and
residual > tdcp_params.max_residual)
{
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "//////////////// Performing RAIM ////////////////\n";
+ #endif
+
int worst_sat_row = -1;
double best_residual = 1e12;
Eigen::Vector4d best_d;
// remove up to 'raim_n' observations (if enough satellites and residual condition holds)
while (raim_discarded_rows.size() < tdcp_params.raim_n and
- A.cols() - raim_discarded_rows.size() > tdcp_params.min_common_sats and
+ A.rows() - raim_discarded_rows.size() > tdcp_params.min_common_sats and
residual > tdcp_params.max_residual)
{
auto A_raim = A;
auto r_raim = r;
- // solve removing each row
- for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
+ // METHOD A: using normalized RMSE residual
+ if (tdcp_params.residual_opt == 0)
+ {
+ // solve removing each row
+ for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
+ {
+ // skip already discarded rows
+ if (raim_discarded_rows.count(row_removed) != 0)
+ continue;
+
+ // remove row
+ A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose();
+ r_raim(row_removed) = 0;
+
+ // solve
+ Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
+ Eigen::Vector4d d_raim = d_0 + delta_d_raim;
+
+ // no NaN
+ if (!d_raim.array().isNaN().any())
+ {
+ // residual
+ if (tdcp_params.residual_opt == 0) // RMSE
+ residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / (A_raim.rows() - raim_discarded_rows.size() - 1)) / sqrt(var_tdcp);
+ else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise)
+ residual = (r_raim + A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp;
+ else
+ throw std::runtime_error("unknown value for residual_opt");
+ // residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
+
+ #if GNSS_UTILS_TDCP_DEBUG == 1
+ std::cout << "RAIM excluding row " << row_removed;// << std::endl;
+ //std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
+ printf("\tresidual = %13.10f\n", residual);
+ //std::cout << "Tdcp: drho = " << r_raim.transpose() << 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
+
+ // store if best residual
+ if (residual < best_residual)
+ {
+ worst_sat_row = row_removed;
+ best_residual = residual;
+ best_d = d_raim;
+ }
+ }
+ // restore initial A and r
+ A_raim.row(row_removed) = A.row(row_removed);
+ r_raim(row_removed) = r(row_removed);
+ }
+ }
+ // METHOD B: using max residual in Mahalanobis distance
+ else
{
- // skip already discarded rows
- if (raim_discarded_rows.count(row_removed) != 0)
- continue;
+ // find index of max residual
+ (r + A * delta_d).cwiseAbs2().maxCoeff(&worst_sat_row);
// remove row
- A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose();
- r_raim(row_removed) = 0; // not necessary
+ A_raim.row(worst_sat_row) = Eigen::Vector4d::Zero().transpose();
+ r_raim(worst_sat_row) = 0;
// solve
Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
@@ -435,43 +502,33 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
{
// residual
if (tdcp_params.residual_opt == 0) // RMSE
- residual = sqrt((r_raim - A_raim * delta_d_raim).squaredNorm() / (A_raim.rows() - raim_discarded_rows.size() - 1)) / sqrt(var_tdcp);
+ residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / (A_raim.rows() - raim_discarded_rows.size() - 1)) / sqrt(var_tdcp);
else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise)
- residual = (r_raim - A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp;
+ residual = (r_raim + A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp;
else
throw std::runtime_error("unknown value for residual_opt");
// residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
#if GNSS_UTILS_TDCP_DEBUG == 1
- 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_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));
+ std::cout << "RAIM excluding row " << worst_sat_row;// << std::endl;
+ //std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
+ printf("\tresidual = %13.10f\n", residual);
+ //std::cout << "Tdcp: drho = " << r_raim.transpose() << 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
- // store if best residual
- if (residual < best_residual)
- {
- worst_sat_row = row_removed;
- best_residual = residual;
- best_d = d_raim;
- }
+ // store as best residual
+ best_residual = residual;
+ best_d = d_raim;
}
- // restore initial A and r
- A_raim.row(row_removed) = A.row(row_removed);
- r_raim(row_removed) = r(row_removed);
}
// No successful RAIM solution
if (worst_sat_row == -1)
{
- printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
+ printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!\n");
output.msg = "NaN values in NLS after RAIM";
output.success = false;
return output;
@@ -482,15 +539,16 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// remove row (set zero)
A.row(worst_sat_row) = Eigen::Vector4d::Zero().transpose();
- r(worst_sat_row) = 0; // not necessary
+ r(worst_sat_row) = 0;
// Choose de best RAIM solution
d = best_d;
cov_d = (A.transpose() * A).inverse();
+ residual = best_residual;
}
#if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "Tdcp After RAIM " << std::endl;
+ std::cout << "//////////////// Best solution after RAIM ////////////////" << std::endl;
std::cout << "\tExcluded rows : ";
for (auto excl_row : raim_discarded_rows)
std::cout << excl_row << " ";
@@ -506,30 +564,27 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
}
#if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "Tdcp iteration " << j << std::endl;
- std::cout << "\tExcluded rows : ";
- for (auto excl_row : raim_discarded_rows)
- std::cout << excl_row << " ";
- std::cout << std::endl;
- std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
- std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
- std::cout << "\tClock offset = " << d(3) << std::endl;
- std::cout << "\tResidual = " << residual << std::endl;
- std::cout << "\tA = \n" << A << std::endl;
- std::cout << "\tH = \n" << A.transpose() * A << std::endl;
- std::cout << "\tcov_d = \n" << cov_d << std::endl;
+ std::cout << "\n//////////////// Final solution at iteration " << j << "////////////////\n";
+ std::cout << "\tExcluded rows : ";
+ for (auto excl_row : raim_discarded_rows)
+ std::cout << excl_row << " ";
+ std::cout << std::endl;
+ std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
+ std::cout << "\tClock offset = " << d(3) << std::endl;
+ std::cout << "\tResidual = " << residual << std::endl;
+ std::cout << "\tA = \n" << A << std::endl;
+ std::cout << "\tH = \n" << A.transpose() * A << std::endl;
+ std::cout << "\tcov_d = \n" << cov_d << std::endl;
#endif
}
// Computing error on measurement space
- std::cout << "r (r-A*delta_d) = " << (r - A * (d-d_0)).transpose() << std::endl;
r.setZero();
for (int row = 0; row < r.size(); row++)
if (raim_discarded_rows.count(row) == 0)
r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
- std::cout << "r (reevaluated) = " << r.transpose() << std::endl;
-
// residual
if (tdcp_params.residual_opt == 0) // RMSE
residual = sqrt(r.squaredNorm() / (r.rows() - raim_discarded_rows.size())) / sqrt(var_tdcp);
@@ -542,7 +597,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// check residual condition
if (residual > tdcp_params.max_residual)
{
- printf("Tdcp: Didn't success. Final residual bigger than max_residual.");
+ printf("Tdcp: Didn't success. Final residual=%f bigger than max_residual=%f.\n", residual, tdcp_params.max_residual);
output.msg = "Residual bigger than max_residual";
output.success = false;
}
diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp
index f022cd9..d8158e0 100644
--- a/test/gtest_tdcp.cpp
+++ b/test/gtest_tdcp.cpp
@@ -7,6 +7,11 @@
using namespace GnssUtils;
using namespace Eigen;
+int N_sats = 20;
+int N_tests = 100;
+double distortion1 = -30;
+double distortion2 = -20;
+
Vector3d computeRandomReceiverLatLonAlt()
{
Vector3d receiver_LLA = Vector3d::Random(); // in [-1, 1]
@@ -18,11 +23,12 @@ Vector3d computeRandomReceiverLatLonAlt()
return receiver_LLA;
}
-void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
- Vector3d& sat_ENU,
- Vector3d& sat_ECEF,
- Vector2d& sat_azel,
- double& range)
+void computeVisibleSatellite(const Vector3d& receiver_latlonalt,
+ const int& n_sat,
+ Vector3d& sat_ENU,
+ Vector3d& sat_ECEF,
+ Vector2d& sat_azel,
+ double& range)
{
Vector3d t_ECEF_ENU, t_ENU_ECEF;
Matrix3d R_ENU_ECEF;
@@ -31,10 +37,21 @@ void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
computeEnuEcefFromLatLonAlt(receiver_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
+ // equidistributed azimuth at elevation 45º
+ if (n_sat < 6)
+ {
+ sat_azel(0) = -M_PI + n_sat * 2*M_PI/6 + 0.1; // in [-pi, pi]
+ sat_azel(1) = M_PI / 4; // in [0, pi/2]
+ }
// random elevation and azimuth
- sat_azel = Vector2d::Random(); // in [-1, 1]
- sat_azel(0) *= M_PI; // in [-pi, pi]
- sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2; // in [0, pi/2]
+ else
+ {
+ sat_azel = Vector2d::Random(); // in [-1, 1]
+ sat_azel(0) *= M_PI; // in [-pi, pi]
+ sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2; // in [0, pi/2]
+ }
+
+ // random range
range = VectorXd::Random(1)(0) * 5e2 + 1e3; // in [500, 1500]
// ENU
@@ -51,16 +68,16 @@ void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
TEST(Tdcp, Tdcp)
{
- int N_sats = 20;
-
TdcpBatchParams tdcp_params;
- tdcp_params.max_iterations = 10;
+ tdcp_params.max_iterations = 5;
tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 0;
- tdcp_params.max_residual = 0;
+ tdcp_params.residual_opt = 0;
+ tdcp_params.max_residual = 1e20;
tdcp_params.relinearize_jacobian = true;
tdcp_params.tdcp.multi_freq = false;
- tdcp_params.residual_opt = 0;
+ tdcp_params.tdcp.sigma_atm = 1;
+ tdcp_params.tdcp.sigma_carrier = 1;
Vector3d sat_ENU, sat_ECEF;
Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
@@ -87,7 +104,7 @@ TEST(Tdcp, Tdcp)
snapshot_k->getObservations()->addObservation(obs_k);
// Random receiver position
- for (auto i = 0; i<100; i++)
+ for (auto i = 0; i<N_tests; i++)
{
// clear satellites and ranges
snapshot_r->getSatellites().clear();
@@ -109,7 +126,7 @@ TEST(Tdcp, Tdcp)
d_gt.head<3>() = d_ECEF;
d_gt(3) = (clock_k - clock_r) * CLIGHT;
- std::cout << "Iteration " << i << ":\n";
+ std::cout << "Test " << 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";
@@ -124,8 +141,8 @@ TEST(Tdcp, Tdcp)
{
common_sats.insert(j);
- // Satellite r (random)
- computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range);
+ // Satellite r
+ computeVisibleSatellite(x_r_LLA, j, sat_ENU, sat_ECEF, azel, range);
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});
@@ -143,12 +160,12 @@ TEST(Tdcp, Tdcp)
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";
+ //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);
+ computeVisibleSatellite(x_k_LLA, j, sat_ENU, sat_ECEF, azel, range);
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});
@@ -166,8 +183,8 @@ TEST(Tdcp, Tdcp)
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";
+ //std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
+ //std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
}
// TDCP
@@ -179,25 +196,28 @@ TEST(Tdcp, Tdcp)
tdcp_params);
// CHECKS
- std::cout << "CHECKS iteration " << i << std::endl;
+ std::cout << "CHECKS Test " << i << std::endl;
ASSERT_TRUE(tdcp_out.success);
- ASSERT_LE(tdcp_out.residual, 1e-9);
- ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6);
+ ASSERT_LE(tdcp_out.residual, 1e-3);
+ ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
}
}
-TEST(Tdcp, Tdcp_raim)
+TEST(Tdcp, Tdcp_raim_residual_rmse)
{
int N_sats = 20;
TdcpBatchParams tdcp_params;
- tdcp_params.max_iterations = 10;
+ tdcp_params.max_iterations = 5;
tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 2;
- tdcp_params.max_residual = 0;
tdcp_params.relinearize_jacobian = true;
+ tdcp_params.residual_opt = 0; // Normalized RMSE
+ tdcp_params.max_residual = 0.1; // low threshold to detect outliers...
tdcp_params.tdcp.multi_freq = false;
- tdcp_params.residual_opt = 0;
+ tdcp_params.tdcp.sigma_atm = 1;
+ tdcp_params.tdcp.sigma_carrier = 1;
+
Vector3d sat_ENU, sat_ECEF;
Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
@@ -224,7 +244,7 @@ TEST(Tdcp, Tdcp_raim)
snapshot_k->getObservations()->addObservation(obs_k);
// Random receiver position
- for (auto i = 0; i<100; i++)
+ for (auto i = 0; i<N_tests; i++)
{
// clear satellites and ranges
snapshot_r->getSatellites().clear();
@@ -246,12 +266,17 @@ TEST(Tdcp, Tdcp_raim)
d_gt.head<3>() = d_ECEF;
d_gt(3) = (clock_k - clock_r) * CLIGHT;
- std::cout << "Iteration " << i << ":\n";
+ // distorted sats
+ int wrong_sat1 = i % N_sats;
+ int wrong_sat2 = (i + N_sats / 2) % N_sats;
+
+ std::cout << "Test " << 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::cout << "\tdistorted sats: " << wrong_sat1 << ", " << wrong_sat2 << std::endl;
std::set<int> common_sats;
std::set<int> raim_discarded_sats;
@@ -262,7 +287,7 @@ TEST(Tdcp, Tdcp_raim)
common_sats.insert(j);
// Satellite r (random)
- computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range);
+ computeVisibleSatellite(x_r_LLA, j, sat_ENU, sat_ECEF, azel, range);
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});
@@ -280,12 +305,12 @@ TEST(Tdcp, Tdcp_raim)
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";
+ //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);
+ computeVisibleSatellite(x_k_LLA, j, sat_ENU, sat_ECEF, azel, range);
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});
@@ -303,18 +328,170 @@ TEST(Tdcp, Tdcp_raim)
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";
+ //std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
+ //std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
}
// Distort 2 ranges -> to be detected by RAIM
+ snapshot_k->getRanges().at(wrong_sat1).L_corrected += distortion1;
+ snapshot_k->getRanges().at(wrong_sat1).L += distortion1;
+ snapshot_r->getRanges().at(wrong_sat2).L_corrected += distortion2;
+ snapshot_r->getRanges().at(wrong_sat2).L += distortion2;
+
+
+ // TDCP
+ 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(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-3);
+ ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
+ }
+}
+
+TEST(Tdcp, Tdcp_raim_residual_max_mah)
+{
+ int N_sats = 20;
+
+ TdcpBatchParams tdcp_params;
+ tdcp_params.max_iterations = 5;
+ tdcp_params.min_common_sats = 6;
+ tdcp_params.raim_n = 2;
+ tdcp_params.relinearize_jacobian = true;
+ tdcp_params.residual_opt = 1; // Max residual in Mahalanobis distance
+ tdcp_params.max_residual = 3.84; // 95% of confidence
+ tdcp_params.tdcp.multi_freq = false;
+ tdcp_params.tdcp.sigma_atm = 1;
+ tdcp_params.tdcp.sigma_carrier = 1;
+
+
+ Vector3d sat_ENU, sat_ECEF;
+ 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;
+ Matrix4d cov_d;
+ 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<N_tests; 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);
+
+ 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) * CLIGHT;
+
+ // distorted sats
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;
+ std::cout << "\nTest " << 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::cout << "\tdistorted sats: " << wrong_sat1 << ", " << wrong_sat2 << std::endl;
+
+ std::set<int> common_sats;
+ std::set<int> raim_discarded_sats;
+
+ // random visible satellites
+ for (auto j = 0; j<N_sats; j++)
+ {
+ common_sats.insert(j);
+
+ // Satellite r (random)
+ computeVisibleSatellite(x_r_LLA, j, sat_ENU, sat_ECEF, azel, range);
+ 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);
+
+ // Range r
+ Range range_r;
+ range_r.sat = j;
+ range_r.L = range + CLIGHT * clock_r;
+ range_r.L_corrected = range_r.L;
+ range_r.L_var = 1;
+ 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);
+
+ //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)
+ computeVisibleSatellite(x_k_LLA, j, sat_ENU, sat_ECEF, azel, range);
+ 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);
+
+ // Range k
+ Range range_k;
+ range_k.sat = j;
+ range_k.L = range + CLIGHT * clock_k;
+ range_k.L_corrected = range_k.L;
+ range_k.L_var = 1;
+ 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);
+
+ //std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
+ //std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
+ }
+
+ // Distort 2 ranges -> to be detected by RAIM
+ snapshot_k->getRanges().at(wrong_sat1).L_corrected += distortion1;
+ snapshot_k->getRanges().at(wrong_sat1).L += distortion1;
+ snapshot_r->getRanges().at(wrong_sat2).L_corrected += distortion2;
+ snapshot_r->getRanges().at(wrong_sat2).L += distortion2;
// TDCP
auto tdcp_out = Tdcp(snapshot_r,
@@ -325,13 +502,13 @@ TEST(Tdcp, Tdcp_raim)
tdcp_params);
// CHECKS
- std::cout << "CHECKS iteration " << i << std::endl;
+ std::cout << "Checks iteration " << i << std::endl;
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);
+ ASSERT_LE(tdcp_out.residual, 1e-3);
+ ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
}
}
--
GitLab