diff --git a/include/gnss/capture/capture_gnss_fix.h b/include/gnss/capture/capture_gnss_fix.h
index c894d3e7f887ee2deaed7241f9a3fc819f0e8ace..cfbf8900d360798b371e7982bb3084f0d6979ed1 100644
--- a/include/gnss/capture/capture_gnss_fix.h
+++ b/include/gnss/capture/capture_gnss_fix.h
@@ -37,38 +37,66 @@ WOLF_PTR_TYPEDEFS(CaptureGnssFix);
 class CaptureGnssFix : public CaptureBase
 {
     protected:
-        Eigen::Vector3d position_ECEF_; ///< position in ECEF coordinates.
-        Eigen::Matrix3d covariance_ECEF_; ///< Noise of the fix in ECEF coordinates.
+        Eigen::Vector4d fix_ECEF_; ///< position in ECEF coordinates and clock_bias.
+        Eigen::Matrix4d covariance_ECEF_; ///< Noise of the fix in ECEF coordinates.
         TimeStamp ts_GPST_; ///< Time stamp in GPS time
 
     public:
-        CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::Vector3d& _position, const Eigen::Matrix3d& _covariance, bool _ecef_coordinates = true);
+        CaptureGnssFix(const TimeStamp& _ts, 
+                       SensorBasePtr _sensor_ptr, 
+                       const Eigen::Vector4d& _fix,
+                       const Eigen::Matrix4d& _covariance,
+                       bool _ecef_coordinates = true);
         ~CaptureGnssFix() override;
 
-        const Eigen::Vector3d& getPositionEcef() const;
-        const Eigen::Matrix3d& getCovarianceEcef() const;
+        Eigen::Vector4d getFixEcef() const;
+        Eigen::Matrix4d getFixCovarianceEcef() const;
+        double getClockBias() const;
+        double getClockBiasVariance() const;
+        Eigen::Vector3d getPositionEcef() const;
+        Eigen::Matrix3d getPositionCovarianceEcef() const;
         void getPositionAndCovarianceEcef(Eigen::Vector3d& position, Eigen::Matrix3d& cov) const;
-        const TimeStamp& getGpsTimeStamp() const;
+        TimeStamp getGpsTimeStamp() const;
         void setGpsTimeStamp(const TimeStamp& _ts_GPST);
 };
 
-inline const Eigen::Vector3d& CaptureGnssFix::getPositionEcef() const
+inline Eigen::Vector4d CaptureGnssFix::getFixEcef() const
 {
-    return position_ECEF_;
+    return fix_ECEF_;
 }
 
-inline const Eigen::Matrix3d& CaptureGnssFix::getCovarianceEcef() const
+inline Eigen::Matrix4d CaptureGnssFix::getFixCovarianceEcef() const
 {
     return covariance_ECEF_;
 }
 
+inline double CaptureGnssFix::getClockBias() const
+{
+    return fix_ECEF_(3);
+}
+
+inline double CaptureGnssFix::getClockBiasVariance() const
+{
+    return covariance_ECEF_(3,3);
+}
+
+inline Eigen::Vector3d CaptureGnssFix::getPositionEcef() const
+{
+    return fix_ECEF_.head<3>();
+}
+
+inline Eigen::Matrix3d CaptureGnssFix::getPositionCovarianceEcef() const
+{
+    return covariance_ECEF_.topLeftCorner<3,3>();
+}
+
 inline void CaptureGnssFix::getPositionAndCovarianceEcef(Eigen::Vector3d& position, Eigen::Matrix3d& cov) const
 {
-    position = position_ECEF_;
-    cov = covariance_ECEF_;
+    position = fix_ECEF_.head<3>();
+    cov = covariance_ECEF_.topLeftCorner<3,3>();
 }
 
-inline const wolf::TimeStamp& CaptureGnssFix::getGpsTimeStamp() const
+inline wolf::TimeStamp CaptureGnssFix::getGpsTimeStamp() const
 {
     return ts_GPST_;
 }
diff --git a/include/gnss/factor/factor_gnss_fix_2d.h b/include/gnss/factor/factor_gnss_fix_2d.h
index 26a8735be406bfdadb7c62f52dd0c3507adf21ed..0a59456b207a4fa63dc7cd84a70748b72257516e 100644
--- a/include/gnss/factor/factor_gnss_fix_2d.h
+++ b/include/gnss/factor/factor_gnss_fix_2d.h
@@ -96,7 +96,6 @@ inline bool FactorGnssFix2d::operator ()(const T* const _x,
     Eigen::Matrix<T,3,1> t_ENU_ECEF = sensor_gnss_ptr_->gettEnuEcef().cast<T>();
     Eigen::Matrix<T,2,3> R_map_ENU = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]).transpose().topRows(2);
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
-    Eigen::Matrix<T,3,1> fix_ECEF = getMeasurement().cast<T>();
 
     //std::cout << "computeREnuMap():\n" << sensor_gnss_ptr_->computeREnuMap((const double) _roll[0], (const double) _pitch[0], (const double) _yaw[0]).transpose().topRows(2) << std::endl;
     //std::cout << "getREnuEcef():\n" << sensor_gnss_ptr_->gettEnuEcef() << std::endl;
@@ -106,7 +105,7 @@ inline bool FactorGnssFix2d::operator ()(const T* const _x,
     //std::cout << "R_map_ENU:\n\t" << R_map_ENU(0,0) << "\t" << R_map_ENU(0,1) << "\t" << R_map_ENU(0,2) << "\n\t" << R_map_ENU(1,0) << "\t" << R_map_ENU(1,1) << "\t" << R_map_ENU(1,2) << std::endl;
     //std::cout << "R_ENU_ECEF:\n\t" << R_ENU_ECEF(0,0) << "\t" << R_ENU_ECEF(0,1) << "\t" << R_ENU_ECEF(0,2) << "\n\t" << R_ENU_ECEF(1,0) << "\t" << R_ENU_ECEF(1,1) << "\t" << R_ENU_ECEF(1,2) << "\n\t" << R_ENU_ECEF(2,0) << "\t" << R_ENU_ECEF(2,1) << "\t" << R_ENU_ECEF(2,2) << std::endl;
     //std::cout << "t_ENU_ECEF:\n\t" << t_ENU_ECEF(0) << "\n\t" << t_ENU_ECEF(1) << "\n\t" << t_ENU_ECEF(2) << std::endl;
-    Eigen::Matrix<T,2,1> fix_map = R_map_ENU * (R_ENU_ECEF * fix_ECEF + t_ENU_ECEF - t_ENU_map);
+    Eigen::Matrix<T,2,1> fix_map = R_map_ENU * (R_ENU_ECEF * getMeasurement().head<3>() + t_ENU_ECEF - t_ENU_map);
     //std::cout << "fix 3d:\n\t" << getMeasurement()(0) << "\n\t" << getMeasurement()(1) << "\n\t" << getMeasurement()(2) << std::endl;
     //std::cout << "fix_map:\n\t" << fix_map[0] << "\n\t" << fix_map[1] << std::endl;
 
@@ -117,7 +116,7 @@ inline bool FactorGnssFix2d::operator ()(const T* const _x,
     // Compute residual rotating information matrix to 2d Map coordinates
     // Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R'
     // In this case R = R_map_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_map_ENU'
-    residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map);
+    residuals_ECEF = (getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map);
     //std::cout << "residuals_ECEF:\n\t" << _residuals[0] << "\n\t" << _residuals[1] << "\n\t" << _residuals[2]<< std::endl;
 
     return true;
diff --git a/include/gnss/factor/factor_gnss_fix_3d.h b/include/gnss/factor/factor_gnss_fix_3d.h
index 650a1055f52b804bccf1f46352b71b3819a56721..c3b3929e94fedc5db62a509bdb1e5cf826f86978 100644
--- a/include/gnss/factor/factor_gnss_fix_3d.h
+++ b/include/gnss/factor/factor_gnss_fix_3d.h
@@ -95,10 +95,9 @@ inline bool FactorGnssFix3d::operator ()(const T* const _x,
     Eigen::Matrix<T,3,1> t_ENU_ECEF = sensor_gnss_ptr_->gettEnuEcef().cast<T>();
     Eigen::Matrix<T,3,3> R_map_ENU = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]).transpose();
     Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
-    Eigen::Matrix<T,3,1> fix_ECEF = getMeasurement().cast<T>();
 
     // Transform ECEF 3d Fix Feature to Map coordinates
-    Eigen::Matrix<T,3,1> fix_map = R_map_ENU * (R_ENU_ECEF * fix_ECEF + t_ENU_ECEF - t_ENU_map);
+    Eigen::Matrix<T,3,1> fix_map = R_map_ENU * (R_ENU_ECEF * getMeasurement().head<3>() + t_ENU_ECEF - t_ENU_map);
 
     // Antena position in Map coordinates
     Eigen::Matrix<T,3,1> antena_map = q_map_base * t_base_antena + t_map_base;
@@ -106,7 +105,7 @@ inline bool FactorGnssFix3d::operator ()(const T* const _x,
     // Compute residual rotating information matrix in Map coordinates
     // Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R'
     // In this case R = R_map_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_map_ENU'
-    residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map);
+    residuals_ECEF = (getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map);
 
     return true;
 }
diff --git a/include/gnss/factor/factor_gnss_fix_3d_with_clock.h b/include/gnss/factor/factor_gnss_fix_3d_with_clock.h
new file mode 100644
index 0000000000000000000000000000000000000000..fe582423e8f35cc62a928caea6ba3bf74a8a038f
--- /dev/null
+++ b/include/gnss/factor/factor_gnss_fix_3d_with_clock.h
@@ -0,0 +1,116 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#pragma once
+
+//Wolf includes
+#include "core/common/wolf.h"
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+
+namespace wolf {
+    
+WOLF_PTR_TYPEDEFS(FactorGnssFix3dWithClock);
+
+class FactorGnssFix3dWithClock : public FactorAutodiff<FactorGnssFix3dWithClock, 4, 3, 4, 3, 3, 1, 1, 1, 1>
+{
+    protected:
+        SensorGnssPtr sensor_gnss_ptr_;
+
+    public:
+
+        FactorGnssFix3dWithClock(FeatureBasePtr& _ftr_ptr, 
+                                 const SensorGnssPtr& _sensor_gnss_ptr,
+                                 const ProcessorBasePtr& _processor_ptr,
+                                 bool _apply_loss_function,
+                                 FactorStatus _status = FAC_ACTIVE) :
+            FactorAutodiff("FactorGnssFix3dWithClock",
+                           TOP_ABS,
+                           _ftr_ptr,
+                           nullptr,
+                           nullptr,
+                           nullptr,
+                           nullptr,
+                           _processor_ptr,
+                           _apply_loss_function,
+                           _status,
+                           _ftr_ptr->getFrame()->getP(),
+                           _ftr_ptr->getFrame()->getO(),
+                           _sensor_gnss_ptr->getP(),
+                           _sensor_gnss_ptr->getEnuMapTranslation(),
+                           _sensor_gnss_ptr->getEnuMapRoll(),
+                           _sensor_gnss_ptr->getEnuMapPitch(),
+                           _sensor_gnss_ptr->getEnuMapYaw(),
+                           _ftr_ptr->getCapture()->getStateBlock('T')),
+            sensor_gnss_ptr_(_sensor_gnss_ptr)
+        {
+            WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3d factor without initializing ENU");
+        }
+
+        ~FactorGnssFix3dWithClock() override = default;
+
+        template<typename T>
+        bool operator ()(const T* const _x,
+                         const T* const _o,
+                         const T* const _x_antena,
+                         const T* const _t_ENU_map,
+                         const T* const _roll_ENU_map,
+                         const T* const _pitch_ENU_map,
+                         const T* const _yaw_ENU_map,
+                         const T* const _clock_bias,
+                         T* _residuals) const;
+
+};
+
+template<typename T>
+inline bool FactorGnssFix3dWithClock::operator ()(const T* const _x,
+                                                  const T* const _o,
+                                                  const T* const _x_antena,
+                                                  const T* const _t_ENU_map,
+                                                  const T* const _roll_ENU_map,
+                                                  const T* const _pitch_ENU_map,
+                                                  const T* const _yaw_ENU_map,
+                                                  const T* const _clock_bias,
+                                                  T* _residuals) const
+{
+    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<Eigen::Matrix<T,4,1> > residuals(_residuals);
+
+    Eigen::Matrix<T,3,3> R_ECEF_ENU = sensor_gnss_ptr_->getREnuEcef().cast<T>().transpose();
+    Eigen::Matrix<T,3,1> t_ENU_ECEF = sensor_gnss_ptr_->gettEnuEcef().cast<T>();
+    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);
+
+    // Expected antena position in ECEF coordinates
+    Eigen::Matrix<T,4,1> fix_expected;
+    fix_expected.head<3>() = R_ECEF_ENU * (R_ENU_map * (q_map_base * t_base_antena + t_map_base) + t_ENU_map - t_ENU_ECEF);
+    fix_expected(3) = _clock_bias;
+
+    // Compute residual 
+    residuals_ECEF = getMeasurementSquareRootInformationUpper() * (antena_ecef - getMeasurement());
+
+    return true;
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/gnss/feature/feature_gnss_fix.h b/include/gnss/feature/feature_gnss_fix.h
index 60f8debf65e8e60e3d58703611cf7e3f42b5bc84..37d2acf42869fbfc67eed01fe316d9fe2cc96db3 100644
--- a/include/gnss/feature/feature_gnss_fix.h
+++ b/include/gnss/feature/feature_gnss_fix.h
@@ -49,12 +49,14 @@ class FeatureGnssFix : public FeatureBase
         ~FeatureGnssFix() override{};
 
     private:
-      GnssUtils::ComputePosOutput gnss_fix_output_;
+        GnssUtils::ComputePosOutput gnss_fix_output_;
 
 };
 
 inline FeatureGnssFix::FeatureGnssFix(const GnssUtils::ComputePosOutput& _gnss_fix_output) :
-        FeatureBase("FeatureGnssFix", _gnss_fix_output.pos, _gnss_fix_output.pos_covar),
+        FeatureBase("FeatureGnssFix", 
+                    (Eigen::Vector4d() << _gnss_fix_output.pos, _gnss_fix_output.rcv_bias(0)).finished(), 
+                    _gnss_fix_output.sol_cov),
         gnss_fix_output_(_gnss_fix_output)
 {
     //
diff --git a/src/capture/capture_gnss_fix.cpp b/src/capture/capture_gnss_fix.cpp
index 5ddd2bb8a87e12e512a92a7313576f6893463ec3..50a2fa24b3b403170285ea78edf3cbe19e48ab29 100644
--- a/src/capture/capture_gnss_fix.cpp
+++ b/src/capture/capture_gnss_fix.cpp
@@ -20,16 +20,42 @@
 //
 //--------LICENSE_END--------
 #include "gnss/capture/capture_gnss_fix.h"
+#include <core/state_block/state_block_derived.h>
 
 
 namespace wolf {
 
-CaptureGnssFix::CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::Vector3d& _position, const Eigen::Matrix3d& _covariance, bool _ecef_coordinates) :
+CaptureGnssFix::CaptureGnssFix(const TimeStamp& _ts, 
+							   SensorBasePtr _sensor_ptr, 
+							   const Eigen::Vector4d& _fix, 
+							   const Eigen::Matrix4d& _covariance, 
+							   bool _ecef_coordinates) :
 	CaptureBase("CaptureGnssFix", _ts, _sensor_ptr),
-	position_ECEF_(_ecef_coordinates ? _position : GnssUtils::latLonAltToEcef(_position)),
-	covariance_ECEF_(_ecef_coordinates ?_covariance : GnssUtils::enuToEcefCov(_position, _covariance))
+	fix_ECEF_(_fix),
+	covariance_ECEF_(_covariance)
 {
-    //
+	// Convert to ECEF from latlonalt
+	if (not _ecef_coordinates)
+	{
+		fix_ECEF_.head<3>() = GnssUtils::latLonAltToEcef(_fix.head<3>());
+		covariance_ECEF_.topLeftCorner<3,3>() = GnssUtils::enuToEcefCov(_fix.head<3>(), 
+																	    _covariance.topLeftCorner<3,3>());
+	}
+
+    // Clock bias
+    assert(_sensor_ptr->getStateBlock('T') != nullptr and _sensor_ptr->isStateBlockDynamic('T'));
+    addStateBlock('T', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
+
+    // interconstellation clock bias
+    assert(_sensor_ptr->getStateBlock('G') != nullptr);
+    if(_sensor_ptr->isStateBlockDynamic('G'))
+        addStateBlock('G', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
+    assert(_sensor_ptr->getStateBlock('E') != nullptr);
+    if(_sensor_ptr->isStateBlockDynamic('E'))
+        addStateBlock('E', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
+    assert(_sensor_ptr->getStateBlock('M') != nullptr);
+    if(_sensor_ptr->isStateBlockDynamic('M'))
+        addStateBlock('M', std::make_shared<StateParams1>(Vector1d(),true), nullptr);
 }
 
 CaptureGnssFix::~CaptureGnssFix()
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 6066c1f0aea1e80fbfb698fda952de48cd9bf872..de9eed7707736db191d2360ca619b765208934cf 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -139,12 +139,14 @@ FeatureBasePtr ProcessorGnssFix::emplaceFeature(CaptureBasePtr _capture)
     {
         WOLF_DEBUG("ProcessorGnssFix: creating feature from incoming navsatfix. Type:", _capture->getType());
         auto fix_capture = std::static_pointer_cast<CaptureGnssFix>(_capture);
-        pos_output.time = fix_capture->getGpsTimeStamp().getSeconds();           // time_t time;
-        pos_output.sec =  1e-9 * fix_capture->getGpsTimeStamp().getSeconds();    // double sec;
-        pos_output.pos = fix_capture->getPositionEcef();                         // Eigen::Vector3d pos;        // position (m)
-        pos_output.vel = Eigen::Vector3d::Zero();                                // Eigen::Vector3d vel;        // velocity (m/s)
-        pos_output.pos_covar = fix_capture->getCovarianceEcef();                 // Eigen::Matrix3d pos_covar;  // position covariance (m^2)
-        pos_output.type = 0;                                                     // int type;                   // coordinates used (0:xyz-ecef,1:enu-baseline)
+        pos_output.time = fix_capture->getGpsTimeStamp().getSeconds();           // time_t
+        pos_output.sec =  1e-9 * fix_capture->getGpsTimeStamp().getSeconds();    // double
+        pos_output.pos = fix_capture->getPositionEcef();                         // Vector3d - position (m)
+        pos_output.vel = Eigen::Vector3d::Zero();                                // Vector3d - velocity (m/s)
+        pos_output.sol_cov = fix_capture->getFixCovarianceEcef();                // Matrix4d - solution covariance [x, y, z, dt] (m², s²)
+        pos_output.rcv_bias(0) = fix_capture->getClockBias();                    // Vector4d - receiver clock bias to time systems (s) 0: receiver clock bias (s), 1: glo-gps time offset (s), 2: gal-gps time offset (s), 3: bds-gps time offset (s)
+        pos_output.rcv_bias_var(0) = fix_capture->getClockBiasVariance();        // Matrix4d - receiver clock bias variances 0: receiver clock bias (s), 1: glo-gps time offset (s), 2: gal-gps time offset (s), 3: bds-gps time offset (s)
+        pos_output.type = 0;                                                     // int      - coordinates used (0:xyz-ecef,1:enu-baseline)
     }
     else
         throw std::runtime_error("ProcessorGnssFix::emplaceFeature wrong capture type");
@@ -238,7 +240,7 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature)
     if (!sensor_gnss_->isEnuDefined())
     {
         WOLF_INFO("Defining ecef enu with first fix");
-        sensor_gnss_->setEcefEnu(feature->getMeasurement());
+        sensor_gnss_->setEcefEnu(feature->getMeasurement().head<3>());
     }
 
     // Store the first capture that established a factor (for later initialization ENU-MAP)
@@ -246,7 +248,7 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature)
         not sensor_gnss_->isEnuMapFixed())
     {
         first_frame_state_ = feature->getCapture()->getFrame()->getState();
-        first_pos_ = feature->getMeasurement();
+        first_pos_ = feature->getMeasurement().head<3>();
     }
 
     // Initialize ENU-MAP if: ENU defined and ENU-MAP not initialized (and not fixed) and far enough
@@ -255,7 +257,7 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature)
         sensor_gnss_->isEnuDefined() and
         not sensor_gnss_->isEnuMapInitialized() and
         not sensor_gnss_->isEnuMapFixed() and
-        (first_pos_-feature->getMeasurement()).norm() > params_gnss_->enu_map_init_dist_min)
+        (first_pos_-feature->getMeasurement().head<3>()).norm() > params_gnss_->enu_map_init_dist_min)
     {
         assert(first_frame_state_.count('P') and 
                first_frame_state_.count('O') and
@@ -264,14 +266,14 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature)
         sensor_gnss_->initializeEnuMap(first_frame_state_.vector("PO"),
                                        first_pos_,
                                        feature->getCapture()->getFrame()->getState().vector("PO"),
-                                       feature->getMeasurement());
+                                       feature->getMeasurement().head<3>());
         // Set as not-initialized if factors not separated enough ( < enu_map_init_dist_max)
-        if ((first_pos_ - feature->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_max)
+        if ((first_pos_ - feature->getMeasurement().head<3>()).norm() < params_gnss_->enu_map_init_dist_max)
             sensor_gnss_->setEnuMapInitialized(false);
     }
 
     // Fix ENU-MAP if separated enough from first pos ( > enu_map_fix_dist )
-    if ((first_pos_ - feature->getMeasurement()).norm() > params_gnss_->enu_map_fix_dist)
+    if ((first_pos_ - feature->getMeasurement().head<3>()).norm() > params_gnss_->enu_map_fix_dist)
         sensor_gnss_->fixEnuMap();
 }
 
diff --git a/test/gtest_factor_gnss_fix_2d.cpp b/test/gtest_factor_gnss_fix_2d.cpp
index a5a439cfc4d7ec232122da70552d75fecfc3a875..fca8420cebb3aea6705ee24f502e356916ceb800 100644
--- a/test/gtest_factor_gnss_fix_2d.cpp
+++ b/test/gtest_factor_gnss_fix_2d.cpp
@@ -25,7 +25,7 @@
  *  Created on: Aug 1, 2018
  *      \author: jvallve
  */
-
+#include "gnss/internal/config.h"
 #include <core/ceres_wrapper/solver_ceres.h>
 #include <core/utils/utils_gtest.h>
 
@@ -38,16 +38,19 @@
 using namespace Eigen;
 using namespace wolf;
 
-void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr,
+void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr, CaptureBasePtr cap_ptr,
                  const Vector1d& o_enu_map,
                  const Vector3d& t_base_antena,
                  const Vector3d& t_enu_map,
                  const Vector3d& t_map_base,
-                 const Vector1d& o_map_base)
+                 const Vector1d& o_map_base,
+                 const double clock_bias)
 {
+    // clock bias
+    cap_ptr->getStateBlock('T')->setState(Vector1d(clock_bias));
     // ENU-MAP
-    gnss_sensor_ptr->getEnuMapRoll()->setState(Eigen::Vector1d::Zero());
-    gnss_sensor_ptr->getEnuMapPitch()->setState(Eigen::Vector1d::Zero());
+    gnss_sensor_ptr->getEnuMapRoll()->setState(Vector1d::Zero());
+    gnss_sensor_ptr->getEnuMapPitch()->setState(Vector1d::Zero());
     gnss_sensor_ptr->getEnuMapYaw()->setState(o_enu_map);
     gnss_sensor_ptr->getEnuMapTranslation()->setState(t_enu_map);
     // Antena
@@ -106,12 +109,14 @@ Matrix3d R_ecef_enu = (AngleAxis<double>(1.3, Vector3d::UnitX()) *
 Matrix3d R_enu_map  = AngleAxis<double>(o_enu_map(0),  Vector3d::UnitZ()).toRotationMatrix();
 Matrix3d R_map_base = AngleAxis<double>(o_map_base(0), Vector3d::UnitZ()).toRotationMatrix();
 Vector3d t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
+double clock_bias = 0.5;
 
 // WOLF
 ProblemPtr problem_ptr = Problem::create("PO", 2);
 SolverCeresPtr solver_ceres = std::make_shared<SolverCeres>(problem_ptr);
 SensorGnssPtr gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>()));
 FrameBasePtr frame_ptr;
+CaptureGnssFixPtr cap_gnss_ptr;
 
 ////////////////////////////////////////////////////////
 
@@ -138,7 +143,10 @@ TEST(FactorGnssFix2dTest, configure_tree)
     problem_ptr->keyFrameCallback(frame_ptr, nullptr);
 
     // Create & process GNSS Fix capture
-    CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3d::Identity());
+    cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), 
+                                                    gnss_sensor_ptr,
+                                                    (Vector4d() << t_ecef_antena, clock_bias).finished(),
+                                                    1e-3*Matrix4d::Identity());
     gnss_sensor_ptr->process(cap_gnss_ptr);
 
     // Checks
@@ -156,7 +164,7 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_position)
     problem_ptr->print();
     
     // --------------------------- Reset states
-    resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
+    resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias);
 
     // --------------------------- fixing/unfixing states
     fixAllStates(gnss_sensor_ptr, frame_ptr);
@@ -202,7 +210,7 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_position)
 TEST(FactorGnssFix2dTest, gnss_1_map_base_orientation)
 {
     // --------------------------- Reset states
-    resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
+    resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias);
 
     // --------------------------- fixing/unfixing states
     fixAllStates(gnss_sensor_ptr, frame_ptr);
@@ -245,7 +253,7 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_orientation)
 TEST(FactorGnssFix2dTest, gnss_1_enu_map_yaw)
 {
     // --------------------------- Reset states
-    resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
+    resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias);
 
     // --------------------------- fixing/unfixing states
     fixAllStates(gnss_sensor_ptr, frame_ptr);
@@ -291,7 +299,7 @@ TEST(FactorGnssFix2dTest, gnss_1_enu_map_yaw)
 TEST(FactorGnssFix2dTest, gnss_1_enu_map_position)
 {
     // --------------------------- Reset states
-    resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
+    resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias);
 
     // --------------------------- fixing/unfixing states
     fixAllStates(gnss_sensor_ptr, frame_ptr);
@@ -335,7 +343,7 @@ TEST(FactorGnssFix2dTest, gnss_1_enu_map_position)
 TEST(FactorGnssFix2dTest, gnss_1_base_antena)
 {
     // --------------------------- Reset states
-    resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
+    resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias);
 
     // --------------------------- fixing/unfixing states
     fixAllStates(gnss_sensor_ptr, frame_ptr);