From 746520d0a5db06d21df92b904e08e99b8b1fb108 Mon Sep 17 00:00:00 2001 From: joanvallve <jvallve@iri.upc.edu> Date: Mon, 14 Jun 2021 11:31:18 +0200 Subject: [PATCH] added optional check of residual --- include/gnss_utils/tdcp.h | 1 + src/tdcp.cpp | 6 ++++-- test/gtest_tdcp.cpp | 6 ++++++ 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h index 424e639..a543441 100644 --- a/include/gnss_utils/tdcp.h +++ b/include/gnss_utils/tdcp.h @@ -12,6 +12,7 @@ struct TdcpBatchParams TdcpOptions tdcp; int min_common_sats; int raim_n; + bool validate_residual; double max_residual; // max allowed residual to be considered good solution. RAIM applied if enabled in this case. bool relinearize_jacobian; int max_iterations; diff --git a/src/tdcp.cpp b/src/tdcp.cpp index dcf37ee..7f93604 100644 --- a/src/tdcp.cpp +++ b/src/tdcp.cpp @@ -492,7 +492,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, } } // METHOD B: using max residual in Mahalanobis distance - else + else if (tdcp_params.residual_opt == 1) { // find index of max residual (r + A * delta_d).cwiseAbs2().maxCoeff(&worst_sat_row); @@ -532,6 +532,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, best_d = d_raim; } } + else + throw std::runtime_error("unknown value for 'residual_opt'"); // No successful RAIM solution if (worst_sat_row == -1) @@ -603,7 +605,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, //residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size())); // check residual condition - if (residual > tdcp_params.max_residual) + if (tdcp_params.validate_residual and residual > tdcp_params.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"; diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp index d8158e0..5bd4265 100644 --- a/test/gtest_tdcp.cpp +++ b/test/gtest_tdcp.cpp @@ -74,6 +74,8 @@ TEST(Tdcp, Tdcp) tdcp_params.raim_n = 0; tdcp_params.residual_opt = 0; tdcp_params.max_residual = 1e20; + tdcp_params.validate_residual = false; + tdcp_params.sagnac_correction = false; tdcp_params.relinearize_jacobian = true; tdcp_params.tdcp.multi_freq = false; tdcp_params.tdcp.sigma_atm = 1; @@ -214,6 +216,8 @@ TEST(Tdcp, Tdcp_raim_residual_rmse) 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.validate_residual = false; + tdcp_params.sagnac_correction = false; tdcp_params.tdcp.multi_freq = false; tdcp_params.tdcp.sigma_atm = 1; tdcp_params.tdcp.sigma_carrier = 1; @@ -369,6 +373,8 @@ TEST(Tdcp, Tdcp_raim_residual_max_mah) 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.validate_residual = false; + tdcp_params.sagnac_correction = false; tdcp_params.tdcp.multi_freq = false; tdcp_params.tdcp.sigma_atm = 1; tdcp_params.tdcp.sigma_carrier = 1; -- GitLab