From a31a306f89426e500a34b2fe52e76930023b8763 Mon Sep 17 00:00:00 2001 From: joanvallve <jvallve@iri.upc.edu> Date: Mon, 21 Jun 2021 15:07:11 +0200 Subject: [PATCH] sagnac add/substract option --- include/gnss_utils/tdcp.h | 2 +- src/tdcp.cpp | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h index eb32399..de5ef4e 100644 --- a/include/gnss_utils/tdcp.h +++ b/include/gnss_utils/tdcp.h @@ -17,7 +17,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; + int sagnac_correction; // 0 deactivated, 1/2 substraction/addition }; struct TdcpOutput diff --git a/src/tdcp.cpp b/src/tdcp.cpp index a270b8c..110c0f2 100644 --- a/src/tdcp.cpp +++ b/src/tdcp.cpp @@ -246,11 +246,15 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, } // sagnac corrections - if (tdcp_params.sagnac_correction) + if (tdcp_params.sagnac_correction != 0) { double sagnac_corr_r = computeSagnacCorrection(x_r,s_r); //OMGE*(s_r.col(row)(0)*x_r(1)-s_r.col(row)(1)*x_r(0))/CLIGHT; double sagnac_corr_k = computeSagnacCorrection(x_r,s_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 (tdcp_params.sagnac_correction == 1) + drho_m(row) += -sagnac_corr_k + sagnac_corr_r; + else + drho_m(row) -= -sagnac_corr_k + sagnac_corr_r; } #if GNSS_UTILS_TDCP_DEBUG == 1 -- GitLab