From f99b26892668caf56ed54616f1e5b8101f0a5653 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 28 Apr 2021 23:17:28 +0200
Subject: [PATCH] parameters modified in tdcp batch

---
 include/gnss_utils/tdcp.h |   6 ++-
 src/tdcp.cpp              | 106 ++++++++++++++++++++------------------
 test/gtest_tdcp.cpp       |   6 ++-
 3 files changed, 65 insertions(+), 53 deletions(-)

diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 7005a84..5bcd6ad 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -12,9 +12,10 @@ struct TdcpBatchParams
   TdcpOptions tdcp;
   int    min_common_sats;
   int    raim_n;
-  double raim_min_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;
+  int    residual_opt; // 0: RMS of residual vector. 1: Max residual in Mahalanobis squared distance
 };
 
 struct TdcpOutput
@@ -66,7 +67,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
                 Eigen::VectorXd&        drho_m,
                 Eigen::MatrixXd&        s_k,
                 Eigen::MatrixXd&        s_r,
-                const Eigen::Vector4d&        d_0,
+                const Eigen::Vector4d&  d_0,
+                const double&           var_tdcp,
                 std::set<int>&          raim_discarded_rows,
                 const TdcpBatchParams&  tdcp_params);
 
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index 995489c..23f981a 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -142,12 +142,11 @@ TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
     }
 
     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)
+    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, required_n_sats);
+        printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]", n_common_sats, std::max(tdcp_params.min_common_sats, 4));
         #endif
         output.msg = "Not enough common sats";
         output.success = false;
@@ -277,10 +276,13 @@ TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
             A(row, 3) = -1.0;
         }
     }
+    // Compute TDCP measurement noise variance (proportional to time)
+    double var_tdcp = (tk - tr) * tdcp_params.tdcp.sigma_atm * tdcp_params.tdcp.sigma_atm +
+                       2 * tdcp_params.tdcp.sigma_carrier * tdcp_params.tdcp.sigma_carrier;
 
     // LEAST SQUARES SOLUTION =======================================================================
     std::set<int> raim_discarded_rows;
-    output = Tdcp(x_r, A, r, drho_m, s_k, s_r, d_0, raim_discarded_rows, tdcp_params);
+    output = Tdcp(x_r, A, r, drho_m, s_k, s_r, d_0, var_tdcp, raim_discarded_rows, tdcp_params);
 
     // FILL OUTPUT
     output.used_sats = common_sats;
@@ -291,11 +293,6 @@ TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
     }
     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;
-    output.cov_d *= sq_sigma_Tdcp;
-
     return output;
 }
 
@@ -306,6 +303,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
                 Eigen::MatrixXd&        s_k,
                 Eigen::MatrixXd&        s_r,
                 const Eigen::Vector4d&  d_0,
+                const double&           var_tdcp,
                 std::set<int>&          raim_discarded_rows,
                 const TdcpBatchParams&  tdcp_params)
 {
@@ -376,10 +374,13 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
         }
 
         // residual
-        // residual = sqrt((r - A * delta_d).squaredNorm() / A.rows());
-        residual = (r + A * delta_d).norm();
-        // std::cout << "residual = " << residual << std::endl;
-        // std::cout << "Tdcp2: r-A*delta_d = " << (r + A * delta_d).transpose() << std::endl;
+        if (tdcp_params.residual_opt == 0) // RMSE
+            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;
+        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;
@@ -396,27 +397,32 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
             printf("Tdcp: dT            = %8.3f secs\n", d(3));
         #endif
 
-        // RAIM ====================================== (only at first iteration) RAIM WITH SATELLITES, not ROWS
-        if (j == 0 and tdcp_params.raim_n > 0 and residual > tdcp_params.raim_min_residual)
+        // RAIM ====================================== (at first iteration)
+        if (j == 0 and
+            A.cols() > tdcp_params.min_common_sats and
+            tdcp_params.raim_n > 0 and
+            residual > tdcp_params.max_residual)
         {
             int             worst_sat_row = -1;
             double          best_residual = 1e12;
             Eigen::Vector4d best_d;
 
-            // remove some satellites
-            while (raim_discarded_rows.size() < tdcp_params.raim_n)
+            // 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
+                   residual > tdcp_params.max_residual)
             {
                 auto A_raim = A;
                 auto r_raim = r;
 
-                // solve removing each satellite
+                // 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 satellite row
+                    // remove row
                     A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose();
                     r_raim(row_removed)     = 0;  // not necessary
 
@@ -428,8 +434,13 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
                     if (!d_raim.array().isNaN().any())
                     {
                         // residual
-                        residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
-                        // residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
+                        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;
@@ -469,34 +480,11 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
                 // store removed sat
                 raim_discarded_rows.insert(worst_sat_row);
 
-                // remove row
+                // remove row (set zero)
                 A.row(worst_sat_row) = Eigen::Vector4d::Zero().transpose();
                 r(worst_sat_row)     = 0;  // not necessary
 
-                /*// Remove selected satellite from problem
-                std::cout << "resizing problem..." << std::endl;
-                auto A_aux      = A;
-                auto r_aux      = r;
-                auto drho_m_aux = drho_m;
-                auto s_r_aux    = s_r;
-                auto s_k_aux    = s_k;
-                A.conservativeResize(n_differences, Eigen::NoChange);
-                r.conservativeResize(n_differences);
-                drho_m.conservativeResize(n_differences);
-                s_r.conservativeResize(Eigen::NoChange, n_differences);
-                s_k.conservativeResize(Eigen::NoChange, n_differences);
-
-                int back_elements_changing = A_aux.rows() - worst_sat_row - n_removed_rows;
-                if (back_elements_changing != 0)  // if last satelite removed, conservative resize did the job
-                {
-                    A.bottomRows(back_elements_changing)  = A_aux.bottomRows(back_elements_changing);
-                    s_r.rightCols(back_elements_changing) = s_r_aux.rightCols(back_elements_changing);
-                    s_k.rightCols(back_elements_changing) = s_k_aux.rightCols(back_elements_changing);
-                    r.tail(back_elements_changing)        = r_aux.tail(back_elements_changing);
-                    drho_m.tail(back_elements_changing)   = drho_m_aux.tail(back_elements_changing);
-                }*/
-
-                // apply the RAIM solution
+                // Choose de best RAIM solution
                 d     = best_d;
                 cov_d = (A.transpose() * A).inverse();
             }
@@ -534,15 +522,35 @@ TdcpOutput Tdcp(const Eigen::Vector3d&  x_r,
     }
 
     // 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);
 
-    // residual = (r - A * d).norm() / A.rows();
-    residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
+    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);
+    else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise)
+        residual = r.cwiseAbs2().maxCoeff() / var_tdcp;
+    else
+        throw std::runtime_error("unknown value for residual_opt");
+    //residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
+
+    // check residual condition
+    if (residual > tdcp_params.max_residual)
+    {
+        printf("Tdcp: Didn't success. Final residual bigger than max_residual.");
+        output.msg = "Residual bigger than max_residual";
+        output.success = false;
+    }
+    else
+        output.success = true;
 
-    output.success = true;
+    // covariance
+    cov_d *= var_tdcp;
 
     return output;
 }
diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp
index cdb90c9..f022cd9 100644
--- a/test/gtest_tdcp.cpp
+++ b/test/gtest_tdcp.cpp
@@ -57,9 +57,10 @@ TEST(Tdcp, Tdcp)
     tdcp_params.max_iterations = 10;
     tdcp_params.min_common_sats = 6;
     tdcp_params.raim_n = 0;
-    tdcp_params.raim_min_residual = 0;
+    tdcp_params.max_residual = 0;
     tdcp_params.relinearize_jacobian = true;
     tdcp_params.tdcp.multi_freq = false;
+    tdcp_params.residual_opt = 0;
 
     Vector3d sat_ENU, sat_ECEF;
     Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
@@ -193,9 +194,10 @@ TEST(Tdcp, Tdcp_raim)
     tdcp_params.max_iterations = 10;
     tdcp_params.min_common_sats = 6;
     tdcp_params.raim_n = 2;
-    tdcp_params.raim_min_residual = 0;
+    tdcp_params.max_residual = 0;
     tdcp_params.relinearize_jacobian = true;
     tdcp_params.tdcp.multi_freq = false;
+    tdcp_params.residual_opt = 0;
 
     Vector3d sat_ENU, sat_ECEF;
     Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
-- 
GitLab