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