diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h
index dab37126d12f03cfa5eaffb0b26dd5a5c2b225e9..921e69a44cb1e16f082ced9254b93c9a60a4758b 100644
--- a/include/gnss/factor/factor_gnss_pseudo_range.h
+++ b/include/gnss/factor/factor_gnss_pseudo_range.h
@@ -1,6 +1,6 @@
 #ifndef FACTOR_GNSS_PSEUDO_RANGE_H_
 #define FACTOR_GNSS_PSEUDO_RANGE_H_
-#include <type_traits>
+
 //Wolf includes
 #include "core/common/wolf.h"
 #include "core/factor/factor_autodiff.h"
@@ -114,7 +114,7 @@ inline bool FactorGnssPseudoRange::operator ()(const T* const _x,
     */
     if (!sagnac_set_)
     {
-        Eigen::Matrix<T,3,1> antena_ENU = t_ENU_map + t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base + q_map_base * t_base_antena);
+        Eigen::Matrix<T,3,1> antena_ENU = t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base + q_map_base * t_base_antena);
         initSagnac(antena_ENU);
     }
 
@@ -160,7 +160,7 @@ template<>
 void FactorGnssPseudoRange::initSagnac<double>(const Eigen::Matrix<double,3,1>& antena_ENU) const
 {
     Eigen::Vector3d antena_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (antena_ENU - sensor_gnss_ptr_->gettEnuEcef());
-    sagnac_correction_ = OMGE*(satellite_ECEF_(0)*antena_ECEF(1)-satellite_ECEF_(1)*antena_ECEF(0))/CLIGHT;
+    sagnac_correction_ = GnssUtils::computeSagnacCorrection(antena_ECEF, satellite_ECEF_);
     sagnac_set_ = true;
 }
 
diff --git a/include/gnss/factor/factor_gnss_tdcp.h b/include/gnss/factor/factor_gnss_tdcp.h
index 95f77c76a880d92d6a7f5a5fc731c82d66828b53..f780e910eac982c0957fa44b65a7d97deabbe663 100644
--- a/include/gnss/factor/factor_gnss_tdcp.h
+++ b/include/gnss/factor/factor_gnss_tdcp.h
@@ -17,7 +17,8 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
 {
     protected:
         SensorGnssPtr sensor_gnss_ptr_;
-        double d_pseudo_range_;
+        mutable double d_pseudo_range_;
+        mutable bool sagnac_set_;
         double std_dev_;
         Eigen::Vector3d satellite_ENU_k_, satellite_ENU_r_;
 
@@ -52,6 +53,7 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
                                                                                _sensor_gnss_ptr->getEnuMapPitch(),
                                                                                _sensor_gnss_ptr->getEnuMapYaw()),
             sensor_gnss_ptr_(_sensor_gnss_ptr),
+            sagnac_set_(false),
             std_dev_(_std_dev),
             satellite_ENU_k_(sensor_gnss_ptr_->getREnuEcef() * _ftr_k->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef()),
             satellite_ENU_r_(sensor_gnss_ptr_->getREnuEcef() * _ftr_r->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef())
@@ -81,6 +83,9 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
                          const T* const _yaw_ENU_map,
                          T* _residual) const;
 
+        template<typename T>
+        void initSagnac(const Eigen::Matrix<T,3,1>& antena_r_ENU, const Eigen::Matrix<T,3,1>& antena_k_ENU) const;
+
 };
 
 template<typename T>
@@ -139,9 +144,19 @@ inline bool FactorGnssTdcp::operator ()(const T* const _x_r,
     //std::cout << "\texp = " <<  exp << "\n";
     //std::cout << "\terror = " << exp - d_pseudo_range_ << "\n";
 
-    // Residual
-    //_residual[0] = (exp - d_pseudo_range_) / std_dev_;
+    /* SAGNAC EFFECT CORRECTION
+    * It should be recomputed with the new antenna position in ECEF coordinates,
+    * but it is multiplied by a factor of 1e-14 (earth rotation / CLIGHT).
+    * We use instead a precomputed sagnac effect taking the first values of antenna position
+    */
+    if (not sagnac_set_)
+    {
+        Eigen::Matrix<T,3,1> antena_r_ENU = t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base_r + q_map_base_r * t_base_antena);
+        Eigen::Matrix<T,3,1> antena_k_ENU = t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base_k + q_map_base_k * t_base_antena);
+        initSagnac(antena_r_ENU, antena_k_ENU);
+    }
 
+    // Residual
     _residual[0] = ((t_ENU_map + R_ENU_map * (t_map_base_k + q_map_base_k * t_base_antena)-satellite_ENU_k_.cast<T>()).norm()
                       + _clock_bias_k[0]
                       - (t_ENU_map + R_ENU_map * (t_map_base_r + q_map_base_r * t_base_antena)-satellite_ENU_r_.cast<T>()).norm()
@@ -151,6 +166,27 @@ inline bool FactorGnssTdcp::operator ()(const T* const _x_r,
     return true;
 }
 
+template<typename T>
+void FactorGnssTdcp::initSagnac(const Eigen::Matrix<T,3,1>& antena_r_ENU, const Eigen::Matrix<T,3,1>& antena_k_ENU) const
+{
+}
+
+template<>
+void FactorGnssTdcp::initSagnac<double>(const Eigen::Matrix<double,3,1>& antena_r_ENU, const Eigen::Matrix<double,3,1>& antena_k_ENU) const
+{
+    Eigen::Vector3d antena_r_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (antena_r_ENU - sensor_gnss_ptr_->gettEnuEcef());
+    Eigen::Vector3d sat_r_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (satellite_ENU_r_ - sensor_gnss_ptr_->gettEnuEcef());
+    double sagnac_corr_r = GnssUtils::computeSagnacCorrection(antena_r_ECEF, sat_r_ECEF);
+
+    Eigen::Vector3d antena_k_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (antena_k_ENU - sensor_gnss_ptr_->gettEnuEcef());
+    Eigen::Vector3d sat_k_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (satellite_ENU_k_ - sensor_gnss_ptr_->gettEnuEcef());
+    double sagnac_corr_k = GnssUtils::computeSagnacCorrection(antena_k_ECEF, sat_k_ECEF);
+
+    d_pseudo_range_ += -sagnac_corr_k + sagnac_corr_r;
+
+    sagnac_set_ = true;
+}
+
 } // namespace wolf
 
 #endif