diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 4565017604fc766e0e797371c16e38e508fbe49f..6f5980d7ced970ffae4aae48459b800545a4158b 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -6,6 +6,25 @@ stages:
   - deploy_ros
 
 ############ YAML ANCHORS ############
+.print_variables_template: &print_variables_definition
+  # Print variables
+  - echo $CI_COMMIT_BRANCH
+  - echo $WOLF_IMU_BRANCH
+  - echo $WOLF_GNSS_BRANCH
+  - echo $WOLF_LASER_BRANCH
+  - echo $WOLF_VISION_BRANCH
+  - echo $WOLF_APRILTAG_BRANCH
+  - echo $WOLF_BODYDYNAMICS_BRANCH
+  - echo $GNSSUTILS_BRANCH
+  - echo $LASERSCANUTILS_BRANCH
+  - echo $DEPLOY_CI_ROS
+  - echo $WOLF_ROS_IMU_BRANCH
+  - echo $WOLF_ROS_GNSS_BRANCH
+  - echo $WOLF_ROS_LASER_BRANCH
+  - echo $WOLF_ROS_VISION_BRANCH
+  - echo $WOLF_ROS_APRILTAG_BRANCH
+  - echo $WOLF_ROS_BODYDYNAMICS_BRANCH
+
 .preliminaries_template: &preliminaries_definition
   ## Install ssh-agent if not already installed, it is required by Docker.
   ## (change apt-get to yum if you use an RPM-based image)
@@ -80,6 +99,7 @@ license_header:
   stage: license
   image: labrobotica/wolf_deps:20.04
   before_script:  
+    - *print_variables_definition
     - *preliminaries_definition
   script: 
     - *license_header_definition
@@ -89,6 +109,7 @@ build_and_test:bionic:
   stage: build_and_test
   image: labrobotica/wolf_deps:18.04
   script:
+    - *print_variables_definition
     - *build_and_test_definition
 
 ############ UBUNTU 20.04 TESTS ############
@@ -96,6 +117,7 @@ build_and_test:focal:
   stage: build_and_test
   image: labrobotica/wolf_deps:20.04
   script:
+    - *print_variables_definition
     - *build_and_test_definition
 
 ############ DEPLOY PLUGINS ANY BRANCY EXCEPT FOR main ############
@@ -117,6 +139,7 @@ deploy_gnss:
     - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
     DEPLOY_CI_ROS: "false"
     GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
   trigger: 
@@ -142,6 +165,7 @@ deploy_laser:
     - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
@@ -154,6 +178,7 @@ deploy_apriltag:
     - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
@@ -166,6 +191,7 @@ deploy_bodydynamics:
     - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
@@ -178,7 +204,7 @@ deploy_imu_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/imu
@@ -190,7 +216,8 @@ deploy_gnss_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
+    GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
@@ -202,7 +229,7 @@ deploy_vision_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/vision
@@ -214,7 +241,8 @@ deploy_laser_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
+    LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
@@ -226,7 +254,8 @@ deploy_apriltag_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
+    WOLF_VISION_BRANCH: main
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
@@ -238,7 +267,8 @@ deploy_bodydynamics_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
+    WOLF_IMU_BRANCH: main
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
diff --git a/demos/hello_wolf/factor_bearing.h b/demos/hello_wolf/factor_bearing.h
index d06ace3a796002e87d33a2e9546ad280bea49416..f790a24839bb6c63f1a09703fe283a6f48c11949 100644
--- a/demos/hello_wolf/factor_bearing.h
+++ b/demos/hello_wolf/factor_bearing.h
@@ -36,18 +36,28 @@ namespace wolf
 
 using namespace Eigen;
 
-class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2>
+class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>
 {
     public:
-        FactorBearing(const LandmarkBasePtr& _landmark_other_ptr,
+        FactorBearing(const FeatureBasePtr& _feature_ptr,
+                      const LandmarkBasePtr& _landmark_other_ptr,
                       const ProcessorBasePtr& _processor_ptr,
                       bool _apply_loss_function, FactorStatus _status) :
-                FactorAutodiff<FactorBearing, 1, 2, 1, 2>("BEARING", TOP_LMK, nullptr, nullptr, nullptr,
-                                                          _landmark_other_ptr, _processor_ptr,
-                                                          _apply_loss_function, _status,
-                                                          getCapture()->getFrame()->getP(),
-                                                          getCapture()->getFrame()->getO(),
-                                                          _landmark_other_ptr->getP())
+                FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>("BEARING", 
+                                                        TOP_LMK, 
+                                                        _feature_ptr,
+                                                        nullptr, 
+                                                        nullptr, 
+                                                        nullptr,
+                                                        _landmark_other_ptr, 
+                                                        _processor_ptr,
+                                                        _apply_loss_function, 
+                                                        _status,
+                                                        _feature_ptr->getCapture()->getFrame()->getP(),
+                                                        _feature_ptr->getCapture()->getFrame()->getO(),
+                                                        _feature_ptr->getCapture()->getSensor()->getP(),
+                                                        _feature_ptr->getCapture()->getSensor()->getO(),
+                                                        _landmark_other_ptr->getP())
         {
             //
         }
@@ -58,9 +68,11 @@ class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2>
         }
 
         template<typename T>
-        bool operator ()(const T* const _p1,
-                         const T* const _o1,
-                         const T* const _p2,
+        bool operator ()(const T* const _p_w_r,
+                         const T* const _o_w_r,
+                         const T* const _p_r_s, // sensor position
+                         const T* const _o_r_s, // sensor orientation
+                         const T* const _lmk,
                          T* _res) const;
 
 };
@@ -73,24 +85,30 @@ namespace wolf
 {
 
 template<typename T>
-inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
-                                                                    const T* const _p2, T* _res) const
+inline bool FactorBearing::operator ()( const T* const _p_w_r, 
+                                        const T* const _o_w_r,
+                                        const T* const _p_r_s, // sensor position
+                                        const T* const _o_r_s, // sensor orientation
+                                        const T* const _lmk, 
+                                        T* _res) const
 {
 
     // 1. produce a transformation matrix to transform from robot frame to world frame
-    Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p1[0], _p1[1]) * Rotation2D<T>(*_o1) ; // Robot frame = robot-to-world transform
+    Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform
+    Transform<T, 2, Isometry> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Robot frame = robot-to-world transform
+    
     // Map input pointers into meaningful Eigen elements
-    Map<const Matrix<T, 2, 1>>      point_w(_p2);
+    Map<const Matrix<T, 2, 1>>      point_w(_lmk);
     Map<const Matrix<T, 1, 1>>      res(_res);
 
-    // 2. Transform world point to robot-referenced point
-    Matrix<T, 2, 1> point_r = H_w_r.inverse() * point_w;
+    // 2. Transform world point to sensor-referenced point
+    Matrix<T, 2, 1> point_s = (H_w_r * H_r_s).inverse() * point_w;
 
     // 3. Get the expected bearing of the point
-    T bearing   = atan2(point_r(1), point_r(0));
+    T bearing   = atan2(point_s(1), point_s(0));
 
     // 4. Get the measured range-and-bearing to the point
-    Matrix<T, 2, 1> range_bearing       = getMeasurement();
+    Matrix<T, 2, 1> range_bearing = getMeasurement().cast<T>();
 
     // 5. Get the bearing error by comparing against the bearing measurement
     T er   = range_bearing(1) - bearing;
@@ -100,7 +118,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
         er -= T(2*M_PI);
 
     // 6. Compute the residual by scaling according to the standard deviation of the bearing part
-    *_res   = er * T(getMeasurementSquareRootInformationUpper()(1,1));
+    *_res   = er * getMeasurementSquareRootInformationUpper()(1,1);
 
     return true;
 }
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index d533ff0da8b627dd0d12ca9a32637956da871af5..1e18985f9a047dea575af0852b24eac6ff6305bd 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -307,6 +307,8 @@ class Factory
         static bool unregisterCreator(const std::string& _type);
         static TypeBasePtr create(const std::string& _type, TypeInput... _input);
         std::string getClass() const;
+        static void printAddress();
+        static void printCallbacks();
 
     private:
         CallbackMap callbacks_;
@@ -321,9 +323,15 @@ class Factory
         void operator=(Factory const&)  = delete;
     private:
         Factory()  { }
-        ~Factory() { }
+        ~Factory();
 };
 
+template<class TypeBase, typename... TypeInput>
+inline Factory<TypeBase, TypeInput...>::~Factory<TypeBase, TypeInput...>()
+{
+    std::cout << " Factory destructor  " << this->getClass() << std::endl;
+}
+
 template<class TypeBase, typename... TypeInput>
 inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string& _type, CreatorCallback createFn)
 {
@@ -358,7 +366,7 @@ inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, T
 template<class TypeBase, typename... TypeInput>
 inline Factory<TypeBase, TypeInput...>& Factory<TypeBase, TypeInput...>::get()
 {
-    static Factory instance_;
+    static Factory<TypeBase, TypeInput...> instance_;
     return instance_;
 }
 
@@ -368,6 +376,21 @@ inline std::string Factory<TypeBase, TypeInput...>::getClass() const
     return "Factory<class TypeBase>";
 }
 
+template<class TypeBase, typename... TypeInput>
+inline void Factory<TypeBase, TypeInput...>::printAddress()
+{
+    std::cout << get().getClass() << " address: " << &get() << std::endl;
+}
+
+template<class TypeBase, typename... TypeInput>
+inline void Factory<TypeBase, TypeInput...>::printCallbacks()
+{
+    std::cout << get().getClass() << " callbacks size: " << get().callbacks_.size() << std::endl;
+    for (auto cb: get().callbacks_){
+        std::cout << "\t" << cb.first << std::endl;
+    }
+}
+
 } // namespace wolf
 
 namespace wolf
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 8d426eacab111c81b435e48e630d39a802e02a6f..8d64663585af0982d7ac3209a3fcb808c753080a 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -154,20 +154,20 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
         CaptureBaseConstPtrList getCapturesOfType(const std::string& type) const;
         CaptureBasePtrList getCapturesOfType(const std::string& type);
 
-        CaptureBaseConstPtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const;
-        CaptureBasePtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr);
+        CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr) const;
+        CaptureBasePtr getCaptureOf(SensorBaseConstPtr _sensor_ptr);
 
-        CaptureBaseConstPtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const;
-        CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type);
+        CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr, const std::string& type) const;
+        CaptureBasePtr getCaptureOf(SensorBasePtr _sensor_ptr, const std::string& type);
         
-        CaptureBaseConstPtrList getCapturesOf(const SensorBasePtr _sensor_ptr) const;
-        CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr);
+        CaptureBaseConstPtrList getCapturesOf(SensorBaseConstPtr _sensor_ptr) const;
+        CaptureBasePtrList getCapturesOf(SensorBasePtr _sensor_ptr);
 
-        FactorBaseConstPtr getFactorOf(const ProcessorBasePtr _processor_ptr) const;
-        FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr);
+        FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr) const;
+        FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr);
         
-        FactorBaseConstPtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const;
-        FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
+        FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const;
+        FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type);
         
         unsigned int getHits() const;
 
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 69e55ea8556dc03a323e0aa62e90d125b93015a0..9dfb7cae3e62adac06662e9414c75764045148e2 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -37,13 +37,11 @@ struct ParamsProcessorBase;
 
 //wolf includes
 #include "core/common/wolf.h"
+#include "core/utils/params_server.h"
 #include "core/frame/frame_base.h"
 #include "core/state_block/state_block.h"
-#include "core/utils/params_server.h"
-#include "core/sensor/factory_sensor.h"
-#include "core/processor/factory_processor.h"
-#include <core/processor/motion_provider.h>
 #include "core/state_block/state_composite.h"
+#include "core/processor/motion_provider.h"
 
 // std includes
 #include <mutex>
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index 9a6e84d9c8949b4c9d5c5e69cdf3ec071d66fa57..d7bd26d00df45c7f14d0ff5ae01966e271af03ef 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -80,7 +80,7 @@ class MotionProvider
         void setStatePriority(int);
 
     public:
-        const StateStructure& getStateStructure ( ) { return state_structure_; };
+        const StateStructure& getStateStructure ( ) const { return state_structure_; };
         void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
         void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr);
 
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index fc93357dd46a948c4d225e982b8ade7bf4682af9..52eab3b6b239922b5afbfe08e84361acd2c0e98e 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -92,6 +92,7 @@ class Buffer
 public:
 
     typedef typename std::map<TimeStamp,T>::iterator Iterator; // buffer iterator
+    typedef typename std::map<TimeStamp,T>::const_iterator ConstIterator; // buffer iterator
 
     Buffer(){};
     ~Buffer(void){};
@@ -101,27 +102,28 @@ public:
     *  Select from the buffer the closest element (w.r.t. time stamp),
     * respecting a defined time tolerances
     */
-    T select(const TimeStamp& _time_stamp, const double& _time_tolerance);
+    T select(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
     
     /**\brief Select an element iterator from the buffer
     *
     *  Select from the buffer the iterator pointing to the closest element (w.r.t. time stamp),
     * respecting a defined time tolerances
     */
+    ConstIterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
     Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
 
-    T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance);
+    T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
     
-    T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance);
+    T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
 
-    T selectFirst();
+    T selectFirst() const;
 
-    T selectLast();
+    T selectLast() const;
 
     /**\brief Buffer size
     *
     */
-    SizeStd size(void);
+    SizeStd size(void) const;
 
     /**\brief Add a element to the buffer
     *
@@ -132,6 +134,7 @@ public:
     *
     * elements are ordered from most recent to oldest
     */
+    const std::map<TimeStamp,T>& getContainer() const;
     std::map<TimeStamp,T>& getContainer();
 
     /**\brief Remove all elements in the buffer with a time stamp older than the specified
@@ -152,7 +155,7 @@ public:
     /**\brief is the buffer empty ?
     *
     */
-    bool empty();
+    bool empty() const;
 
 protected:
     /**\brief Check time tolerance
@@ -160,14 +163,19 @@ protected:
     * Check if the time distance between two time stamps is smaller than
     * the time tolerance.
     */
-    static bool checkTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2, const double& _time_tolerance);
+    static bool checkTimeTolerance(const TimeStamp& _time_stamp1, 
+                                   const TimeStamp& _time_stamp2,
+                                   const double& _time_tolerance);
 
     /**\brief Check time tolerance
     *
     * Check if the time distance between two time stamps is smaller than
     * the minimum time tolerance of the two frames.
     */
-    static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1, const TimeStamp& _time_stamp2, const double& _time_tolerance2);
+    static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
+                                         const double& _time_tolerance1, 
+                                         const TimeStamp& _time_stamp2, 
+                                         const double& _time_tolerance2);
 
 protected:
 
@@ -454,6 +462,42 @@ std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&...
 
 /////////////////////////////////////////////////////////////////////////////////////////
 
+template <typename T>
+typename Buffer<T>::ConstIterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const
+{
+    Buffer<T>::ConstIterator post = container_.upper_bound(_time_stamp);
+
+    bool prev_exists = (post != container_.begin());
+    bool post_exists = (post != container_.end());
+
+    bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
+
+    if (prev_exists)
+    {
+        Buffer<T>::ConstIterator prev = std::prev(post);
+
+        bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
+
+        if (prev_ok && !post_ok)
+            return prev;
+
+        else if (!prev_ok && post_ok)
+            return post;
+
+        else if (prev_ok && post_ok)
+        {
+            if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
+                return post;
+            else
+                return prev;
+        }
+    }
+    else if (post_ok)
+        return post;
+
+    return container_.end();
+}
+
 template <typename T>
 typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance)
 {
@@ -491,12 +535,12 @@ typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_st
 }
 
 template <typename T>
-T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
+T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) const
 {
     if (container_.empty())
         return nullptr;
 
-    Buffer<T>::Iterator it = selectIterator(_time_stamp, _time_tolerance);
+    auto it = selectIterator(_time_stamp, _time_tolerance);
 
     // end is returned from selectIterator if an element of the buffer complying with the time stamp
     // and time tolerance has not been found
@@ -508,7 +552,7 @@ T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
 }
 
 template <typename T>
-T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance)
+T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const
 {
     // There is no element
     if (container_.empty())
@@ -529,7 +573,7 @@ T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time
 
 
 template <typename T>
-T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance)
+T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const
 {
     // There is no element
     if (container_.empty())
@@ -549,7 +593,7 @@ T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_t
 }
 
 template <typename T>
-T Buffer<T>::selectFirst()
+T Buffer<T>::selectFirst() const
 {
     // There is no element
     if (container_.empty())
@@ -560,7 +604,7 @@ T Buffer<T>::selectFirst()
 }
 
 template <typename T>
-T Buffer<T>::selectLast()
+T Buffer<T>::selectLast() const
 {
     // There is no element
     if (container_.empty())
@@ -576,6 +620,12 @@ void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element)
     container_.emplace(_time_stamp, _element);
 }
 
+template <typename T>
+const std::map<TimeStamp,T>& Buffer<T>::getContainer() const
+{
+    return container_;
+}
+
 template <typename T>
 std::map<TimeStamp,T>& Buffer<T>::getContainer()
 {
@@ -589,13 +639,13 @@ inline void Buffer<T>::clear()
 }
 
 template <typename T>
-inline bool Buffer<T>::empty()
+inline bool Buffer<T>::empty() const
 {
     return container_.empty();
 }
 
 template <typename T>
-inline SizeStd Buffer<T>::size(void)
+inline SizeStd Buffer<T>::size(void) const
 {
     return container_.size();
 }
@@ -628,8 +678,8 @@ inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
 
 template <typename T>
 inline bool Buffer<T>::checkTimeTolerance(const TimeStamp& _time_stamp1,
-                                                const TimeStamp& _time_stamp2,
-                                                const double& _time_tolerance)
+                                          const TimeStamp& _time_stamp2,
+                                          const double& _time_tolerance)
 {
     double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
     bool pass = time_diff <= _time_tolerance;
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index d93c9baa367b8ba65374a4b64d21bab194ce1d2a..b0683aba04b3e5d8fe041775199823436c728655 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -136,7 +136,7 @@ class ProcessorTracker : public ProcessorBase
                          ParamsProcessorTrackerPtr _params_tracker);
         ~ProcessorTracker() override;
 
-        StateStructure getStateStructure() const;
+        const StateStructure& getStateStructure() const;
 
         virtual CaptureBaseConstPtr getOrigin() const;
         virtual CaptureBasePtr getOrigin();
@@ -313,7 +313,7 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming()
     return new_features_incoming_;
 }
 
-inline StateStructure ProcessorTracker::getStateStructure ( ) const
+inline const StateStructure& ProcessorTracker::getStateStructure ( ) const
 {
     return state_structure_;
 }
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index f295cdbca724a4e4ee89b8d2aa5c52ae487b537a..aae21b6eb2fa9d5552e8212259299b71e0e0c440 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -230,7 +230,7 @@ CaptureBasePtrList FrameBase::getCapturesOfType(const std::string& type)
     return captures;
 }
 
-CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const
+CaptureBaseConstPtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr, const std::string& type) const
 {
     for (auto capture_ptr : getCaptureList())
         if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
@@ -238,7 +238,7 @@ CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, con
     return nullptr;
 }
 
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type)
+CaptureBasePtr FrameBase::getCaptureOf(SensorBasePtr _sensor_ptr, const std::string& type)
 {
     for (auto capture_ptr : getCaptureList())
         if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
@@ -246,7 +246,7 @@ CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const st
     return nullptr;
 }
 
-CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const
+CaptureBaseConstPtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr) const
 {
     if (_sensor_ptr)
         for (auto capture_ptr : getCaptureList())
@@ -255,7 +255,7 @@ CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr
     return nullptr;
 }
 
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr)
+CaptureBasePtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr)
 {
     if (_sensor_ptr)
         for (auto capture_ptr : getCaptureList())
@@ -264,7 +264,7 @@ CaptureBasePtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr)
     return nullptr;
 }
 
-CaptureBaseConstPtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr) const
+CaptureBaseConstPtrList FrameBase::getCapturesOf(SensorBaseConstPtr _sensor_ptr) const
 {
     CaptureBaseConstPtrList captures;
     for (auto capture_ptr : getCaptureList())
@@ -273,7 +273,7 @@ CaptureBaseConstPtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr
     return captures;
 }
 
-CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
+CaptureBasePtrList FrameBase::getCapturesOf(SensorBasePtr _sensor_ptr)
 {
     CaptureBasePtrList captures;
     for (auto capture_ptr : getCaptureList())
@@ -282,7 +282,7 @@ CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
     return captures;
 }
 
-FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const
+FactorBaseConstPtr FrameBase::getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const
 {
     for (auto factor_ptr : getConstrainedByList())
         if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
@@ -295,7 +295,7 @@ FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr,
     return nullptr;
 }
 
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) 
+FactorBasePtr FrameBase::getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type) 
 {
     for (auto factor_ptr : getConstrainedByList())
         if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
@@ -308,7 +308,7 @@ FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, cons
     return nullptr;
 }
 
-FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) const
+FactorBaseConstPtr FrameBase::getFactorOf(ProcessorBaseConstPtr _processor_ptr) const
 {
     for (auto factor_ptr : getConstrainedByList())
         if (factor_ptr->getProcessor() == _processor_ptr)
@@ -321,7 +321,7 @@ FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr)
     return nullptr;
 }
 
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr)
+FactorBasePtr FrameBase::getFactorOf(ProcessorBasePtr _processor_ptr)
 {
     for (auto factor_ptr : getConstrainedByList())
         if (factor_ptr->getProcessor() == _processor_ptr)
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 88b5f411dc6b718cab9f5e1a2a70071abffa4f8e..66367305a0b9ad82e9ef5987c3bdacb9e9a1eeea 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -23,7 +23,7 @@
 // wolf
 #include "core/map/map_base.h"
 #include "core/landmark/landmark_base.h"
-//#include "core/common/factory.h"
+#include "core/common/factory.h"
 
 // YAML
 #include <yaml-cpp/yaml.h>
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 75f23c36e4d86da038bf21c7fc17577c05a637d1..12ec2c4f0b14c5800be2fa617902c466bca7e6cf 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -21,40 +21,18 @@
 //--------LICENSE_END--------
 // wolf includes
 #include "core/problem/problem.h"
-#include "core/hardware/hardware_base.h"
-#include "core/trajectory/trajectory_base.h"
-#include "core/map/map_base.h"
 #include "core/map/factory_map.h"
-#include "core/sensor/sensor_base.h"
 #include "core/sensor/factory_sensor.h"
 #include "core/processor/factory_processor.h"
-#include "core/processor/processor_motion.h"
-#include "core/processor/processor_tracker.h"
-#include "core/capture/capture_pose.h"
 #include "core/capture/capture_void.h"
-#include "core/factor/factor_base.h"
 #include "core/factor/factor_block_absolute.h"
 #include "core/factor/factor_quaternion_absolute.h"
-#include "core/state_block/state_block.h"
 #include "core/state_block/state_quaternion.h"
 #include "core/state_block/state_angle.h"
 #include "core/tree_manager/factory_tree_manager.h"
 #include "core/tree_manager/tree_manager_base.h"
-
-#include "core/utils/params_server.h"
 #include "core/utils/loader.h"
-#include "core/utils/check_log.h"
-#include "core/math/covariance.h"
-#include "core/state_block/factory_state_block.h"
-
-// C++ includes
-#include <algorithm>
-#include <map>
-#include <sstream>
-#include <stdexcept>
-#include <string>
-#include <vector>
-#include <unordered_set>
+
 
 namespace wolf
 {
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 512d941b7669c13e22a87aee23c5a8f84ed44329..3d3794e75b1a87461832695c26341b8f40fc11f5 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -216,10 +216,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
                 last_ptr_->getFrame()->link(getProblem());
 
-                // // make F; append incoming to new F
-                // FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp());
-                // incoming_ptr_->link(frm);
-
                 // Establish factors
                 establishFactors();
 
@@ -232,7 +228,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 // make F; append incoming to new F
                 FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                                  getProblem()->getFrameStructure(),
-                                                                 last_ptr_->getFrame()->getState());
+                                                                 getProblem()->getState(incoming_ptr_->getTimeStamp()));
                 incoming_ptr_   ->link(frame);
                 origin_ptr_     = last_ptr_;
                 last_ptr_       = incoming_ptr_;
@@ -250,7 +246,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 // Replace last frame for a new one in incoming
                 FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                                  getProblem()->getFrameStructure(),
-                                                                 last_ptr_->getFrame()->getState());
+                                                                 getProblem()->getState(incoming_ptr_->getTimeStamp()));
                 incoming_ptr_->link(frame);
                 last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last
 
diff --git a/src/yaml/processor_odom_3d_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp
index a8dd748ea29aa2a63eb72003b5c37465d62c3d17..771d19b7414b2a4a7342c8afea09d08d19f1b750 100644
--- a/src/yaml/processor_odom_3d_yaml.cpp
+++ b/src/yaml/processor_odom_3d_yaml.cpp
@@ -31,7 +31,7 @@
 
 // wolf
 #include "core/processor/processor_odom_3d.h"
-#include "core/common/factory.h"
+#include "core/processor/factory_processor.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_odom_2d_yaml.cpp b/src/yaml/sensor_odom_2d_yaml.cpp
index 68a44ff61adaabb99cb16cc1bc1c72017de6d813..dcb0cf0c10e428a6dfce876a1765e8c0d245e1a2 100644
--- a/src/yaml/sensor_odom_2d_yaml.cpp
+++ b/src/yaml/sensor_odom_2d_yaml.cpp
@@ -33,6 +33,7 @@
 
 // wolf
 #include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/factory_sensor.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_odom_3d_yaml.cpp b/src/yaml/sensor_odom_3d_yaml.cpp
index 02e0ff3d8e5296398a5f9cfc52b66fd21cf15d85..0cb47d441d9b8a0786a9fcf8d00952a682072c4a 100644
--- a/src/yaml/sensor_odom_3d_yaml.cpp
+++ b/src/yaml/sensor_odom_3d_yaml.cpp
@@ -31,7 +31,7 @@
 
 // wolf
 #include "core/sensor/sensor_odom_3d.h"
-#include "core/common/factory.h"
+#include "core/sensor/factory_sensor.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/src/yaml/sensor_pose_yaml.cpp b/src/yaml/sensor_pose_yaml.cpp
index 102729bd91dcfca4f51a567f47d790ce1de6c6b7..bfbe151c43315c21e5f636e2632fa9ceb614c9d4 100644
--- a/src/yaml/sensor_pose_yaml.cpp
+++ b/src/yaml/sensor_pose_yaml.cpp
@@ -31,7 +31,7 @@
 
 // wolf
 #include "core/sensor/sensor_pose.h"
-#include "core/common/factory.h"
+#include "core/sensor/factory_sensor.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 8283b62b8ef35e591dad8533256a517ad65f7718..c37bed03e30da209b9b644d49b92011afeeb5f93 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -626,7 +626,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
 
     // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
     int N = 6;
-    Vector3d data;
+    Vector2d data;
     Matrix2d data_cov; data_cov.setIdentity();
 
     data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp
index a5e0beabb5447222a27bd02138144997d3389a00..23c0afdf5a4bcf4ef2586d7e63b692ae4c953906 100644
--- a/test/gtest_processor_odom_3d.cpp
+++ b/test/gtest_processor_odom_3d.cpp
@@ -149,7 +149,7 @@ TEST(ProcessorOdom3d, deltaPlusDelta_Jac)
 
     MatrixXd J_D(7,7), J_d(7,7);
 
-    JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, Constants::EPS);
+    JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, 1000*Constants::EPS);
     WOLF_DEBUG("J_D:\n ", J_D);
     WOLF_DEBUG("J_d:\n ", J_d);