diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h index 424e639257bce86c0ab40e002b6308afb97e0129..a543441d7c23fb1aba41b9303072c6ebffc50692 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 dcf37eee70a28a7a37bad753770006508730b900..7f9360477b07bbd520ae370a9e51cdcf483162df 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 d8158e0108411bace868743dd7e3d98978c9d881..5bd42659f13e7546983121838b4381bee882d3ae 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;