diff --git a/CMakeLists.txt b/CMakeLists.txt
index fb83ccf5399baef48a2267f0657cf68d8bbd60db..7dafd62f1dcd36919049428014466507f73cb70e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -185,10 +185,10 @@ SET(HDRS_UTILS
   include/core/utils/converter.h
   include/core/utils/eigen_assert.h
   include/core/utils/eigen_predicates.h
-  include/core/utils/loader.hpp
+  include/core/utils/loader.h
   include/core/utils/logging.h
   include/core/utils/make_unique.h
-  include/core/utils/params_server.hpp
+  include/core/utils/params_server.h
   include/core/utils/singleton.h
   include/core/utils/utils_gtest.h
   include/core/utils/converter_utils.h
@@ -287,7 +287,7 @@ SET(HDRS_SOLVER
   )
 
 SET(HDRS_YAML
-  include/core/yaml/parser_yaml.hpp
+  include/core/yaml/parser_yaml.h
   include/core/yaml/yaml_conversion.h
   )
 #SOURCES
@@ -321,6 +321,8 @@ SET(SRCS_MATH
   )
 SET(SRCS_UTILS
   src/utils/converter_utils.cpp
+  src/utils/params_server.cpp
+  src/utils/loader.cpp
   )
 
 SET(SRCS_CAPTURE
@@ -369,6 +371,7 @@ SET(SRCS_SOLVER
   src/solver/solver_manager.cpp
   )
 SET(SRCS_YAML
+  src/yaml/parser_yaml.cpp
   src/yaml/processor_odom_3d_yaml.cpp
   src/yaml/sensor_odom_2d_yaml.cpp
   src/yaml/sensor_odom_3d_yaml.cpp
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp
index a97d8d2f9c5c7fb8c7f1f37c12330038beb3ba98..a387adfe9656523c37d025bb3a74f0f198281b3d 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -9,7 +9,7 @@
 // wolf core includes
 #include "core/common/wolf.h"
 #include "core/capture/capture_odom_2d.h"
-#include "core/yaml/parser_yaml.hpp"
+#include "core/yaml/parser_yaml.h"
 #include "core/ceres_wrapper/ceres_manager.h"
 
 // hello wolf local includes
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index e62bfc1a5cd4ff7115cac06f9738204c71017f2b..491efff4f533edc993051482de7d0e84562a790a 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -9,7 +9,7 @@
 #define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
 
 #include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 
 namespace wolf
 {
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index a5eac842ef7afa410d15a42e7e749ddccc97635c..2491d5a6a5afbdc4d3b52a3e86003c3794b6cd81 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -103,7 +103,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
         static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
 
     protected:
-        SizeEigen computeCalibSize() const;
+        virtual SizeEigen computeCalibSize() const;
 
     private:
         FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h
index f124325950a06f74642cff38b6d0c053958cab8e..802ab7b49dabf93455f8f93fc8c30745415fe33e 100644
--- a/include/core/ceres_wrapper/ceres_manager.h
+++ b/include/core/ceres_wrapper/ceres_manager.h
@@ -8,7 +8,7 @@
 
 //wolf includes
 #include "core/solver/solver_manager.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 #include "core/ceres_wrapper/cost_function_wrapper.h"
 #include "core/ceres_wrapper/local_parametrization_wrapper.h"
 #include "core/ceres_wrapper/create_numeric_diff_cost_function.h"
@@ -84,6 +84,8 @@ class CeresManager : public SolverManager
 
         void check();
 
+        const Eigen::SparseMatrixd computeHessian() const;
+
     protected:
 
         std::string solveImpl(const ReportVerbosity report_level) override;
diff --git a/include/core/common/params_base.h b/include/core/common/params_base.h
index 5295bc0ac17f941503a8f28e462962290a9ee82b..80f6516415e28ddce47ab272415604014ffb0a66 100644
--- a/include/core/common/params_base.h
+++ b/include/core/common/params_base.h
@@ -1,7 +1,7 @@
 #ifndef PARAMS_BASE_H_
 #define PARAMS_BASE_H_
 
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 
 namespace wolf {
   struct ParamsBase
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index eb55faae757f7fe1b02d2fe29c75a049e0b04299..0743548b7b4aad0517ecdd2590955c4cdf7c023c 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.h
@@ -41,7 +41,7 @@ class FactorBlockAbsolute : public FactorAnalytic
                             ProcessorBasePtr _processor_ptr = nullptr,
                             bool _apply_loss_function = false,
                             FactorStatus _status = FAC_ACTIVE) :
-            FactorAnalytic("BLOCK ABS",
+            FactorAnalytic("FactorBlockAbsolute",
                            nullptr,
                            nullptr,
                            nullptr,
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index 8f62b63838d79c6a654c80120bff4e4a144f4098..d3558ef21aa6e6577fc54d26015db3f9e3193312 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/include/core/factor/factor_block_difference.h
@@ -37,8 +37,12 @@ class FactorBlockDifference : public FactorAnalytic
          *
          */
         FactorBlockDifference(
-                            StateBlockPtr _sb1_ptr,
-                            StateBlockPtr _sb2_ptr,
+                            const StateBlockPtr& _sb1_ptr,
+                            const StateBlockPtr& _sb2_ptr,
+                            const FrameBasePtr& _frame_other = nullptr,
+                            const CaptureBasePtr& _cap_other = nullptr,
+                            const FeatureBasePtr& _feat_other = nullptr,
+                            const LandmarkBasePtr& _lmk_other = nullptr,
                             unsigned int _start_idx1 = 0,
                             int _size1 = -1,
                             unsigned int _start_idx2 = 0,
@@ -46,11 +50,11 @@ class FactorBlockDifference : public FactorAnalytic
                             ProcessorBasePtr _processor_ptr = nullptr,
                             bool _apply_loss_function = false,
                             FactorStatus _status = FAC_ACTIVE) :
-            FactorAnalytic("BLOCK ABS",
-                           nullptr,
-                           nullptr,
-                           nullptr,
-                           nullptr,
+            FactorAnalytic("FactorBlockDifference",
+                           _frame_other,
+                           _cap_other,
+                           _feat_other,
+                           _lmk_other,
                            _processor_ptr,
                            _apply_loss_function,
                            _status,
@@ -146,10 +150,16 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
     assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
     assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
     assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
-
-    // normalized jacobian
-    _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
-    _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+    assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+    assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
+
+    // normalized jacobian, computed according to the _compute_jacobian flag
+    if (_compute_jacobian[0]){
+        _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+    }
+    if (_compute_jacobian[1]){
+        _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+    }
 }
 
 inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
@@ -163,10 +173,16 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
     assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
     assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
     assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
-
-    // normalized jacobian
-    _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
-    _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+    assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+    assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
+
+    // normalized jacobian, computed according to the _compute_jacobian flag
+    if (_compute_jacobian[0]){
+        _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+    }
+    if (_compute_jacobian[1]){
+        _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+    }
 }
 
 inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& _pure_jacobians) const
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 63674826d08a026767aeb19a62803b46625b0d2b..f2b19ef05c22f7a4c6777f889031dbda45d80a5b 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -18,7 +18,7 @@ struct ProcessorParamsBase;
 #include "core/common/wolf.h"
 #include "core/frame/frame_base.h"
 #include "core/state_block/state_block.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 #include "core/sensor/sensor_factory.h"
 #include "core/processor/processor_factory.h"
 #include "core/processor/is_motion.h"
@@ -46,8 +46,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         HardwareBasePtr     hardware_ptr_;
         TrajectoryBasePtr   trajectory_ptr_;
         MapBasePtr          map_ptr_;
-        IsMotionPtr  processor_motion_ptr_;
-//        IsMotionPtr         is_motion_ptr_;
+        std::list<IsMotionPtr>  processor_is_motion_list_;
         std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
         SizeEigen state_size_, state_cov_size_, dim_;
         std::map<FactorBasePtr, Notification> factor_notification_map_;
@@ -147,15 +146,15 @@ class Problem : public std::enable_shared_from_this<Problem>
     protected:
         /** \brief Set the processor motion
          *
-         * Set the processor motion.
+         * Add a new processor of type is motion to the processor is motion list.
          */
-        void setProcessorMotion(IsMotionPtr _processor_motion_ptr);
-        IsMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
-        void clearProcessorMotion();
+        void addProcessorIsMotion(IsMotionPtr _processor_motion_ptr);
+        void clearProcessorIsMotion(IsMotionPtr proc);
 
     public:
-        IsMotionPtr& getProcessorMotion();
-
+        IsMotionPtr getProcessorIsMotion();
+        std::list<IsMotionPtr> getProcessorIsMotionList();
+        
         // Trajectory branch ----------------------------------
         TrajectoryBasePtr getTrajectory() const;
         virtual FrameBasePtr setPrior(const Eigen::VectorXd& _prior_state, //
@@ -271,10 +270,10 @@ class Problem : public std::enable_shared_from_this<Problem>
         // Zero state provider
         Eigen::VectorXd zeroState ( ) const;
         bool priorIsSet() const;
+        void setPriorIsSet(bool _prior_is_set);
         // Perturb state
         void            perturb(double amplitude = 0.01);
 
-
         // Map branch -----------------------------------------
         MapBasePtr getMap() const;
         void loadMap(const std::string& _filename_dot_yaml);
@@ -350,7 +349,7 @@ class Problem : public std::enable_shared_from_this<Problem>
                    bool constr_by = false, //
                    bool metric = true, //
                    bool state_blocks = false) const;
-        bool check(int verbose_level) const;
+        bool check(int verbose_level = 0) const;
         bool check(bool verbose, std::ostream& stream) const;
 
 };
@@ -367,9 +366,22 @@ inline bool Problem::priorIsSet() const
     return prior_is_set_;
 }
 
-inline IsMotionPtr& Problem::getProcessorMotion()
+inline void Problem::setPriorIsSet(bool _prior_is_set)
+{
+    prior_is_set_ = _prior_is_set;
+}
+
+inline IsMotionPtr Problem::getProcessorIsMotion()
+{
+    if (!processor_is_motion_list_.empty())
+        return processor_is_motion_list_.front();
+    return nullptr;
+}
+
+
+inline std::list<IsMotionPtr> Problem::getProcessorIsMotionList()
 {
-    return processor_motion_ptr_;
+    return processor_is_motion_list_;
 }
 
 inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap()
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index f3cf0d781f39c9fda6ffb9e64817ceed0acfa162..8c99fe72e37698a2210e35a3af0c14bf9168f65e 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -43,6 +43,12 @@ class IsMotion
         TimeStamp       getCurrentTimeStamp() const;
         Eigen::VectorXd getState(const TimeStamp& _ts) const;
 
+        std::string getStateStructure(){return state_structure_;};
+        void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;};
+    
+    protected:
+        std::string state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
+
 };
 
 }
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index f3feeb56a3bcc6787d8efeb1355175b989dd3f3e..dc62abcac8375fc238f7c93617a33ccb8662d246 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -117,13 +117,18 @@ public:
     *
     * elements are ordered from most recent to oldest
     */
-    std::map<TimeStamp,T> getContainer();
+    const std::map<TimeStamp,T>& getContainer();
 
     /**\brief Remove all packs in the buffer with a time stamp older than the specified
     *
     */
     void removeUpTo(const TimeStamp& _time_stamp);
 
+    /**\brief Remove all packs in the buffer with a time stamp older than the specified
+    *
+    */
+    void removeUpToLower(const TimeStamp& _time_stamp);
+
     /**\brief Clear the buffer
     *
     */
@@ -503,7 +508,7 @@ void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element)
 }
 
 template <typename T>
-std::map<TimeStamp,T> Buffer<T>::getContainer()
+const std::map<TimeStamp,T>& Buffer<T>::getContainer()
 {
     return container_;
 }
@@ -533,6 +538,13 @@ inline void Buffer<T>::removeUpTo(const TimeStamp& _time_stamp)
     container_.erase(container_.begin(), post); // erasing by range
 }
 
+template <typename T>
+inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp)
+{
+    Buffer::Iterator post = container_.lower_bound(_time_stamp);
+    container_.erase(container_.begin(), post); // erasing by range
+}
+
 template <typename T>
 inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1,
                                 const TimeStamp& _time_stamp2, const double& _time_tolerance2)
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 2ac6c8d5949ad07c1529fd06739c0ce80211f243..81a9184ad4798ccbaac1ebd61a194f6e287331b4 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -13,7 +13,7 @@
 #include "core/processor/processor_base.h"
 #include "core/processor/is_motion.h"
 #include "core/common/time_stamp.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 
 // std
 #include <iomanip>
@@ -148,6 +148,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
     // This is the main public interface
     public:
         ProcessorMotion(const std::string& _type,
+                        std::string _state_structure,
                         int _dim,
                         SizeEigen _state_size,
                         SizeEigen _delta_size,
@@ -505,11 +506,12 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
     assert(origin_ptr_ && "Trying to access origin_ptr_ but it is nullptr!");
 
     // ensure proper size of the provided reference
-    _x.resize( getProblem()->getFrameStructureSize() );
+    Eigen::VectorXd curr_x = origin_ptr_->getFrame()->getState(state_structure_);
+    _x.resize( curr_x.size() );
 
     // do get timestamp and state corrected by possible self-calibrations
     double Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
-    statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
+    statePlusDelta(curr_x, last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
 }
 
 inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index 6d46ab61a32c1b9573f386c97f1ae671086478c6..b03416837ec1b0a6a86a67ff0cf40fdf820b1370 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -12,7 +12,7 @@
 #include "core/capture/capture_odom_2d.h"
 #include "core/factor/factor_odom_2d.h"
 #include "core/math/rotations.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 
 namespace wolf {
 
diff --git a/include/core/sensor/sensor_factory.h b/include/core/sensor/sensor_factory.h
index 3930176443a22cebb8c9591253af7f58b0a40a2c..daf01499ac759d799517cdd73ab830fb6ccaf4d5 100644
--- a/include/core/sensor/sensor_factory.h
+++ b/include/core/sensor/sensor_factory.h
@@ -16,7 +16,7 @@ struct ParamsSensorBase;
 
 // wolf
 #include "core/common/factory.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 
 namespace wolf
 {
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index d6a431d3102b0a82a218ee71247552066c1ef8eb..45499f5c56fb342ed8a82b8521e6c2e156d06031 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -3,7 +3,7 @@
 
 //wolf includes
 #include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 
 namespace wolf {
 
diff --git a/include/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h
index 67efe1e9af27f8456401c400766dcbf32f44057b..3d08375b4fc30e43f07f201a5653d5e83a258381 100644
--- a/include/core/sensor/sensor_odom_3d.h
+++ b/include/core/sensor/sensor_odom_3d.h
@@ -10,7 +10,7 @@
 
 //wolf includes
 #include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 
 namespace wolf {
 
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 5561e2dfdf51b13dbb6e104398478570a005f5a5..07c6d757bc09125c4a4e8a55aefd6d37b480c7d4 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -15,6 +15,10 @@
 namespace wolf
 {
 
+/// State of nodes containing several state blocks
+typedef std::unordered_map<std::string, Eigen::VectorXd> State;
+
+
 class HasStateBlocks
 {
     public:
@@ -65,11 +69,13 @@ class HasStateBlocks
         void removeStateBlocks(ProblemPtr _problem);
 
         // States
-        virtual void    setState(const Eigen::VectorXd& _state, const bool _notify = true);
-        Eigen::VectorXd getState() const;
-        void            getState(Eigen::VectorXd& _state) const;
-        unsigned int    getSize() const;
-        unsigned int    getLocalSize() const;
+        inline void setState(const Eigen::VectorXd& _state, std::string _sub_structure="", const bool _notify = true);
+        void getState(Eigen::VectorXd& _state, std::string structure="") const;
+        Eigen::VectorXd getState(std::string structure="") const;
+        unsigned int getSize(std::string _sub_structure="") const;
+        unsigned int getLocalSize(std::string _sub_structure="") const;
+
+        State getStateComposite();
 
         // Perturb state
         void            perturb(double amplitude = 0.01);
@@ -201,55 +207,86 @@ inline bool HasStateBlocks::isFixed() const
     return fixed;
 }
 
-inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const bool _notify)
+inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string _sub_structure, const bool _notify)
 {
-    int size = getSize();
-
+    if (_sub_structure == ""){
+        _sub_structure = structure_;
+    }
+    int size = getSize(_sub_structure);
     assert(_state.size() == size && "In FrameBase::setState wrong state size");
 
     unsigned int index = 0;
-    for (const char key : getStructure())
+    for (const char key : _sub_structure)
     {
         const auto& sb = getStateBlock(key);
-
+        if (!sb){
+            WOLF_ERROR("Stateblock key ", key, " not in the structure");
+        }
         sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver
         index += sb->getSize();
     }
 
 }
 
-inline void HasStateBlocks::getState(Eigen::VectorXd& _state) const
+// _sub_structure can be either stateblock structure of the node or a subset of this structure
+inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_structure) const
 {
-    _state.resize(getSize());
+    if (_sub_structure == ""){
+        _sub_structure = structure_;
+    }
+    _state.resize(getSize(_sub_structure));
 
     unsigned int index = 0;
-    for (const char key : getStructure())
+    for (const char key : _sub_structure)
     {
         const auto& sb = getStateBlock(key);
-
+        if (!sb){
+            WOLF_ERROR("Stateblock key ", key, " not in the structure");
+        }
         _state.segment(index,sb->getSize()) = sb->getState();
         index += sb->getSize();
     }
 
 }
 
-inline Eigen::VectorXd HasStateBlocks::getState() const
+inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) const
 {
     Eigen::VectorXd state;
 
-    getState(state);
+    getState(state, _sub_structure);
 
     return state;
 }
 
-inline unsigned int HasStateBlocks::getSize() const
+inline State HasStateBlocks::getStateComposite()
 {
+    State state;
+    for (auto& pair_key_kf : state_block_map_)
+    {
+        state.emplace(pair_key_kf.first, pair_key_kf.second->getState());
+    }
+    return state;
+}
+
+inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const
+{
+    if (_sub_structure == ""){
+        _sub_structure = structure_;
+    }
     unsigned int size = 0;
-    for (const auto& pair_key_sb : getStateBlockMap())
-            size += pair_key_sb.second->getSize();
+    for (const char key : _sub_structure)
+    {
+        const auto& sb = getStateBlock(key);
+        if (!sb){
+            WOLF_ERROR("Stateblock key ", key, " not in the structure");
+        }
+        size += sb->getSize();
+    }
+
     return size;
 }
 
+//<<<<<<< HEAD
 inline std::unordered_map<std::string, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
 {
     const auto& it = std::find_if(state_block_map_.begin(),
@@ -288,13 +325,27 @@ inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string&
     }
 }
 
-inline unsigned int HasStateBlocks::getLocalSize() const
+//inline unsigned int HasStateBlocks::getLocalSize() const
+//=======
+inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) const
+//>>>>>>> devel
 {
+    if (_sub_structure == ""){
+        _sub_structure = structure_;
+    }
     unsigned int size = 0;
-    for (const auto& pair_key_sb : getStateBlockMap())
-            size += pair_key_sb.second->getLocalSize();
+    for (const char key : _sub_structure)
+    {
+        const auto& sb = getStateBlock(key);
+        if (!sb){
+            WOLF_ERROR("Stateblock key ", key, " not in the structure");
+        }
+        size += sb->getLocalSize();
+    }
+
     return size;
 }
 
 } // namespace wolf
+
 #endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 33aa6fcb4b221ea1dc23f24b21f4a0fb12cbfc0d..ab3bb7147f3c81f57a3436216412274394acc04a 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -74,6 +74,10 @@ public:
          **/
         Eigen::VectorXd getState() const;
 
+        /** \brief Returns the state vector data array
+         **/
+        double* getStateData();
+
         /** \brief Sets the state vector
          **/
         void setState(const Eigen::VectorXd& _state, const bool _notify = true);
@@ -283,6 +287,11 @@ inline void StateBlock::resetLocalParamUpdated()
     local_param_updated_.store(false);
 }
 
+inline double* StateBlock::getStateData()
+{
+    return state_.data();
+}
+
 }// namespace wolf
 
 #endif
diff --git a/include/core/utils/loader.h b/include/core/utils/loader.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f1a4cc68c2d0dcb20a6ef18f53ec259507fc176
--- /dev/null
+++ b/include/core/utils/loader.h
@@ -0,0 +1,32 @@
+#ifndef LOADER_H
+#define LOADER_H
+
+#include <string>
+
+class Loader{
+protected:
+    std::string path_;
+public:
+    Loader(std::string _file);
+    virtual void load() = 0;
+    virtual void close() = 0;
+};
+
+class LoaderRaw: public Loader{
+    void* resource_;
+public:
+    LoaderRaw(std::string _file);
+    ~LoaderRaw();
+    void load();
+    void close();
+};
+// class LoaderPlugin: public Loader{
+//     ClassLoader* resource_;
+//     void load(){
+//         this->resource_ = new ClassLoader(this->path_);
+//     }
+//     void close(){
+//         delete this->resource_;
+//     }
+// };
+#endif
diff --git a/include/core/utils/loader.hpp b/include/core/utils/loader.hpp
deleted file mode 100644
index 3d867d799b36bfd53fb42e93577554caf1b050c1..0000000000000000000000000000000000000000
--- a/include/core/utils/loader.hpp
+++ /dev/null
@@ -1,45 +0,0 @@
-#ifndef LOADER_HPP
-#define LOADER_HPP
-#include <dlfcn.h>
-#include <string>
-#include <stdexcept>
-class Loader{
-protected:
-    std::string path_;
-public:
-    Loader(std::string _file){
-        this->path_ = _file;
-    }
-    virtual void load() = 0;
-    virtual void close() = 0;
-};
-class LoaderRaw: public Loader{
-    void* resource_;
-public:
-    LoaderRaw(std::string _file):
-        Loader(_file)
-    {
-        //
-    }
-    ~LoaderRaw(){
-        this->close();
-    }
-    void load(){
-        this->resource_ = dlopen(this->path_.c_str(), RTLD_LAZY);
-        if(not this->resource_)
-            throw std::runtime_error("Couldn't load resource with path " + this->path_ + "\n" + "Error info: " + dlerror());
-    }
-    void close(){
-        if(this->resource_) dlclose(this->resource_);
-    }
-};
-// class LoaderPlugin: public Loader{
-//     ClassLoader* resource_;
-//     void load(){
-//         this->resource_ = new ClassLoader(this->path_);
-//     }
-//     void close(){
-//         delete this->resource_;
-//     }
-// };
-#endif
\ No newline at end of file
diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8b473ca26703f17ea932cca4d1e913717108026
--- /dev/null
+++ b/include/core/utils/params_server.h
@@ -0,0 +1,55 @@
+#ifndef PARAMS_SERVER_H
+#define PARAMS_SERVER_H
+
+#include "core/utils/converter.h"
+
+#include <map>
+#include <exception>
+
+namespace wolf{
+
+class MissingValueException : public std::runtime_error
+{
+public:
+  MissingValueException(std::string _msg) : std::runtime_error(_msg) {}
+};
+
+class ParamsServer{
+    std::map<std::string, std::string> params_;
+public:
+    ParamsServer();
+    ParamsServer(std::map<std::string, std::string> _params);
+    ~ParamsServer(){
+        //
+    }
+
+    void print();
+
+
+    void addParam(std::string _key, std::string _value);
+
+    void addParams(std::map<std::string, std::string> _params);
+
+   // template<typename T>
+   // T getParam(std::string key, std::string def_value) const {
+   //     if(params_.find(key) != params_.end()){
+   //         return converter<T>::convert(params_.find(key)->second);
+   //     }else{
+   //         return converter<T>::convert(def_value);
+   //     }
+   // }
+
+    template<typename T>
+    T getParam(std::string _key) const {
+        if(params_.find(_key) != params_.end()){
+            return converter<T>::convert(params_.find(_key)->second);
+        }else{
+            throw MissingValueException("The following key: '" + _key + "' has not been found in the parameters server.");
+        }
+    }
+
+};
+
+}
+
+#endif
diff --git a/include/core/utils/params_server.hpp b/include/core/utils/params_server.hpp
deleted file mode 100644
index a750eab79f4ee90b39e4e21d1926129897c29d92..0000000000000000000000000000000000000000
--- a/include/core/utils/params_server.hpp
+++ /dev/null
@@ -1,70 +0,0 @@
-#ifndef PARAMS_SERVER_HPP
-#define PARAMS_SERVER_HPP
-
-#include "core/utils/converter.h"
-//#include "core/yaml/parser_yaml.hpp"
-
-#include <vector>
-#include <regex>
-#include <map>
-#include <exception>
-
-namespace wolf{
-
-class MissingValueException : public std::runtime_error
-{
-public:
-  MissingValueException(std::string msg) : std::runtime_error(msg) {}
-};
-
-class ParamsServer{
-    std::map<std::string, std::string> _params;
-public:
-    ParamsServer(){
-        _params = std::map<std::string, std::string>();
-    }
-    ParamsServer(std::map<std::string, std::string> params){
-        _params = params;
-    }
-    ~ParamsServer(){
-        //
-    }
-
-    void print(){
-        for(auto it : _params)
-            std::cout << it.first << "~~" << it.second << std::endl;
-    }
-
-
-    void addParam(std::string key, std::string value){
-        _params.insert(std::pair<std::string, std::string>(key, value));
-    }
-
-    void addParams(std::map<std::string, std::string> params)
-    {
-        _params.insert(params.begin(), params.end());
-    }
-
-   // template<typename T>
-   // T getParam(std::string key, std::string def_value) const {
-   //     if(_params.find(key) != _params.end()){
-   //         return converter<T>::convert(_params.find(key)->second);
-   //     }else{
-   //         return converter<T>::convert(def_value);
-   //     }
-   // }
-
-    template<typename T>
-    T getParam(std::string key) const {
-        if(_params.find(key) != _params.end()){
-            return converter<T>::convert(_params.find(key)->second);
-        }else{
-            throw MissingValueException("The following key: '" + key + "' has not been found in the parameters server.");
-        }
-    }
-
-};
-
-}
-
-#endif
diff --git a/include/core/yaml/parser_yaml.h b/include/core/yaml/parser_yaml.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b6f659ebd609dffef4f3c472800cf32714f22e1
--- /dev/null
+++ b/include/core/yaml/parser_yaml.h
@@ -0,0 +1,80 @@
+#ifndef PARSER_YAML_H
+#define PARSER_YAML_H
+
+#include "core/utils/converter.h"
+#include "core/common/wolf.h"
+
+#include "yaml-cpp/yaml.h"
+
+namespace wolf
+{
+class ParserYAML {
+    struct ParamsInitSensor{
+        std::string type_;
+        std::string name_;
+        std::string plugin_;
+        YAML::Node n_;
+    };
+    struct ParamsInitProcessor{
+        std::string type_;
+        std::string name_;
+        std::string name_assoc_sensor_;
+        std::string plugin_;
+        YAML::Node n_;
+    };
+    struct SubscriberManager{
+        std::string package_;
+        std::string subscriber_;
+        std::string topic_;
+        std::string sensor_name_;
+        YAML::Node n_;
+    };
+    struct PublisherManager{
+        std::string subscriber_;
+        std::string topic_;
+        std::string period_;
+        YAML::Node n_;
+    };
+    std::map<std::string, std::string> params_;
+    std::string active_name_;
+    std::vector<ParamsInitSensor> paramsSens_;
+    std::vector<ParamsInitProcessor> paramsProc_;
+    std::stack<std::string> parsing_file_;
+    std::string file_;
+    std::string path_root_;
+    std::vector<SubscriberManager> subscriber_managers_;
+    std::vector<PublisherManager> publisher_managers_;
+    YAML::Node problem;
+    std::string generatePath(std::string);
+    YAML::Node loadYAML(std::string);
+    void insert_register(std::string, std::string);
+public:
+    ParserYAML(std::string _file, std::string _path_root = "",
+               bool _freely_parse = false);
+    ~ParserYAML()
+    {
+        //
+    }
+    void parse_freely();
+    std::map<std::string, std::string> getParams();
+
+  private:
+    void walkTree(std::string _file);
+    void walkTree(std::string _file, std::vector<std::string>& _tags);
+    void walkTree(std::string _file, std::vector<std::string>& _tags, std::string _hdr);
+/** @Brief Recursively walks the YAML tree while filling a map with the values parsed from the file
+ * @param YAML node to be parsed
+ * @param tags represents the path from the root of the YAML tree to the current node
+ * @param hdr is the name of the current YAML node
+ */
+    void walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string _hdr);
+    void updateActiveName(std::string _tag);
+/** @Brief Parse the sensors, processors, callbacks and files elements of the YAML file. We assume that these elements
+    are defined at the top level of the YAML file.
+ * @param file is the path to the YAML file */
+    void parseFirstLevel(std::string _file);
+    std::string tagsToString(std::vector<std::string>& _tags);
+    void parse();
+};
+}
+#endif
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
deleted file mode 100644
index 22f584d76bab2aa2c3a6bd6ca30edbeb4873a4ea..0000000000000000000000000000000000000000
--- a/include/core/yaml/parser_yaml.hpp
+++ /dev/null
@@ -1,532 +0,0 @@
-#ifndef PARSER_YAML_HPP
-#define PARSER_YAML_HPP
-
-#include "core/utils/converter.h"
-#include "core/common/wolf.h"
-
-#include "yaml-cpp/yaml.h"
-
-#include <string>
-#include <vector>
-#include <list>
-#include <stack>
-#include <regex>
-#include <map>
-#include <iostream>
-#include <algorithm>
-#include <numeric>
-
-namespace {
-  //====== START OF FORWARD DECLARATION ========
-  std::string parseAtomicNode(YAML::Node);
-  std::string fetchMapEntry(YAML::Node);
-  std::string mapToString(std::map<std::string,std::string>);
-  //====== END OF FORWARD DECLARATION ========
-
-/** @Brief Interprets a map as being atomic and thus parses it as a single entity. We assume that the map has as values only scalars and sequences.
- *  @param n the node representing a map
- *  @return std::map<std::string, std::string> populated with the key,value pairs in n
- */
-std::map<std::string, std::string> fetchAsMap(YAML::Node n){
-    assert(n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node");
-    auto m = std::map<std::string, std::string>();
-    for(const auto& kv : n){
-        std::string key = kv.first.as<std::string>();
-        switch (kv.second.Type()) {
-        case YAML::NodeType::Scalar : {
-            std::string value = kv.second.Scalar();
-            m.insert(std::pair<std::string,std::string>(key, value));
-            break;
-        }
-        case YAML::NodeType::Sequence : {
-            std::string aux = parseAtomicNode(kv.second);
-            m.insert(std::pair<std::string,std::string>(key, aux));
-            break;
-        }
-        case YAML::NodeType::Map : {
-          std::string value = fetchMapEntry(kv.second);
-          std::regex r("^\\$.*");
-          if (std::regex_match(key, r)) key = key.substr(1,key.size()-1);
-          m.insert(std::pair<std::string,std::string>(key, value));
-          break;
-        }
-        default:
-            assert(1 == 0 && "Unsupported node Type at fetchAsMap");
-            break;
-        }
-    }
-    return m;
-}
-  std::string fetchMapEntry(YAML::Node n){
-    switch (n.Type()) {
-    case YAML::NodeType::Scalar: {
-      return n.Scalar();
-      break;
-    }
-    case YAML::NodeType::Sequence: {
-      return parseAtomicNode(n);
-      break;
-    }
-    case YAML::NodeType::Map: {
-      return mapToString(fetchAsMap(n));
-      break;
-    }
-    default: {
-      assert(1 == 0 && "Unsupported node Type at fetchMapEntry");
-      return "";
-      break;
-    }
-    }
-  }
-    /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...]
-    * @param _map just a std::map<std::string,std::string>
-    * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...]
-    */
-    std::string mapToString(std::map<std::string,std::string> _map){
-        std::string result = "";
-        auto v = std::vector<std::string>();
-        std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<std::string,std::string> p){return "{" + p.first + ":" + p.second + "}";});
-        auto concat = [](std::string ac, std::string str)-> std::string {
-                          return ac + str + ",";
-                      };
-        std::string aux = "";
-        std::string accumulated = std::accumulate(v.begin(), v.end(), aux, concat);
-        if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1);
-        else accumulated = "";
-        return "[" + accumulated + "]";
-    }
-    /** @Brief Generates a std::string representing a YAML sequence. The sequence is assumed to be scalar or at most be a sequence of sequences of scalars.
-    * @param n a vector of YAML::Node that represents a YAML::Sequence
-    * @return <b>{std::string}</b> representing the YAML sequence
-    */
-    std::string parseAtomicNode(YAML::Node n){
-      std::string aux = "";
-      bool first = true;
-      std::string separator = "";
-      switch(n.Type()){
-      case YAML::NodeType::Scalar:
-        return n.Scalar();
-        break;
-      case YAML::NodeType::Sequence:
-        for(auto it : n){
-          aux += separator + parseAtomicNode(it);
-          if(first){
-            separator = ",";
-            first = false;
-          }
-        }
-        return "[" + aux + "]";
-        break;
-      case YAML::NodeType::Map:
-        return mapToString(fetchAsMap(n));
-        break;
-      default:
-        return "";
-        break;
-      }
-    }
-
-    /** @Brief checks if a node of the YAML tree is atomic. Only works if the nodes are of type
-     * Scalar, Sequence or Map.
-     * @param key is the key associated to the node n if n.Type() == YAML::NodeType::Map
-     * @param n node to be test for atomicity
-    */
-    bool isAtomic(std::string key, YAML::Node n){
-      assert(n.Type() != YAML::NodeType::Undefined && n.Type() != YAML::NodeType::Null && "Cannot determine atomicity of Undefined/Null node");
-      std::regex r("^\\$.*");
-      bool is_atomic = true;
-      switch(n.Type()){
-      case YAML::NodeType::Scalar:
-        return true;
-        break;
-      case YAML::NodeType::Sequence:
-        for(auto it : n) {
-          switch(it.Type()){
-          case YAML::NodeType::Map:
-            for(const auto& kv : it){
-              is_atomic = is_atomic and isAtomic(kv.first.as<std::string>(), it);
-            }
-            break;
-          default:
-            is_atomic = is_atomic and isAtomic("", it);
-            break;
-          }
-        }
-        return is_atomic;
-        break;
-      case YAML::NodeType::Map:
-        is_atomic = std::regex_match(key, r);
-        return is_atomic;
-        break;
-      default:
-        throw std::runtime_error("Cannot determine atomicity of node type " + std::to_string(n.Type()));
-        return false;
-        break;
-      }
-      return false;
-    }
-}
-class ParserYAML {
-    struct ParamsInitSensor{
-        std::string _type;
-        std::string _name;
-        std::string _plugin;
-        YAML::Node n;
-    };
-    struct ParamsInitProcessor{
-        std::string _type;
-        std::string _name;
-        std::string _name_assoc_sensor;
-        std::string _plugin;
-        YAML::Node n;
-    };
-    struct SubscriberManager{
-        std::string _package;
-        std::string _subscriber;
-        std::string _topic;
-        std::string _sensor_name;
-        YAML::Node n;
-    };
-    struct PublisherManager{
-        std::string _subscriber;
-        std::string _topic;
-        std::string _period;
-        YAML::Node n;
-    };
-    std::map<std::string, std::string> _params;
-    std::string _active_name;
-    std::vector<ParamsInitSensor> _paramsSens;
-    std::vector<ParamsInitProcessor> _paramsProc;
-    std::stack<std::string> _parsing_file;
-    std::string _file;
-    std::string _path_root;
-    std::vector<SubscriberManager> _subscriber_managers;
-    std::vector<PublisherManager> _publisher_managers;
-    YAML::Node problem;
-    std::string generatePath(std::string);
-    YAML::Node loadYAML(std::string);
-    void insert_register(std::string, std::string);
-public:
-    ParserYAML(std::string file, std::string path_root = "",
-               bool freely_parse = false) {
-      _params = std::map<std::string, std::string>();
-      _active_name = "";
-      _paramsSens = std::vector<ParamsInitSensor>();
-      _paramsProc = std::vector<ParamsInitProcessor>();
-      _subscriber_managers = std::vector<SubscriberManager>();
-      _publisher_managers = std::vector<PublisherManager>();
-      _parsing_file = std::stack<std::string>();
-      _file = file;
-      if (path_root != "") {
-        std::regex r(".*/ *$");
-        if (not std::regex_match(path_root, r))
-          _path_root = path_root + "/";
-        else
-          _path_root = path_root;
-      }
-      if(not freely_parse) this->parse();
-      else this->parse_freely();
-    }
-    ~ParserYAML()
-    {
-        //
-    }
-    void parse_freely();
-    std::map<std::string, std::string> getParams();
-
-  private:
-    void walkTree(std::string file);
-    void walkTree(std::string file, std::vector<std::string>& tags);
-    void walkTree(std::string file, std::vector<std::string>& tags, std::string hdr);
-    void walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr);
-    void updateActiveName(std::string tag);
-    void parseFirstLevel(std::string file);
-    std::string tagsToString(std::vector<std::string>& tags);
-    void parse();
-};
-std::string ParserYAML::generatePath(std::string file){
-    std::regex r("^/.*");
-    if(std::regex_match(file, r)){
-        return file;
-    }else{
-        return _path_root + file;
-    }
-}
-YAML::Node ParserYAML::loadYAML(std::string file){
-  try{
-    // std::cout << "Parsing " << generatePath(file) << std::endl;
-    WOLF_INFO("Parsing file: ", generatePath(file));
-    return YAML::LoadFile(generatePath(file));
-  }catch (YAML::BadFile& e){
-    throw std::runtime_error("Couldn't load file " + generatePath(file) + ". Tried to open it from " + this->_parsing_file.top());
-  }
-}
-std::string ParserYAML::tagsToString(std::vector<std::string> &tags){
-    std::string hdr = "";
-    for(auto it : tags){
-        hdr = hdr + "/" + it;
-    }
-    return hdr;
-}
-void ParserYAML::walkTree(std::string file){
-    YAML::Node n;
-    n = loadYAML(generatePath(file));
-    this->_parsing_file.push(generatePath(file));
-    std::vector<std::string> hdrs = std::vector<std::string>();
-    walkTreeR(n, hdrs, "");
-    this->_parsing_file.pop();
-}
-void ParserYAML::walkTree(std::string file, std::vector<std::string>& tags){
-    YAML::Node n;
-    n = loadYAML(generatePath(file));
-    this->_parsing_file.push(generatePath(file));
-    walkTreeR(n, tags, "");
-    this->_parsing_file.pop();
-}
-void ParserYAML::walkTree(std::string file, std::vector<std::string>& tags, std::string hdr){
-    YAML::Node n;
-    n = loadYAML(generatePath(file));
-    this->_parsing_file.push(generatePath(file));
-    walkTreeR(n, tags, hdr);
-    this->_parsing_file.pop();
-}
-/** @Brief Recursively walks the YAML tree while filling a map with the values parsed from the file
-* @param YAML node to be parsed
-* @param tags represents the path from the root of the YAML tree to the current node
-* @param hdr is the name of the current YAML node
-*/
-void ParserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::string hdr){
-    switch (n.Type()) {
-    case YAML::NodeType::Scalar : {
-        std::regex r("^@.*");
-        if(std::regex_match(n.Scalar(), r)){
-            std::string str = n.Scalar();
-            walkTree(str.substr(1,str.size() - 1), tags, hdr);
-        }else{
-          insert_register(hdr, n.Scalar());
-        }
-        break;
-    }
-    case YAML::NodeType::Sequence : {
-      if(isAtomic("", n)){
-        insert_register(hdr, parseAtomicNode(n));
-        }else{
-          for(const auto& kv : n){
-            walkTreeR(kv, tags, hdr);
-          }
-        }
-        break;
-    }
-    case YAML::NodeType::Map : {
-      for(const auto& kv : n){
-        if(isAtomic(kv.first.as<std::string>(), n)){
-          std::string key = kv.first.as<std::string>();
-          //WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key);
-          key = key.substr(1,key.size() - 1);
-          insert_register(hdr + "/" + key, parseAtomicNode(kv.second));
-      }else{
-            /*
-              If key=="follow" then the parser will assume that the value is a path and will parse
-              the (expected) yaml file at the specified path. Note that this does not increase the header depth.
-              The following example shows how the header remains unafected:
-              @my_main_config                |  @some_path
-              - cov_det: 1                   |  - my_value : 23
-              - follow: "@some_path"         |
-              - var: 1.2                     |
-              Resulting map:
-              cov_det -> 1
-              my_value-> 23
-              var: 1.2
-              Instead of:
-              cov_det -> 1
-              follow/my_value-> 23
-              var: 1.2
-              Which would result from the following yaml files
-              @my_main_config                |  @some_path
-              - cov_det: 1                   |  - my_value : 23
-              - $follow: "@some_path"        |
-              - var: 1.2                     |
-            */
-          std::string key = kv.first.as<std::string>();
-          //WOLF_DEBUG("KEY IN MAP NON ATOMIC ", key);
-          std::regex rr("follow");
-          if(not std::regex_match(kv.first.as<std::string>(), rr)) {
-            tags.push_back(kv.first.as<std::string>());
-            if(tags.size() == 2) this->updateActiveName(kv.first.as<std::string>());
-            walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<std::string>());
-            tags.pop_back();
-            if(tags.size() == 1) this->updateActiveName("");
-          }else{
-            walkTree(kv.second.as<std::string>(), tags, hdr);
-          }
-      }
-      }
-        break;
-    }
-      case YAML::NodeType::Null :
-        break;
-    default:
-        assert(1 == 0 && "Unsupported node Type at walkTreeR.");
-        break;
-    }
-}
-void ParserYAML::updateActiveName(std::string tag){
-    this->_active_name = tag;
-}
-/** @Brief Parse the sensors, processors, callbacks and files elements of the YAML file. We assume that these elements
-    are defined at the top level of the YAML file.
- * @param file is the path to the YAML file */
-void ParserYAML::parseFirstLevel(std::string file){
-    YAML::Node n;
-    n = loadYAML(generatePath(file));
-
-    YAML::Node n_config = n["config"];
-    // assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node");
-    if(n_config.Type() != YAML::NodeType::Map) throw std::runtime_error("Could not find config node. Please make sure that your YAML file " + generatePath(file) + " starts with 'config:'");
-    if(n_config["problem"].Type() != YAML::NodeType::Map) throw std::runtime_error("Could not find problem node. Please make sure that the 'config' node in YAML file " + generatePath(file) + " has a 'problem' entry");
-    this->problem = n_config["problem"];
-    std::vector<std::map<std::string, std::string>> map_container;
-    try{
-      for(const auto& kv : n_config["sensors"]){
-          ParamsInitSensor pSensor = {kv["type"].Scalar(), kv["name"].Scalar(), kv["plugin"].Scalar(), kv};
-          _paramsSens.push_back(pSensor);
-          map_container.push_back(std::map<std::string, std::string>({
-                                                                      {"type", kv["type"].Scalar()},
-                                                                      {"name", kv["name"].Scalar()},
-                                                                      {"plugin", kv["plugin"].Scalar()}
-                  }));
-      }
-      insert_register("sensors", wolf::converter<std::string>::convert(map_container));
-      map_container.clear();
-    } catch(YAML::InvalidNode& e){
-      throw std::runtime_error("Error parsing sensors @" + generatePath(file) + ". Please make sure that each sensor entry has 'type', 'name' and 'plugin' entries.");
-    }
-
-    try{
-      for(const auto& kv : n_config["processors"]){
-          ParamsInitProcessor pProc = {kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv["plugin"].Scalar(), kv};
-          _paramsProc.push_back(pProc);
-          map_container.push_back(std::map<std::string, std::string>({
-                                                                      {"type", kv["type"].Scalar()},
-                                                                      {"name", kv["name"].Scalar()},
-                                                                      {"plugin", kv["plugin"].Scalar()},
-                                                                      {"sensor_name", kv["sensor_name"].Scalar()}}));
-      }
-      insert_register("processors",
-                      wolf::converter<std::string>::convert(map_container));
-      map_container.clear();
-    } catch(YAML::InvalidNode& e){
-      throw std::runtime_error("Error parsing processors @" + generatePath(file) + ". Please make sure that each processor has 'type', 'name', 'plugin' and 'sensor_name' entries.");
-    }
-    try {
-      for (const auto &kv : n_config["ROS subscriber"]) {
-          SubscriberManager pSubscriberManager = {kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["sensor_name"].Scalar(), kv};
-          _subscriber_managers.push_back(pSubscriberManager);
-          map_container.push_back(std::map<std::string, std::string>({
-                                                                      {"package", kv["package"].Scalar()},
-                                                                      {"type", kv["type"].Scalar()},
-                                                                      {"topic", kv["topic"].Scalar()},
-                                                                      {"sensor_name", kv["sensor_name"].Scalar()}}));
-      }
-      insert_register("ROS subscriber", wolf::converter<std::string>::convert(map_container));
-      map_container.clear();
-    } catch (YAML::InvalidNode &e) {
-        throw std::runtime_error("Error parsing subscriber @" + generatePath(file) + ". Please make sure that each manager has 'package', 'type', 'topic' and 'sensor_name' entries.");
-    }
-
-    try {
-      for (const auto &kv : n_config["ROS publisher"]) {
-        WOLF_DEBUG("WHAT");
-          PublisherManager pPublisherManager = {kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv};
-          _publisher_managers.push_back(pPublisherManager);
-          map_container.push_back(std::map<std::string, std::string>({
-                                                                      {"type", kv["type"].Scalar()},
-                                                                      {"topic", kv["topic"].Scalar()},
-                                                                      {"period", kv["period"].Scalar()}}));
-      }
-      insert_register("ROS publisher", wolf::converter<std::string>::convert(map_container));
-      map_container.clear();
-    } catch (YAML::InvalidNode &e) {
-        throw std::runtime_error("Error parsing publisher @" + generatePath(file) + ". Please make sure that each manager has 'type', 'topic' and 'period' entries.");
-    }
-}
-std::map<std::string,std::string> ParserYAML::getParams(){
-    std::map<std::string,std::string> rtn = _params;
-    return rtn;
-}
-void ParserYAML::parse(){
-    this->_parsing_file.push(generatePath(this->_file));
-    this->parseFirstLevel(this->_file);
-
-    if(this->problem.Type() != YAML::NodeType::Undefined){
-      std::vector<std::string> tags = std::vector<std::string>();
-      this->walkTreeR(this->problem, tags , "problem");
-    }
-    for(auto it : _paramsSens){
-      std::vector<std::string> tags = std::vector<std::string>();
-      tags.push_back("sensor");
-      this->walkTreeR(it.n , tags , "sensor/" + it._name);
-    }
-    for(auto it : _paramsProc){
-      std::vector<std::string> tags = std::vector<std::string>();
-      tags.push_back("processor");
-      this->walkTreeR(it.n , tags , "processor/" + it._name);
-    }
-    std::list<std::string> plugins, packages;
-    for (const auto& it : this->_paramsSens)
-        plugins.push_back(it._plugin);
-    for (const auto& it : this->_paramsProc)
-        plugins.push_back(it._plugin);
-    for (const auto& it : this->_subscriber_managers)
-        packages.push_back(it._package);
-    plugins.sort();
-    plugins.unique();
-    packages.sort();
-    packages.unique();
-    insert_register("plugins", wolf::converter<std::string>::convert(plugins));
-    insert_register("packages", wolf::converter<std::string>::convert(packages));
-
-    YAML::Node n;
-    n = loadYAML(generatePath(this->_file));
-
-    if (n.Type() == YAML::NodeType::Map)
-        {
-            for (auto it : n)
-                {
-                    auto node = it.second;
-                    // WOLF_INFO("WUT ", it.first);
-                    std::vector<std::string> tags = std::vector<std::string>();
-                    if(it.first.as<std::string>() != "config")
-                        this->walkTreeR(node, tags, it.first.as<std::string>());
-                    else
-                        {
-                            for (auto itt : node)
-                                {
-                                    std::string node_key = itt.first.as<std::string>();
-                                    if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
-                                        node_key != "ROS subscriber" and node_key != "ROS publisher")
-                                        {
-                                            this->walkTreeR(itt.second, tags, node_key);
-                                        }
-                                }
-                        }
-                }
-        }else
-        {
-            std::vector<std::string> tags = std::vector<std::string>();
-            this->walkTreeR(n, tags, "");
-        }
-    this->_parsing_file.pop();
-    }
-void ParserYAML::parse_freely(){
-    this->_parsing_file.push(generatePath(this->_file));
-    std::vector<std::string> tags = std::vector<std::string>();
-    this->walkTreeR(loadYAML(this->_file), tags, "");
-    this->_parsing_file.pop();
-}
-void ParserYAML::insert_register(std::string key, std::string value){
-    if(key.substr(0,1) == "/") key = key.substr(1,key.size()-1);
-  auto inserted_it = _params.insert(std::pair<std::string, std::string>(key, value));
-  if(not inserted_it.second) WOLF_WARN("Skipping key '", key, "' with value '", value, "'. There already exists the register: (", inserted_it.first->first, ",", inserted_it.first->second, ")");
-}
-#endif
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index ecf57e7d7e29b4b8ccfecf09f8c39d8eacdafdbb..0d8cba6f97cb7e2c2cdf067e7ebcb65d8ac79c10 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -443,6 +443,77 @@ void CeresManager::check()
     }
 }
 
+void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
+{
+    for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
+        for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
+            original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
+
+    original.makeCompressed();
+}
+
+const Eigen::SparseMatrixd CeresManager::computeHessian() const
+{
+    Eigen::SparseMatrixd H;
+    Eigen::SparseMatrixd A;
+    // fac_2_residual_idx_
+    // fac_2_costfunction_
+    // state_blocks_local_param_
+    int ix_row = 0;  // index of the factor/measurement in the 
+    for (auto fac_res_pair: fac_2_residual_idx_){
+        FactorBasePtr fac_ptr = fac_res_pair.first;
+        ix_row += fac_ptr->getSize();
+
+        // retrieve all stateblocks data related to this factor
+        std::vector<const double*> fac_states_ptr;
+        for (auto sb : fac_res_pair.first->getStateBlockPtrVector()){
+            fac_states_ptr.push_back(sb->getStateData());
+        }
+        
+        // retrieve residual value, not used afterwards in this case since we just care about jacobians
+        Eigen::VectorXd residual(fac_ptr->getSize());
+        // retrieve jacobian in group size, not local size
+        std::vector<Eigen::MatrixXd> jacobians;
+        fac_ptr->evaluate(fac_states_ptr, residual, jacobians);
+
+        // Retrieve the block row sparse matrix of this factor
+        Eigen::SparseMatrixd A_block_row(fac_ptr->getSize(), A.cols());
+        for (auto i = 0; i < jacobians.size(); i++)
+        {
+            StateBlockPtr sb = fac_ptr->getStateBlockPtrVector()[i];
+            if (!sb->isFixed())
+            {
+                assert((state_blocks_local_param_.find(sb) != state_blocks_local_param_.end()) && "factor involving a state block not added");
+                assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols()) - 1 && "bad A number of cols");
+
+                // insert since A_block_row has just been created so it's empty for sure
+                if (sb->hasLocalParametrization()){
+                    // if the state block has a local parameterization, we need to right multiply by the manifold element / tangent element jacobian
+                    Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize());
+                    Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map(J_manif_tang.data(), sb->getSize(), sb->getLocalSize());
+                    Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize());
+
+                    sb->getLocalParametrization()->computeJacobian(sb_state_map, J_manif_tang_map);
+                    insertSparseBlock(jacobians[i] * J_manif_tang, A_block_row, 0, sb->getLocalSize());  // (to_insert, matrix_to_fill, row, col)
+                }
+                else {
+                    insertSparseBlock(jacobians[i], A_block_row, 0, sb->getLocalSize());
+                }
+            }
+        }
+        
+        // fill the weighted jacobian matrix block row
+        A.block(ix_row, 0, fac_ptr->getSize(), A.cols());
+
+    }
+
+    // compute the hessian matrix from the weighted jacobian 
+    H = A.transpose() * A;
+
+    return H;
+}
+
+
 } // namespace wolf
 #include "core/solver/solver_factory.h"
 namespace wolf {
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index e7fabaefc24245604cad750d19461d124a231432..c0f4d310e1062c9a68ea94a0ee0b70f912cd9d15 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -64,14 +64,14 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
            time_stamp_(_ts)
 {
     if(_frame_structure.compare("PO") == 0 and _dim == 2){
-        assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2d!");
+        assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for PO 2D!");
         StateBlockPtr p_ptr ( std::make_shared<StateBlock>    (_x.head    <2> ( ) ) );
         StateBlockPtr o_ptr ( std::make_shared<StateAngle>    ((double) _x(2) ) );
         addStateBlock("P", p_ptr);
         addStateBlock("O", o_ptr);
         this->setType("PO 2d");
     }else if(_frame_structure.compare("PO") == 0 and _dim == 3){
-        assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3d!");
+        assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for PO 3D!");
         StateBlockPtr p_ptr ( std::make_shared<StateBlock>      (_x.head    <3> ( ) ) );
         StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail    <4> ( ) ) );
         addStateBlock("P", p_ptr);
@@ -79,7 +79,7 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
         this->setType("PO 3d");
     }else if(_frame_structure.compare("POV") == 0 and _dim == 3){
         // auto _x = Eigen::VectorXd::Zero(10);
-        assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for 3d!");
+        assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for POV 3D!");
         StateBlockPtr p_ptr ( std::make_shared<StateBlock>      (_x.head    <3> ( ) ) );
         StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) );
         StateBlockPtr v_ptr ( std::make_shared<StateBlock>      (_x.tail    <3> ( ) ) );
@@ -87,6 +87,22 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
         addStateBlock("O", o_ptr);
         addStateBlock("V", v_ptr);
         this->setType("POV 3d");
+    }else if(_frame_structure.compare("POVCDL") == 0 and _dim == 3){
+        // auto _x = Eigen::VectorXd::Zero(10);
+        assert(_x.size() == 19 && "Wrong state vector size. Should be 19 for POVCDL 3D!");
+        StateBlockPtr p_ptr ( std::make_shared<StateBlock>      (_x.head    <3> (  ) ) );
+        StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3 ) ) );
+        StateBlockPtr v_ptr ( std::make_shared<StateBlock>      (_x.segment <3> (7 ) ) );
+        StateBlockPtr c_ptr ( std::make_shared<StateBlock>      (_x.segment <3> (10) ) );
+        StateBlockPtr cd_ptr ( std::make_shared<StateBlock>      (_x.segment <3> (13) ) );
+        StateBlockPtr Lc_ptr ( std::make_shared<StateBlock>      (_x.segment <3> (16) ) );
+        addStateBlock("P", p_ptr);
+        addStateBlock("O", o_ptr);
+        addStateBlock("V", v_ptr);
+        addStateBlock("C", c_ptr);
+        addStateBlock("D", cd_ptr);
+        addStateBlock("L", Lc_ptr);
+        this->setType("POVCDL 3d");
     }else{
         std::cout << _frame_structure << " ^^ " << _dim << std::endl;
         throw std::runtime_error("Unknown frame structure");
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index ccee9e6bf80f67e897db1ec84225457b96a9eff2..1d72f5b8fff0b187d192e575eb140a2e442f101e 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -12,8 +12,8 @@
 #include "core/processor/processor_factory.h"
 #include "core/state_block/state_block.h"
 #include "core/utils/logging.h"
-#include "core/utils/params_server.hpp"
-#include "core/utils/loader.hpp"
+#include "core/utils/params_server.h"
+#include "core/utils/loader.h"
 #include "core/utils/check_log.hpp"
 
 
@@ -35,25 +35,27 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
         hardware_ptr_(std::make_shared<HardwareBase>()),
         trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
         map_ptr_(std::make_shared<MapBase>()),
-        processor_motion_ptr_(),
+        processor_is_motion_list_(std::list<IsMotionPtr>()),
         prior_is_set_(false),
         frame_structure_(_frame_structure)
 {
+    dim_ = _dim;
     if (_frame_structure == "PO" and _dim == 2)
     {
         state_size_ = 3;
         state_cov_size_ = 3;
-        dim_ = 2;
     }else if (_frame_structure == "PO" and _dim == 3)
     {
         state_size_ = 7;
         state_cov_size_ = 6;
-        dim_ = 3;
-    } else if (_frame_structure == "POV" and _dim == 3)
+    }else if (_frame_structure == "POV" and _dim == 3)
     {
         state_size_ = 10;
         state_cov_size_ = 9;
-        dim_ = 3;
+    }else if (_frame_structure == "POVCDL" and _dim == 3)
+    {
+        state_size_ = 19;
+        state_cov_size_ = 18;
     }
     else std::runtime_error(
             "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
@@ -235,10 +237,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
     if (prc_ptr->isMotion() && prior_is_set_)
         (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
 
-    // setting the main processor motion
-    if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
-        processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(prc_ptr);
-
     return prc_ptr;
 }
 
@@ -284,10 +282,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
     if (prc_ptr->isMotion() && prior_is_set_)
         (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
 
-    // setting the main processor motion
-    if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
-        processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);
-
     return prc_ptr;
 }
 
@@ -306,47 +300,6 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
     return (*sen_it);
 }
 
-IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
-{
-    for (auto sen : getHardware()->getSensorList()) // loop all sensors
-    {
-        auto prc_it = std::find_if(sen->getProcessorList().begin(), // find processor by its name
-                                   sen->getProcessorList().end(),
-                                   [&](ProcessorBasePtr prc)
-                                   {
-                                       return prc->getName() == _processor_name;
-                                   }); // lambda function for the find_if
-
-        if (prc_it != sen->getProcessorList().end())  // found something!
-        {
-            if ((*prc_it)->isMotion()) // found, and it's motion!
-            {
-                std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl;
-                processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(*prc_it);
-                return processor_motion_ptr_;
-            }
-            else // found, but it's not motion!
-            {
-                std::cout << "Found processor '" << _processor_name << "', but not of type Motion!" << std::endl;
-                return nullptr;
-            }
-        }
-    }
-    // nothing found!
-    std::cout << "Processor '" << _processor_name << "' not found!" << std::endl;
-    return nullptr;
-}
-
-void Problem::setProcessorMotion(IsMotionPtr _processor_motion_ptr)
-{
-    processor_motion_ptr_ = _processor_motion_ptr;
-}
-
-void Problem::clearProcessorMotion()
-{
-    processor_motion_ptr_.reset();
-}
-
 FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
                                    const SizeEigen _dim, //
                                    FrameType _frame_key_type, //
@@ -387,41 +340,108 @@ Eigen::VectorXd Problem::getCurrentState() const
 
 void Problem::getCurrentState(Eigen::VectorXd& _state) const
 {
-    if (processor_motion_ptr_ != nullptr)
-        processor_motion_ptr_->getCurrentState(_state);
-    else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
-        trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
-    else
-        _state = zeroState();
+    TimeStamp ts;  // throwaway timestamp
+    getCurrentStateAndStamp(_state, ts);
 }
 
-void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& ts) const
-{
-    if (processor_motion_ptr_ != nullptr)
+void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) const
+{   
+    if (!processor_is_motion_list_.empty())
     {
-        processor_motion_ptr_->getCurrentState(_state);
-        processor_motion_ptr_->getCurrentTimeStamp(ts);
+        // retrieve the minimum of the most recent ts in all processor is motion then call getSate(ts, state)
+        std::list<TimeStamp> proc_is_motion_current_ts;
+        for (auto proc: processor_is_motion_list_){
+            proc_is_motion_current_ts.push_back(proc->getCurrentTimeStamp());
+        }
+        auto min_it = std::min_element(proc_is_motion_current_ts.begin(), proc_is_motion_current_ts.end());
+        getState(*min_it, _state);
+        _ts = *min_it;
     }
     else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
     {
-        trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(ts);
+        // kind of redundant with getState(_ts, _state)
+        trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(_ts);
         trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
     }
     else
         _state = zeroState();
 }
 
+
+// Problem of this implmentation: if more state
 void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
 {
-    // try to get the state from processor_motion if any, otherwise...
-    if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, _state))
-    {
-        FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+    // if _ts is too recent, for some of the processor is motion, they return the state corresponding to their last frame timestamp
+    FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+    if (processor_is_motion_list_.empty()){
         if (closest_frame != nullptr)
-            closest_frame->getState(_state);
+            _state = closest_frame->getState();
         else
             _state = zeroState();
     }
+
+    // RETRIEVE FROM PROCESSOR MOTION
+    // TODO: current implementation messy, would be much easier with a state being an std::unordered_map
+    else {
+        // Iterate over the problem state structure and get the corresponding state
+        // in the first processor is motion that provides it
+        // finally check if the state to concatenate list has the same total size as the problem state_size 
+        
+        // an map is necessary as a temporary structure because we do not know in which order we will find the state blocks in processors 
+        std::unordered_map<char, Eigen::VectorXd> states_to_concat_map;  // not necessary to be ordered
+        for (auto proc: processor_is_motion_list_){
+            Eigen::VectorXd proc_state = proc->getState(_ts);
+
+            int idx = 0;
+            for (char sb_name: proc->getStateStructure()){
+                // not already there
+                if (states_to_concat_map.find(sb_name) == states_to_concat_map.end()){
+                    if (sb_name == 'O'){
+                        int size_sb = dim_ == 3 ? 4 : 1;  // really bad: should be more transparent
+                        states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
+                        idx += size_sb;
+                    }
+                    else{
+                        int size_sb = dim_ == 3 ? 3 : 2;
+                        states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
+                        idx += size_sb;
+                    } 
+                }
+            }
+        }
+
+        int concat_size = 0;
+        for (auto state_map_it: states_to_concat_map){
+            concat_size += state_map_it.second.size();
+        }
+        // assert(concat_size == state_size_  && "Problem with the concatenated size: " );
+
+        // fill the state value from the state concatenation in the order dictated by frame_structure_
+        int idx = 0;
+        _state.resize(state_size_);
+        for (char sb_name: frame_structure_){
+            Eigen::VectorXd sb_state;
+            int size_sb;  // really bad...
+            if (sb_name == 'O'){
+                size_sb = dim_ == 3 ? 4 : 1;
+            }
+            else {
+                size_sb = dim_ == 3 ? 3 : 2;
+            }
+            if (states_to_concat_map.find(sb_name) != states_to_concat_map.end()){
+                sb_state = states_to_concat_map[sb_name];
+            }
+            else {
+                // Should be taken from the last state but too messy already
+                sb_state.resize(size_sb); 
+                sb_state.setZero();
+            }
+
+            _state.segment(idx, size_sb) = sb_state;
+            idx += size_sb;
+
+        }
+    }
 }
 
 Eigen::VectorXd Problem::getState(const TimeStamp& _ts) const
@@ -451,6 +471,18 @@ std::string Problem::getFrameStructure() const
     return frame_structure_;
 }
 
+void Problem::addProcessorIsMotion(IsMotionPtr _processor_motion_ptr)
+{
+    processor_is_motion_list_.push_back(_processor_motion_ptr);
+}
+
+void Problem::clearProcessorIsMotion(IsMotionPtr proc){
+    auto it = std::find(processor_is_motion_list_.begin(), processor_is_motion_list_.end(), proc);
+    if (it != processor_is_motion_list_.end()){
+        processor_is_motion_list_.erase(it);
+    }
+}
+
 Eigen::VectorXd Problem::zeroState() const
 {
     Eigen::VectorXd state = Eigen::VectorXd::Zero(getFrameStructureSize());
@@ -846,10 +878,16 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXd& _prior_state, const Eigen:
         // create origin capture with the given state as data
         // Capture fix only takes 3d position and Quaternion orientation
         CapturePosePtr init_capture;
-        if (this->getFrameStructure() == "POV" and this->getDim() == 3)
+        // if (this->getFrameStructure() == "POV" and this->getDim() == 3)
+        //     init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
+        // else
+        //     init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
+
+        if (this->getDim() == 3)
             init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
         else
-            init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
+            init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(3), _prior_cov.topLeftCorner(3,3));
+
 
         // emplace feature and factor
         init_capture->emplaceFeatureAndFactor();
@@ -1827,9 +1865,12 @@ void Problem::perturb(double amplitude)
     // Frames and Captures
     for (auto& F : getTrajectory()->getFrameList())
     {
-        F->perturb(amplitude);
-        for (auto& C : F->getCaptureList())
-            C->perturb(amplitude);
+        if (F->isKeyOrAux())
+        {
+            F->perturb(amplitude);
+            for (auto& C : F->getCaptureList())
+                C->perturb(amplitude);
+        }
     }
 
     // Landmarks
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index a35097f2c70d3b047117ae6d0c9bf81a5fe2f5da..f61f861b021e51a08dd92db301aa1bd8de159272 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -53,12 +53,13 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
     assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture");
     WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
 
-    // if trigger, process directly without buffering
-    if (triggerInCapture(_capture_ptr))
-        processCapture(_capture_ptr);
     // asking if capture should be stored
     if (storeCapture(_capture_ptr))
         buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr);
+
+    // if trigger, process directly without buffering
+    if (triggerInCapture(_capture_ptr))
+        processCapture(_capture_ptr);
 }
 
 void ProcessorBase::remove()
@@ -68,12 +69,12 @@ void ProcessorBase::remove()
         is_removing_ = true;
         ProcessorBasePtr this_p = shared_from_this();
 
-        // clear Problem::processor_motion_ptr_
         if (isMotion())
         {
             ProblemPtr P = getProblem();
-            if(P && (P->getProcessorMotion() == std::dynamic_pointer_cast<IsMotion>( shared_from_this() ) ) )
-                P->clearProcessorMotion();
+            auto this_proc_cast_attempt = std::dynamic_pointer_cast<IsMotion>( shared_from_this() );
+            if(P && this_proc_cast_attempt )
+                P->clearProcessorIsMotion(this_proc_cast_attempt);
         }
 
         // remove from upstream
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 634d1f6d1284997573ef763bd329dcdf37cc94d3..aded1532cc17fc6a0f2e0d48edbbca857a1ab28b 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -7,6 +7,7 @@ namespace wolf
 {
 
 ProcessorMotion::ProcessorMotion(const std::string& _type,
+                                 std::string _state_structure,
                                  int _dim,
                                  SizeEigen _state_size,
                                  SizeEigen _delta_size,
@@ -25,7 +26,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
         origin_ptr_(),
         last_ptr_(),
         incoming_ptr_(),
-        dt_(0.0), x_(_state_size),
+        dt_(0.0), 
+        x_(_state_size),
         delta_(_delta_size),
         delta_cov_(_delta_cov_size, _delta_cov_size),
         delta_integrated_(_delta_size),
@@ -34,7 +36,8 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
         jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
         jacobian_delta_(delta_cov_size_, delta_cov_size_),
         jacobian_calib_(delta_cov_size_, calib_size_)
-{
+{   
+    setStateStructure(_state_structure);
     //
 }
 
@@ -267,7 +270,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
     // Update state and time stamps
     last_ptr_->setTimeStamp(getCurrentTimeStamp());
     last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp());
-    last_ptr_->getFrame()->setState(getCurrentState());
+    last_ptr_->getFrame()->setState(getProblem()->getState(getCurrentTimeStamp()));
 
     if (permittedKeyFrame() && voteForKeyFrame())
     {
@@ -315,7 +318,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
 
         // create a new frame
         auto frame_new      = getProblem()->emplaceFrame(NON_ESTIMATED,
-                                                        getCurrentState(),
+                                                        getProblem()->getCurrentState(),
                                                         getCurrentTimeStamp());
         // create a new capture
         auto capture_new    = emplaceCapture(frame_new,
@@ -346,6 +349,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
     postProcess();
 }
 
+// _x needs to have the size of the processor state
 bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
 {
     CaptureMotionPtr capture_motion;
@@ -360,7 +364,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
     {
         // Get origin state and calibration
         CaptureBasePtr cap_orig   = capture_motion->getOriginCapture();
-        VectorXd state_0          = cap_orig->getFrame()->getState();
+        VectorXd state_0          = cap_orig->getFrame()->getState(state_structure_);
         VectorXd calib            = cap_orig->getCalibration();
 
         // Get delta and correct it with new calibration params
@@ -371,7 +375,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
         VectorXd delta            = capture_motion->correctDelta( motion.delta_integr_, delta_step);
 
         // ensure proper size of the provided reference
-        _x.resize( getProblem()->getFrameStructureSize() );
+        _x.resize( state_0.size() );
 
         // Compose on top of origin state using the buffered time stamp, not the query time stamp
         double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_;
@@ -380,7 +384,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
     else
     {
         // We could not find any CaptureMotion for the time stamp requested
-        WOLF_ERROR("Could not find any Capture for the time stamp requested. ");
+        WOLF_ERROR("Could not find any Capture for the time stamp requested. ", _ts);
         WOLF_TRACE("Did you forget to call Problem::setPrior() in your application?")
         return false;
     }
@@ -415,9 +419,11 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
 
     // ---------- LAST ----------
     // Make non-key-frame for last Capture
+    TimeStamp origin_ts = _origin_frame->getTimeStamp();
     auto new_frame_ptr  = getProblem()->emplaceFrame(NON_ESTIMATED,
-                                                     _origin_frame->getState(),
-                                                     _origin_frame->getTimeStamp());
+                                                      _origin_frame->getState(),
+                                                     origin_ts);
+                                        
     // emplace (emtpy) last Capture
     last_ptr_ = emplaceCapture(new_frame_ptr,
                                getSensor(),
@@ -562,7 +568,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
             ++frame_rev_iter)
     {
         frame   = *frame_rev_iter;
-        capture = frame->getCaptureOf(getSensor());
+        auto sensor = getSensor();
+        capture = frame->getCaptureOf(sensor);
         if (capture != nullptr)
         {
             // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
@@ -621,6 +628,9 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
 
 void ProcessorMotion::setProblem(ProblemPtr _problem)
 {
+    std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
+    assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
+
     if (_problem == nullptr or _problem == this->getProblem())
         return;
 
@@ -629,11 +639,10 @@ void ProcessorMotion::setProblem(ProblemPtr _problem)
     // set the origin
     if (origin_ptr_ == nullptr && this->getProblem()->getLastKeyFrame() != nullptr)
         this->setOrigin(this->getProblem()->getLastKeyFrame());
-
-    // set the main processor motion
-    if (this->getProblem()->getProcessorMotion() == nullptr)
-        this->getProblem()->setProcessorMotion(std::static_pointer_cast<ProcessorMotion>(shared_from_this()));
-};
+    
+    // adding processor is motion to the processor is motion vector
+    getProblem()->addProcessorIsMotion(std::dynamic_pointer_cast<IsMotion>(shared_from_this()));
+}
 
 bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr)
 {
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index c986ebf7cf9cd7fb83df1417ea19c42beed66c74..929c1ed7ceb825fdaa265a1af592fa8125832789 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -6,7 +6,7 @@ namespace wolf
 {
 
 ProcessorOdom2d::ProcessorOdom2d(ProcessorParamsOdom2dPtr _params) :
-                ProcessorMotion("ProcessorOdom2d", 2, 3, 3, 3, 2, 0, _params),
+                ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params),
                 params_odom_2d_(_params)
 {
     unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3d::Identity();
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 967031e9ccd1a68fcee41567be0f7dbf7523ef2c..3e1268eb1d705b3a335d9c8d9389e0c9449ebba1 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -3,7 +3,7 @@ namespace wolf
 {
 
 ProcessorOdom3d::ProcessorOdom3d(ProcessorParamsOdom3dPtr _params) :
-                        ProcessorMotion("ProcessorOdom3d", 3, 7, 7, 6, 6, 0, _params),
+                        ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params),
                         params_odom_3d_ (_params),
                         k_disp_to_disp_ (0),
                         k_disp_to_rot_  (0),
diff --git a/src/utils/loader.cpp b/src/utils/loader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9fdf6984b6a3de57fa8a8ae062232a685897209f
--- /dev/null
+++ b/src/utils/loader.cpp
@@ -0,0 +1,37 @@
+#include "core/utils/loader.h"
+
+#include <dlfcn.h>
+#include <stdexcept>
+
+Loader::Loader(std::string _file)
+{
+    path_ = _file;
+}
+LoaderRaw::LoaderRaw(std::string _file) : Loader(_file)
+{
+    //
+}
+LoaderRaw::~LoaderRaw()
+{
+    close();
+}
+void LoaderRaw::load()
+{
+    resource_ = dlopen(path_.c_str(), RTLD_LAZY);
+    if (not resource_)
+        throw std::runtime_error("Couldn't load resource with path " + path_ + "\n" + "Error info: " + dlerror());
+}
+void LoaderRaw::close()
+{
+    if (resource_)
+        dlclose(resource_);
+}
+// class LoaderPlugin: public Loader{
+//     ClassLoader* resource_;
+//     void load(){
+//         resource_ = new ClassLoader(path_);
+//     }
+//     void close(){
+//         delete resource_;
+//     }
+// };
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6344466792706c9e499695721f95e781089ec07e
--- /dev/null
+++ b/src/utils/params_server.cpp
@@ -0,0 +1,28 @@
+#include "core/utils/params_server.h"
+
+using namespace wolf;
+
+ParamsServer::ParamsServer()
+{
+    params_ = std::map<std::string, std::string>();
+}
+ParamsServer::ParamsServer(std::map<std::string, std::string> _params)
+{
+    params_ = _params;
+}
+
+void ParamsServer::print()
+{
+    for (auto it : params_)
+        std::cout << it.first << "~~" << it.second << std::endl;
+}
+
+void ParamsServer::addParam(std::string _key, std::string _value)
+{
+    params_.insert(std::pair<std::string, std::string>(_key, _value));
+}
+
+void ParamsServer::addParams(std::map<std::string, std::string> _params)
+{
+    params_.insert(_params.begin(), _params.end());
+}
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..267d0c9da2024abd59ed5b529d13de7680d14472
--- /dev/null
+++ b/src/yaml/parser_yaml.cpp
@@ -0,0 +1,547 @@
+#include "core/yaml/parser_yaml.h"
+
+#include <string>
+#include <vector>
+#include <list>
+#include <stack>
+#include <regex>
+#include <map>
+#include <iostream>
+#include <algorithm>
+#include <numeric>
+
+using namespace wolf;
+namespace {
+  //====== START OF FORWARD DECLARATION ========
+  std::string parseAtomicNode(YAML::Node);
+  std::string fetchMapEntry(YAML::Node);
+  std::string mapToString(std::map<std::string,std::string>);
+  //====== END OF FORWARD DECLARATION ========
+
+/** @Brief Interprets a map as being atomic and thus parses it as a single entity. We assume that the map has as values only scalars and sequences.
+ *  @param n the node representing a map
+ *  @return std::map<std::string, std::string> populated with the key,value pairs in n
+ */
+std::map<std::string, std::string> fetchAsMap(YAML::Node _n){
+    assert(_n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node");
+    auto m = std::map<std::string, std::string>();
+    for(const auto& kv : _n){
+        std::string key = kv.first.as<std::string>();
+        switch (kv.second.Type()) {
+        case YAML::NodeType::Scalar : {
+            std::string value = kv.second.Scalar();
+            m.insert(std::pair<std::string,std::string>(key, value));
+            break;
+        }
+        case YAML::NodeType::Sequence : {
+            std::string aux = parseAtomicNode(kv.second);
+            m.insert(std::pair<std::string,std::string>(key, aux));
+            break;
+        }
+        case YAML::NodeType::Map : {
+          std::string value = fetchMapEntry(kv.second);
+          std::regex r("^\\$.*");
+          if (std::regex_match(key, r)) key = key.substr(1,key.size()-1);
+          m.insert(std::pair<std::string,std::string>(key, value));
+          break;
+        }
+        default:
+            assert(1 == 0 && "Unsupported node Type at fetchAsMap");
+            break;
+        }
+    }
+    return m;
+}
+  std::string fetchMapEntry(YAML::Node _n){
+    switch (_n.Type()) {
+    case YAML::NodeType::Scalar: {
+      return _n.Scalar();
+      break;
+    }
+    case YAML::NodeType::Sequence: {
+      return parseAtomicNode(_n);
+      break;
+    }
+    case YAML::NodeType::Map: {
+      return mapToString(fetchAsMap(_n));
+      break;
+    }
+    default: {
+      assert(1 == 0 && "Unsupported node Type at fetchMapEntry");
+      return "";
+      break;
+    }
+    }
+  }
+    /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...]
+    * @param map_ just a std::map<std::string,std::string>
+    * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...]
+    */
+    std::string mapToString(std::map<std::string,std::string> _map){
+        std::string result = "";
+        auto v = std::vector<std::string>();
+        std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<std::string,std::string> p){return "{" + p.first + ":" + p.second + "}";});
+        auto concat = [](std::string ac, std::string str)-> std::string {
+                          return ac + str + ",";
+                      };
+        std::string aux = "";
+        std::string accumulated = std::accumulate(v.begin(), v.end(), aux, concat);
+        if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1);
+        else accumulated = "";
+        return "[" + accumulated + "]";
+    }
+    /** @Brief Generates a std::string representing a YAML sequence. The sequence is assumed to be scalar or at most be a sequence of sequences of scalars.
+    * @param n a vector of YAML::Node that represents a YAML::Sequence
+    * @return <b>{std::string}</b> representing the YAML sequence
+    */
+    std::string parseAtomicNode(YAML::Node _n){
+      std::string aux = "";
+      bool first = true;
+      std::string separator = "";
+      switch(_n.Type()){
+      case YAML::NodeType::Scalar:
+        return _n.Scalar();
+        break;
+      case YAML::NodeType::Sequence:
+        for(auto it : _n){
+          aux += separator + parseAtomicNode(it);
+          if(first){
+            separator = ",";
+            first = false;
+          }
+        }
+        return "[" + aux + "]";
+        break;
+      case YAML::NodeType::Map:
+        return mapToString(fetchAsMap(_n));
+        break;
+      default:
+        return "";
+        break;
+      }
+    }
+
+    /** @Brief checks if a node of the YAML tree is atomic. Only works if the nodes are of type
+     * Scalar, Sequence or Map.
+     * @param key is the key associated to the node n if n.Type() == YAML::NodeType::Map
+     * @param n node to be test for atomicity
+    */
+    bool isAtomic(std::string _key, YAML::Node _n){
+      assert(_n.Type() != YAML::NodeType::Undefined && _n.Type() != YAML::NodeType::Null && "Cannot determine atomicity of Undefined/Null node");
+      std::regex r("^\\$.*");
+      bool is_atomic = true;
+      switch(_n.Type()){
+      case YAML::NodeType::Scalar:
+        return true;
+        break;
+      case YAML::NodeType::Sequence:
+        for(auto it : _n) {
+          switch(it.Type()){
+          case YAML::NodeType::Map:
+            for(const auto& kv : it){
+              is_atomic = is_atomic and isAtomic(kv.first.as<std::string>(), it);
+            }
+            break;
+          default:
+            is_atomic = is_atomic and isAtomic("", it);
+            break;
+          }
+        }
+        return is_atomic;
+        break;
+      case YAML::NodeType::Map:
+        is_atomic = std::regex_match(_key, r);
+        return is_atomic;
+        break;
+      default:
+        throw std::runtime_error("Cannot determine atomicity of node type " + std::to_string(_n.Type()));
+        return false;
+        break;
+      }
+      return false;
+    }
+}
+ParserYAML::ParserYAML(std::string _file, std::string path_root, bool freely_parse)
+{
+    params_              = std::map<std::string, std::string>();
+    active_name_         = "";
+    paramsSens_          = std::vector<ParamsInitSensor>();
+    paramsProc_          = std::vector<ParamsInitProcessor>();
+    subscriber_managers_ = std::vector<SubscriberManager>();
+    publisher_managers_  = std::vector<PublisherManager>();
+    parsing_file_        = std::stack<std::string>();
+    file_                = _file;
+    if (path_root != "")
+    {
+        std::regex r(".*/ *$");
+        if (not std::regex_match(path_root, r))
+            path_root_ = path_root + "/";
+        else
+            path_root_ = path_root;
+    }
+    if (not freely_parse)
+        parse();
+    else
+        parse_freely();
+}
+
+std::string ParserYAML::generatePath(std::string _file)
+{
+    std::regex r("^/.*");
+    if (std::regex_match(_file, r))
+    {
+        return _file;
+    }
+    else
+    {
+        return path_root_ + _file;
+    }
+}
+YAML::Node ParserYAML::loadYAML(std::string _file)
+{
+    try
+    {
+        // std::cout << "Parsing " << generatePath(_file) << std::endl;
+        WOLF_INFO("Parsing file: ", generatePath(_file));
+        return YAML::LoadFile(generatePath(_file));
+    }
+    catch (YAML::BadFile& e)
+    {
+        throw std::runtime_error("Couldn't load file " + generatePath(_file) + ". Tried to open it from " +
+                                 parsing_file_.top());
+    }
+}
+std::string ParserYAML::tagsToString(std::vector<std::string>& _tags)
+{
+    std::string hdr = "";
+    for (auto it : _tags)
+    {
+        hdr = hdr + "/" + it;
+    }
+    return hdr;
+}
+void ParserYAML::walkTree(std::string _file)
+{
+    YAML::Node n;
+    n = loadYAML(generatePath(_file));
+    parsing_file_.push(generatePath(_file));
+    std::vector<std::string> hdrs = std::vector<std::string>();
+    walkTreeR(n, hdrs, "");
+    parsing_file_.pop();
+}
+void ParserYAML::walkTree(std::string _file, std::vector<std::string>& _tags)
+{
+    YAML::Node n;
+    n = loadYAML(generatePath(_file));
+    parsing_file_.push(generatePath(_file));
+    walkTreeR(n, _tags, "");
+    parsing_file_.pop();
+}
+void ParserYAML::walkTree(std::string _file, std::vector<std::string>& _tags, std::string hdr)
+{
+    YAML::Node n;
+    n = loadYAML(generatePath(_file));
+    parsing_file_.push(generatePath(_file));
+    walkTreeR(n, _tags, hdr);
+    parsing_file_.pop();
+}
+void ParserYAML::walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string hdr)
+{
+    switch (_n.Type())
+    {
+        case YAML::NodeType::Scalar:
+        {
+            std::regex r("^@.*");
+            if (std::regex_match(_n.Scalar(), r))
+            {
+                std::string str = _n.Scalar();
+                walkTree(str.substr(1, str.size() - 1), _tags, hdr);
+            }
+            else
+            {
+                insert_register(hdr, _n.Scalar());
+            }
+            break;
+        }
+        case YAML::NodeType::Sequence:
+        {
+            if (isAtomic("", _n))
+            {
+                insert_register(hdr, parseAtomicNode(_n));
+            }
+            else
+            {
+                for (const auto& kv : _n)
+                {
+                    walkTreeR(kv, _tags, hdr);
+                }
+            }
+            break;
+        }
+        case YAML::NodeType::Map:
+        {
+            for (const auto& kv : _n)
+            {
+                if (isAtomic(kv.first.as<std::string>(), _n))
+                {
+                    std::string key = kv.first.as<std::string>();
+                    // WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key);
+                    key = key.substr(1, key.size() - 1);
+                    insert_register(hdr + "/" + key, parseAtomicNode(kv.second));
+                }
+                else
+                {
+                    /*
+                      If key=="follow" then the parser will assume that the value is a path and will parse
+                      the (expected) yaml file at the specified path. Note that this does not increase the header depth.
+                      The following example shows how the header remains unafected:
+                      @my_main_config                |  @some_path
+                      - cov_det: 1                   |  - my_value : 23
+                      - follow: "@some_path"         |
+                      - var: 1.2                     |
+                      Resulting map:
+                      cov_det -> 1
+                      my_value-> 23
+                      var: 1.2
+                      Instead of:
+                      cov_det -> 1
+                      follow/my_value-> 23
+                      var: 1.2
+                      Which would result from the following yaml files
+                      @my_main_config                |  @some_path
+                      - cov_det: 1                   |  - my_value : 23
+                      - $follow: "@some_path"        |
+                      - var: 1.2                     |
+                    */
+                    std::string key = kv.first.as<std::string>();
+                    // WOLF_DEBUG("KEY IN MAP NON ATOMIC ", key);
+                    std::regex rr("follow");
+                    if (not std::regex_match(kv.first.as<std::string>(), rr))
+                    {
+                        _tags.push_back(kv.first.as<std::string>());
+                        if (_tags.size() == 2)
+                            updateActiveName(kv.first.as<std::string>());
+                        walkTreeR(kv.second, _tags, hdr + "/" + kv.first.as<std::string>());
+                        _tags.pop_back();
+                        if (_tags.size() == 1)
+                            updateActiveName("");
+                    }
+                    else
+                    {
+                        walkTree(kv.second.as<std::string>(), _tags, hdr);
+                    }
+                }
+            }
+            break;
+        }
+        case YAML::NodeType::Null:
+            break;
+        default:
+            assert(1 == 0 && "Unsupported node Type at walkTreeR.");
+            break;
+    }
+}
+void ParserYAML::updateActiveName(std::string _tag)
+{
+    active_name_ = _tag;
+}
+void ParserYAML::parseFirstLevel(std::string _file)
+{
+    YAML::Node n;
+    n = loadYAML(generatePath(_file));
+
+    YAML::Node n_config = n["config"];
+    // assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node");
+    if (n_config.Type() != YAML::NodeType::Map)
+        throw std::runtime_error("Could not find config node. Please make sure that your YAML file " +
+                                 generatePath(_file) + " starts with 'config:'");
+    if (n_config["problem"].Type() != YAML::NodeType::Map)
+        throw std::runtime_error("Could not find problem node. Please make sure that the 'config' node in YAML file " +
+                                 generatePath(_file) + " has a 'problem' entry");
+    problem = n_config["problem"];
+    std::vector<std::map<std::string, std::string>> map_container;
+    try
+    {
+        for (const auto& kv : n_config["sensors"])
+        {
+            ParamsInitSensor pSensor = { kv["type"].Scalar(), kv["name"].Scalar(), kv["plugin"].Scalar(), kv };
+            paramsSens_.push_back(pSensor);
+            map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+                                                                         { "name", kv["name"].Scalar() },
+                                                                         { "plugin", kv["plugin"].Scalar() } }));
+        }
+        insert_register("sensors", wolf::converter<std::string>::convert(map_container));
+        map_container.clear();
+    }
+    catch (YAML::InvalidNode& e)
+    {
+        throw std::runtime_error("Error parsing sensors @" + generatePath(_file) +
+                                 ". Please make sure that each sensor entry has 'type', 'name' and 'plugin' entries.");
+    }
+
+    try
+    {
+        for (const auto& kv : n_config["processors"])
+        {
+            ParamsInitProcessor pProc = {
+                kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv["plugin"].Scalar(), kv
+            };
+            paramsProc_.push_back(pProc);
+            map_container.push_back(
+                std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+                                                     { "name", kv["name"].Scalar() },
+                                                     { "plugin", kv["plugin"].Scalar() },
+                                                     { "sensor_name", kv["sensor_name"].Scalar() } }));
+        }
+        insert_register("processors", wolf::converter<std::string>::convert(map_container));
+        map_container.clear();
+    }
+    catch (YAML::InvalidNode& e)
+    {
+        throw std::runtime_error("Error parsing processors @" + generatePath(_file) +
+                                 ". Please make sure that each processor has 'type', 'name', 'plugin' and "
+                                 "'sensor_name' entries.");
+    }
+    try
+    {
+        for (const auto& kv : n_config["ROS subscriber"])
+        {
+            SubscriberManager pSubscriberManager = {
+                kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["sensor_name"].Scalar(), kv
+            };
+            subscriber_managers_.push_back(pSubscriberManager);
+            map_container.push_back(
+                std::map<std::string, std::string>({ { "package", kv["package"].Scalar() },
+                                                     { "type", kv["type"].Scalar() },
+                                                     { "topic", kv["topic"].Scalar() },
+                                                     { "sensor_name", kv["sensor_name"].Scalar() } }));
+        }
+        insert_register("ROS subscriber", wolf::converter<std::string>::convert(map_container));
+        map_container.clear();
+    }
+    catch (YAML::InvalidNode& e)
+    {
+        throw std::runtime_error("Error parsing subscriber @" + generatePath(_file) +
+                                 ". Please make sure that each manager has 'package', 'type', 'topic' and "
+                                 "'sensor_name' entries.");
+    }
+
+    try
+    {
+        for (const auto& kv : n_config["ROS publisher"])
+        {
+            WOLF_DEBUG("WHAT");
+            PublisherManager pPublisherManager = {
+                kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv
+            };
+            publisher_managers_.push_back(pPublisherManager);
+            map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
+                                                                         { "topic", kv["topic"].Scalar() },
+                                                                         { "period", kv["period"].Scalar() } }));
+        }
+        insert_register("ROS publisher", wolf::converter<std::string>::convert(map_container));
+        map_container.clear();
+    }
+    catch (YAML::InvalidNode& e)
+    {
+        throw std::runtime_error("Error parsing publisher @" + generatePath(_file) +
+                                 ". Please make sure that each manager has 'type', 'topic' and 'period' entries.");
+    }
+}
+std::map<std::string, std::string> ParserYAML::getParams()
+{
+    std::map<std::string, std::string> rtn = params_;
+    return rtn;
+}
+void ParserYAML::parse()
+{
+    parsing_file_.push(generatePath(file_));
+    parseFirstLevel(file_);
+
+    if (problem.Type() != YAML::NodeType::Undefined)
+    {
+        std::vector<std::string> tags = std::vector<std::string>();
+        walkTreeR(problem, tags, "problem");
+    }
+    for (auto it : paramsSens_)
+    {
+        std::vector<std::string> tags = std::vector<std::string>();
+        tags.push_back("sensor");
+        walkTreeR(it.n_, tags, "sensor/" + it.name_);
+    }
+    for (auto it : paramsProc_)
+    {
+        std::vector<std::string> tags = std::vector<std::string>();
+        tags.push_back("processor");
+        walkTreeR(it.n_, tags, "processor/" + it.name_);
+    }
+    std::list<std::string> plugins, packages;
+    for (const auto& it : paramsSens_)
+        plugins.push_back(it.plugin_);
+    for (const auto& it : paramsProc_)
+        plugins.push_back(it.plugin_);
+    for (const auto& it : subscriber_managers_)
+        packages.push_back(it.package_);
+    plugins.sort();
+    plugins.unique();
+    packages.sort();
+    packages.unique();
+    insert_register("plugins", wolf::converter<std::string>::convert(plugins));
+    insert_register("packages", wolf::converter<std::string>::convert(packages));
+
+    YAML::Node n;
+    n = loadYAML(generatePath(file_));
+
+    if (n.Type() == YAML::NodeType::Map)
+    {
+        for (auto it : n)
+        {
+            auto node = it.second;
+            // WOLF_INFO("WUT ", it.first);
+            std::vector<std::string> tags = std::vector<std::string>();
+            if (it.first.as<std::string>() != "config")
+                walkTreeR(node, tags, it.first.as<std::string>());
+            else
+            {
+                for (auto itt : node)
+                {
+                    std::string node_key = itt.first.as<std::string>();
+                    if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
+                        node_key != "ROS subscriber" and node_key != "ROS publisher")
+                    {
+                        walkTreeR(itt.second, tags, node_key);
+                    }
+                }
+            }
+        }
+    }
+    else
+    {
+        std::vector<std::string> tags = std::vector<std::string>();
+        walkTreeR(n, tags, "");
+    }
+    parsing_file_.pop();
+}
+void ParserYAML::parse_freely()
+{
+    parsing_file_.push(generatePath(file_));
+    std::vector<std::string> tags = std::vector<std::string>();
+    walkTreeR(loadYAML(file_), tags, "");
+    parsing_file_.pop();
+}
+void ParserYAML::insert_register(std::string _key, std::string _value)
+{
+    if (_key.substr(0, 1) == "/")
+        _key = _key.substr(1, _key.size() - 1);
+    auto inserted_it = params_.insert(std::pair<std::string, std::string>(_key, _value));
+    if (not inserted_it.second)
+        WOLF_WARN("Skipping key '",
+                  _key,
+                  "' with value '",
+                  _value,
+                  "'. There already exists the register: (",
+                  inserted_it.first->first,
+                  ",",
+                  inserted_it.first->second,
+                  ")");
+}
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index 20bbe2c42ed27688820569a3da345596da3903f1..1e06c7a80377b99197a721e85473319f9bf34862 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.cpp
@@ -50,18 +50,17 @@ class FixtureFactorBlockDifference : public testing::Test
 
             Vector10d x_origin = problem_->zeroState();
             Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity(); 
-            KF0_ =problem_->setPrior(x_origin, cov_prior, t0, 0.1);
+            KF0_ = problem_->setPrior(x_origin, cov_prior, t0, 0.1);
             
             CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
             FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
             FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, KF0_->getV());
 
-            // KF0_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t0);
             KF1_ = problem_->emplaceFrame(KEY, problem_->zeroState(), t1);
 
-            Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "DIFF", t1);
+            Cap_ = CaptureBase::emplace<CaptureBase>(KF1_, "BlockDifference", t1);
             Eigen::Matrix3d cov = 0.2 * Eigen::Matrix3d::Identity();
-            Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "DIFF", zero3, cov);
+            Feat_ = FeatureBase::emplace<FeatureBase>(Cap_, "BlockDifference", zero3, cov);
         }
 
         virtual void TearDown() override {}
@@ -150,6 +149,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPx)
     Feat_->setMeasurementCovariance(cov_diff);
     FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
         Feat_, KF0_->getP(), KF1_->getP(),
+        nullptr, nullptr, nullptr, nullptr,
         0, 1, 0, 1
     );
 
@@ -174,6 +174,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPxy)
     Feat_->setMeasurementCovariance(cov_diff);
     FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
         Feat_, KF0_->getP(), KF1_->getP(),
+        nullptr, nullptr, nullptr, nullptr,
         0, 2, 0, 2
     );
 
@@ -195,6 +196,7 @@ TEST_F(FixtureFactorBlockDifference, DiffPyz)
     Feat_->setMeasurementCovariance(cov_diff);
     FactorBlockDifferencePtr Fac = FactorBase::emplace<FactorBlockDifference>(
         Feat_, KF0_->getP(), KF1_->getP(),
+        nullptr, nullptr, nullptr, nullptr,
         1, 2, 1, 2
     );
 
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index 18f6b1eded0955faf7c6b823e25bbde62894203d..1f1f468360e1aa5073606f8ea32e7d82cafbc438 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -1,8 +1,8 @@
 #include "core/utils/utils_gtest.h"
 #include "core/utils/converter.h"
 #include "core/common/wolf.h"
-#include "core/yaml/parser_yaml.hpp"
-#include "core/utils/params_server.hpp"
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
 
 using namespace std;
 using namespace wolf;
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index c8702619bc5150eb688bcb7dccfa11f71ab909e5..de5164ec6a9db61ad8a0344e8e6ee6d6e52dec16 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -1,7 +1,7 @@
 #include "core/utils/utils_gtest.h"
 #include "core/utils/converter.h"
 #include "core/common/wolf.h"
-#include "core/yaml/parser_yaml.hpp"
+#include "core/yaml/parser_yaml.h"
 
 using namespace std;
 using namespace wolf;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index d25f1451492f90d90613b5a08882ee02ae9c3f4d..6d262ba06b81438499b29ab3d45fd4bb101c89db 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -98,7 +98,7 @@ TEST(Problem, Processor)
     ProblemPtr P = Problem::create("PO", 3);
 
     // check motion processor is null
-    ASSERT_FALSE(P->getProcessorMotion());
+    ASSERT_FALSE(P->getProcessorIsMotion());
 
     // add a motion sensor and processor
     auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), ParamsSensorOdom3d());
@@ -107,7 +107,7 @@ TEST(Problem, Processor)
     auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ProcessorParamsOdom3d>());
 
     // check motion processor IS NOT by emplace
-    ASSERT_EQ(P->getProcessorMotion(), Pm);
+    ASSERT_EQ(P->getProcessorIsMotion(), Pm);
 }
 
 TEST(Problem, Installers)
@@ -122,16 +122,16 @@ TEST(Problem, Installers)
     auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer");
 
     // check motion processor IS NOT set
-    ASSERT_FALSE(P->getProcessorMotion());
+    ASSERT_FALSE(P->getProcessorIsMotion());
 
     // install processor motion
     ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
 
     // check motion processor is set
-    ASSERT_TRUE(P->getProcessorMotion());
+    ASSERT_TRUE(P->getProcessorIsMotion() != nullptr);
 
     // check motion processor is correct
-    ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorMotion()), pm);
+    ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorIsMotion()), pm);
 }
 
 TEST(Problem, SetOrigin_PO_2d)
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 1933da8599234e61c534e9f05008710ac4396661..c4a1c4b0470c92bf09df1a950a1c19e57d35642d 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -125,7 +125,7 @@ TEST(ProcessorBase, KeyFrameCallback)
         capt_trk = make_shared<CaptureVoid>(t, sens_trk);
         proc_trk->captureCallback(capt_trk);
 
-//        problem->print(4,1,1,0);
+       problem->print(4,1,1,0);
 
         // Only odom creating KFs
         ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2d")==0 );