From e04020bb4b4320afd862bf16651ef5a4a7f5bd61 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Sat, 12 Jun 2021 15:39:23 +0200
Subject: [PATCH] optional sagnac correction in tdcp

---
 include/gnss_utils/tdcp.h | 1 +
 src/tdcp.cpp              | 8 ++++++++
 2 files changed, 9 insertions(+)

diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index abcbc78..424e639 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -16,6 +16,7 @@ struct TdcpBatchParams
   bool   relinearize_jacobian;
   int    max_iterations;
   int    residual_opt; // 0: Normalized RMS of residual vector. 1: Max residual in Mahalanobis squared distance
+  bool   sagnac_correction;
 };
 
 struct TdcpOutput
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index e6503da..dcf37ee 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -245,6 +245,14 @@ TdcpOutput Tdcp(SnapshotPtr               snapshot_r,
                 drho_m(row) = range_k.L2 - range_r.L2;
         }
 
+        // sagnac corrections
+        if (tdcp_params.sagnac_correction)
+        {
+            double sagnac_corr_r = OMGE*(s_r.col(row)(0)*x_r(1)-s_r.col(row)(1)*x_r(0))/CLIGHT;
+            double sagnac_corr_k = OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT;
+            drho_m(row) += -sagnac_corr_k + sagnac_corr_r;
+        }
+
         #if GNSS_UTILS_TDCP_DEBUG == 1
              std::cout << "\tsat " << sat_number;
              std::cout << std::setprecision(10)
-- 
GitLab