diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index 03fc7a2d446f4b568939e1686ccb021f234f1129..fece2683601328edd6d6f86b0efc5ba64ce36dc8 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -110,10 +110,14 @@ class SensorGnss : public SensorBase
         ~SensorGnss() override;
 
         // Gets
-        StateBlockPtr getEnuMapTranslation() const;
-        StateBlockPtr getEnuMapRoll() const;
-        StateBlockPtr getEnuMapPitch() const;
-        StateBlockPtr getEnuMapYaw() const;
+        StateBlockConstPtr getEnuMapTranslation() const;
+        StateBlockPtr getEnuMapTranslation();
+        StateBlockConstPtr getEnuMapRoll() const;
+        StateBlockPtr getEnuMapRoll();
+        StateBlockConstPtr getEnuMapPitch() const;
+        StateBlockPtr getEnuMapPitch();
+        StateBlockConstPtr getEnuMapYaw() const;
+        StateBlockPtr getEnuMapYaw();
         const Eigen::Matrix3d& getREnuEcef() const;
         const Eigen::Vector3d& gettEnuEcef() const;
         Eigen::Matrix3d getREnuMap() const;
@@ -178,22 +182,42 @@ inline bool SensorGnss::isEnuModeAuto() const
     return params_->ENU_mode == "auto";
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapTranslation() const
+inline StateBlockConstPtr SensorGnss::getEnuMapTranslation() const
 {
     return getStateBlock('t');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapRoll() const
+inline StateBlockPtr SensorGnss::getEnuMapTranslation()
+{
+    return getStateBlock('t');
+}
+
+inline StateBlockConstPtr SensorGnss::getEnuMapRoll() const
+{
+    return getStateBlock('r');
+}
+
+inline StateBlockPtr SensorGnss::getEnuMapRoll()
 {
     return getStateBlock('r');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapPitch() const
+inline StateBlockConstPtr SensorGnss::getEnuMapPitch() const
 {
     return getStateBlock('p');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapYaw() const
+inline StateBlockPtr SensorGnss::getEnuMapPitch()
+{
+    return getStateBlock('p');
+}
+
+inline StateBlockConstPtr SensorGnss::getEnuMapYaw() const
+{
+    return getStateBlock('y');
+}
+
+inline StateBlockPtr SensorGnss::getEnuMapYaw()
 {
     return getStateBlock('y');
 }
diff --git a/src/processor/processor_gnss_tdcp.cpp b/src/processor/processor_gnss_tdcp.cpp
index c087f70747614b859de670898ce9cd9dd655ca6f..2cd1718979c39d5835ed617654b864d14ff72e25 100644
--- a/src/processor/processor_gnss_tdcp.cpp
+++ b/src/processor/processor_gnss_tdcp.cpp
@@ -144,11 +144,11 @@ void ProcessorGnssTdcp::processKeyFrame(FrameBasePtr _keyframe)
     }
 
     // Iterate over all KF of the trajectory with GNSS captures
-    for (auto KF_it = getProblem()->getTrajectory()->rbegin();
-         KF_it != getProblem()->getTrajectory()->rend();
-         KF_it++)
+    for (auto KF_rit = getProblem()->getTrajectory()->getFrameMap().rbegin();
+         KF_rit != getProblem()->getTrajectory()->getFrameMap().rend();
+         KF_rit++)
     {
-        auto KF_ref = *KF_it;
+        auto KF_ref = KF_rit->second;
         if (_keyframe->getTimeStamp() < KF_ref->getTimeStamp())
             continue;
 
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index ace52948ce6e00adc5961081c00ee02fe8760781..1a7aa706f22e4d0e9fe1f0a537d9376c8de937e9 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -410,11 +410,11 @@ void ProcessorTrackerGnss::establishFactors()
             WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id());
             FactorBasePtr last_fac_ptr = nullptr;
 
-            for (auto KF_rit = getProblem()->getTrajectory()->rbegin();
-                 KF_rit != getProblem()->getTrajectory()->rend();
+            for (auto KF_rit = getProblem()->getTrajectory()->getFrameMap().rbegin();
+                 KF_rit != getProblem()->getTrajectory()->getFrameMap().rend();
                  KF_rit++)
             {
-                FrameBasePtr KF = (*KF_rit);
+                FrameBasePtr KF = KF_rit->second;
 
                 WOLF_DEBUG("TDCP BATCH ref frame ", KF->id());
 
@@ -424,7 +424,7 @@ void ProcessorTrackerGnss::establishFactors()
                     continue;
 
                 // static cast
-                auto ref_cap_gnss = std::static_pointer_cast<CaptureGnss>((*KF_rit)->getCaptureOf(getSensor(),"CaptureGnss"));
+                auto ref_cap_gnss = std::static_pointer_cast<CaptureGnss>(KF->getCaptureOf(getSensor(),"CaptureGnss"));
                 auto last_cap_gnss = std::static_pointer_cast<CaptureGnss>(last_ptr_);
 
                 // dt