diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h index 6cd6cde2f3c3ff9b1649a18ae7d4b8ed170418d8..818fe3fb735047f0c2a7d373fb59a121ef7371ed 100644 --- a/include/gnss_utils/tdcp.h +++ b/include/gnss_utils/tdcp.h @@ -54,7 +54,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, TdcpOutput Tdcp(const Eigen::Vector3d& x_r, Eigen::MatrixXd& A, Eigen::VectorXd& r, - const Eigen::VectorXd& drho_m, + Eigen::VectorXd& drho_m, const Eigen::MatrixXd& s_k, const Eigen::MatrixXd& s_r, const Eigen::Vector4d& d_0, diff --git a/src/tdcp.cpp b/src/tdcp.cpp index e2e48d91049edbb17dba5ae4fe35746440b37c0e..44b0f247fffc0cf5136050d46417da190b525c84 100644 --- a/src/tdcp.cpp +++ b/src/tdcp.cpp @@ -221,12 +221,9 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, if (tdcp_params.sagnac_correction != 0) { double sagnac_corr_r = computeSagnacCorrection(x_r,s_r.col(row)); //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.col(row)); //OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT; + double sagnac_corr_k = computeSagnacCorrection(x_r+d_0.head(3),s_k.col(row)); //OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT; - 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; + drho_m(row) += -sagnac_corr_k + sagnac_corr_r; } #if GNSS_UTILS_TDCP_DEBUG == 1 @@ -275,7 +272,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, TdcpOutput Tdcp(const Eigen::Vector3d& x_r, Eigen::MatrixXd& A, Eigen::VectorXd& r, - const Eigen::VectorXd& drho_m, + Eigen::VectorXd& drho_m, const Eigen::MatrixXd& s_k, const Eigen::MatrixXd& s_r, const Eigen::Vector4d& d_0, @@ -296,6 +293,17 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, Eigen::Vector4d delta_d(Eigen::Vector4d::Zero()); d = d_0; + // initial sagnac correction + Eigen::VectorXd prev_sagnac_corr(s_k.cols()); + if (tdcp_params.sagnac_correction == 2 ) + for (int row = 0; row < A.rows(); row++) + { + double sagnac_corr_r = computeSagnacCorrection(x_r,s_r.col(row)); //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+d_0.head(3),s_k.col(row)); //OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT; + + prev_sagnac_corr(row) = -sagnac_corr_k + sagnac_corr_r; + } + for (int j = 0; j < tdcp_params.max_iterations; j++) { // fill A and r @@ -305,6 +313,21 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, if (raim_discarded_rows.count(row) != 0) continue; + // update sagnac correction + if (tdcp_params.sagnac_correction == 2 and j > 0) + { + // undo prev_correction + drho_m(row) -= prev_sagnac_corr(row); + + // apply new correction + double sagnac_corr_r = computeSagnacCorrection(x_r,s_r.col(row)); //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+d.head(3),s_k.col(row)); //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; + + // store correction + prev_sagnac_corr(row) = -sagnac_corr_k + sagnac_corr_r; + } + // Evaluate r r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);