diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h
index 17aa7734cca56467daa075d31e748a0d014c31c8..a3a8ea1a12f9c323d0efbafeaa6525db6d1ca7ee 100644
--- a/include/gnss/factor/factor_gnss_pseudo_range.h
+++ b/include/gnss/factor/factor_gnss_pseudo_range.h
@@ -101,12 +101,15 @@ inline bool FactorGnssPseudoRange::operator ()(const T* const _x,
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base(_x);
     Eigen::Map<const Eigen::Quaternion<T> > q_map_base(_o);
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
+    Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
+
+    /* NOT EFFICIENT IMPLE;ENTATION
 
     Eigen::Matrix<T,3,3> R_ENU_map = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]);
-    Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
 
     // Antenna position in ENU coordinates
     Eigen::Matrix<T,3,1> antena_ENU = t_ENU_map + R_ENU_map * (t_map_base + q_map_base * t_base_antena);
+    */
 
     /* SAGNAC EFFECT CORRECTION
     * It should be recomputed with the new antenna position in ECEF coordinates,
@@ -114,13 +117,22 @@ inline bool FactorGnssPseudoRange::operator ()(const T* const _x,
     * We use instead a precomputed sagnac effect taking the first value of antenna position
     */
     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);
         initSagnac(antena_ENU);
+    }
 
     // Expected pseudo-range
+    T exp = (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) - satellite_ENU_.cast<T>()).norm() +      // norm
+             sagnac_correction_ +                                // sagnac correction (precomputed)
+             _clock_bias[0] +                                    // receiver clock bias (w.r.t. GPS clock)
+             (not_GPS_ ? _clock_bias_constellation[0] : T(0));   // interconstellation clock bias
+    /*
     T exp = (antena_ENU-satellite_ENU_.cast<T>()).norm() +      // norm
             sagnac_correction_ +                                // sagnac correction (precomputed)
             _clock_bias[0] +                                    // receiver clock bias (w.r.t. GPS clock)
             (not_GPS_ ? _clock_bias_constellation[0] : T(0));   // interconstellation clock bias
+    */
 
     //std::cout << "sat " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sat << std::endl;
     //std::cout << "\tsys " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sys << std::endl;
diff --git a/include/gnss/factor/factor_gnss_tdcp.h b/include/gnss/factor/factor_gnss_tdcp.h
index 0cf18f03d05bfcbf219e17ccc7ee59990cd3f827..f0351efb36cacc90cac29ff218f1213b1434ab04 100644
--- a/include/gnss/factor/factor_gnss_tdcp.h
+++ b/include/gnss/factor/factor_gnss_tdcp.h
@@ -106,16 +106,26 @@ inline bool FactorGnssTdcp::operator ()(const T* const _x_r,
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base_k(_x_k);
     Eigen::Map<const Eigen::Quaternion<T> > q_map_base_k(_o_k);
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
+    Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
 
     Eigen::Matrix<T,3,3> R_ENU_map = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]);
-    Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
 
+    /* INEFFICIENT IMPLEMENTATION
     // Antenna position in ECEF coordinates
     Eigen::Matrix<T,3,1> antena_r_ENU = t_ENU_map + R_ENU_map * (t_map_base_r + q_map_base_r * t_base_antena);
     Eigen::Matrix<T,3,1> antena_k_ENU = t_ENU_map + R_ENU_map * (t_map_base_k + q_map_base_k * t_base_antena);
 
     // Expected tdcp
-    T exp = (antena_k_ENU-satellite_ENU_k_.cast<T>()).norm() + _clock_bias_k[0] - (antena_r_ENU-satellite_ENU_r_.cast<T>()).norm() - _clock_bias_r[0] ;
+    T exp = (antena_k_ENU-satellite_ENU_k_.cast<T>()).norm()
+          + _clock_bias_k[0]
+          - (antena_r_ENU-satellite_ENU_r_.cast<T>()).norm()
+          - _clock_bias_r[0] ;
+    */
+    // Expected tdcp
+    //T exp = (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()
+    //      - _clock_bias_r[0];
 
     //std::cout << "sat_r " << std::static_pointer_cast<FeatureGnssSatellite>(getFeatureOther())->getSatellite().sat << std::endl;
     //std::cout << "sat_k " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sat << std::endl;
@@ -134,7 +144,13 @@ inline bool FactorGnssTdcp::operator ()(const T* const _x_r,
     //std::cout << "\terror = " << exp - d_pseudo_range_ << "\n";
 
     // Residual
-    _residual[0] = (exp - d_pseudo_range_) / std_dev_;
+    //_residual[0] = (exp - d_pseudo_range_) / std_dev_;
+
+    _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()
+                      - _clock_bias_r[0]
+                      - d_pseudo_range_) / std_dev_;
 
     return true;
 }