diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h index eb323991041bd191b3828f4631a428a72d43a4fe..de5ef4e2335e11fe5804f56f8b3132cd99cf696b 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 a270b8c0f87b5483c4f2623e78ce039393e10de3..110c0f267fc55735def19727780b55a9b44622ad 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