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);