diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h index abcbc78d2bfd5d2c47eeed299f9d155d9c497e69..424e639257bce86c0ab40e002b6308afb97e0129 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 e6503da3ce287c552506e01bf320cff0f959d15e..dcf37eee70a28a7a37bad753770006508730b900 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)