diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0b833cbbfc9d91ee015d3e905199698bfcddb294..6fb5b5b1092ee4251cf4531f7af1e6afa3245d70 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -218,6 +218,7 @@ SET(HDRS_STATE_BLOCK
   include/${PROJECT_NAME}/state_block/state_composite.h
   include/${PROJECT_NAME}/state_block/state_homogeneous_3d.h
   include/${PROJECT_NAME}/state_block/state_quaternion.h
+  include/${PROJECT_NAME}/state_block/type_composite.h
   include/${PROJECT_NAME}/state_block/vector_composite.h
   )
 SET(HDRS_TRAJECTORY
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index dde271376d13561d0055aea7f67dfe37a6fdcf4e..b64421fd8616af8b03b3f0ee92002b67bbbd5222 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -130,8 +130,8 @@ int main()
 
     // sensor odometer 2d
     ParamsSensorOdomPtr intrinsics_odo      = std::make_shared<ParamsSensorOdom>();
-    SpecSensorComposite priors_odo                       = {{'P',SpecStateSensor("P", Vector2d::Zero())},
-                                               {'O',SpecStateSensor("O", Vector1d::Zero())}};
+    SpecSensorComposite priors_odo          = {{'P',SpecStateSensor("StatePoint2d", Vector2d::Zero())},
+                                               {'O',SpecStateSensor("StateAngle", Vector1d::Zero())}};
     SensorBasePtr sensor_odo                = problem->installSensor("SensorOdom", "sensor odo", intrinsics_odo, priors_odo);
 
     // processor odometer 2d
diff --git a/demos/hello_wolf/yaml/processor_odom_2d.yaml b/demos/hello_wolf/yaml/processor_odom_2d.yaml
index 907bbfab3be6e9f66feb6c027f34fa752fd893c4..625676edf735ba2bad5ba307e300bbb866e78ab3 100644
--- a/demos/hello_wolf/yaml/processor_odom_2d.yaml
+++ b/demos/hello_wolf/yaml/processor_odom_2d.yaml
@@ -11,5 +11,5 @@ keyframe_vote:
   cov_det:            999
 apply_loss_function: true
 
-state_getter: true
+state_provider: true
 state_priority: 1
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index bd09e14228352eda82cee813692308c23cc7e2aa..c02474f8924ba06224221dc651f08e259bf92095 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -284,12 +284,9 @@ namespace wolf
 template<class TypeBase, typename... TypeInput>
 class Factory
 {
-    private:
-        typedef std::shared_ptr<TypeBase> TypeBasePtr;
-
     public:
         // example of creator callback (see typedefs below)
-        typedef TypeBasePtr (*CreatorCallback)(TypeInput... _input);
+        typedef TypeBase (*CreatorCallback)(TypeInput... _input);
 
     private:
         typedef std::map<std::string, CreatorCallback> CallbackMap;
@@ -298,7 +295,7 @@ class Factory
         static bool registerCreator(const std::string& _type, CreatorCallback createFn);
         static bool unregisterCreator(const std::string& _type);
         static bool isCreatorRegistered(const std::string& _type);
-        static TypeBasePtr create(const std::string& _type, TypeInput... _input);
+        static TypeBase create(const std::string& _type, TypeInput... _input);
         std::string getClass() const;
         static void printAddress();
         static void printCallbacks();
@@ -356,7 +353,7 @@ inline bool Factory<TypeBase, TypeInput...>::isCreatorRegistered(const std::stri
 }
 
 template<class TypeBase, typename... TypeInput>
-inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input)
+inline TypeBase Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input)
 {
     typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type);
 
@@ -406,7 +403,7 @@ namespace wolf
 
 // Landmarks from YAML
 class LandmarkBase;
-typedef Factory<LandmarkBase,
+typedef Factory<std::shared_ptr<LandmarkBase>,
         const YAML::Node&>  FactoryLandmark;
 template<>
 inline std::string FactoryLandmark::getClass() const
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 1ff2688719d93f1a5b80ea1aaa54a1f01f68d123..4dd04a0d01e799ca4b82a9e978559d67d3743936 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -74,22 +74,22 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
         //  * 
         //  * Constructor with type, time stamp and state pointer
         //  * \param _ts is the time stamp associated to this frame, provided in seconds
-        //  * \param _frame_structure StateStructure (string) with the state keys
+        //  * \param _frame_structure StateKeys (string) with the state keys
         //  * \param _state VectorComposite containing the state of each key
         //  **/   
         // FrameBase(const TimeStamp& _ts,
-        //           const StateStructure& _frame_structure,
+        //           const StateKeys& _frame_structure,
         //           const VectorComposite& _state);
 
         // /** \brief Constructor with type, time stamp and state pointer
         //  * 
         //  * Constructor with type, time stamp and state pointer
         //  * \param _ts is the time stamp associated to this frame, provided in seconds
-        //  * \param _frame_structure StateStructure (string) with the state keys
+        //  * \param _frame_structure StateKeys (string) with the state keys
         //  * \param _vectors std::list of VectorXd containing the state of each key
         //  **/   
         // FrameBase(const TimeStamp& _ts,
-        //           const StateStructure& _frame_structure,
+        //           const StateKeys& _frame_structure,
         //           const std::list<VectorXd>& _vectors);
         
         /** \brief Constructor time stamp and specs composite
diff --git a/include/core/map/factory_map.h b/include/core/map/factory_map.h
index 43ba8ad739df48abc5feb531823414f19ab57eb3..8ce6f81e875742a3db35253a44c881da4fcf58b1 100644
--- a/include/core/map/factory_map.h
+++ b/include/core/map/factory_map.h
@@ -168,7 +168,7 @@ namespace wolf
  *  \endcode
  *
  */
-typedef Factory<MapBase, const YAML::Node&> FactoryMap;
+typedef Factory<std::shared_ptr<MapBase>, const YAML::Node&> FactoryMap;
 
 template<>
 inline std::string FactoryMap::getClass() const
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index 63d07cbee4ca2d169ab2103324e70410fe3d2c91..fbbc8276df890fc7982bdd7cdf8d6c361b7ff387 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -200,7 +200,7 @@ inline void exp(const VectorComposite& _tau, VectorComposite& _delta)
 
 inline VectorComposite exp(const VectorComposite& tau)
 {
-    VectorComposite res("PO", {2,1});
+    VectorComposite res("PO", Vector3d::Zero(), {2,1});
 
     exp(tau, res);
 
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index 9ae6c2bf38b045e77276937bda285a55a9dc82ea..c4ae8f399fbdb35de9829ed734369a27f8c15fe0 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.h
@@ -137,7 +137,7 @@ inline void inverse(const VectorComposite& v, VectorComposite& c)
 
 inline VectorComposite inverse(const VectorComposite& v)
 {
-    VectorComposite c("PO", {3,4});
+    VectorComposite c("PO", Vector7d::Zero(), {3,4});
     inverse(v, c);
     return c;
 }
@@ -241,7 +241,7 @@ inline void compose(const VectorComposite& x1, const VectorComposite& x2, Vector
 
 inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
 {
-    VectorComposite c("PO", {3,4});
+    VectorComposite c("PO", Vector7d::Zero(), {3,4});
     compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']);
     return c;
 }
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 2e77bda2fd8d91faab82bd5cef95f3748c41dc01..ed381086584a3db172fb73912588d869e8a342fd 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -79,8 +79,8 @@ class Problem : public std::enable_shared_from_this<Problem>
         std::map<StateBlockPtr, Notification> state_block_notification_map_;
         mutable std::mutex mut_notifications_;
         mutable std::mutex mut_covariances_;
-        //StateStructure frame_structure_;
-        TypeComposite frame_structure_;
+        //StateKeys frame_structure_;
+        TypeComposite frame_types_;
         SpecComposite prior_options_;
         bool prior_applied_;
 
@@ -89,17 +89,15 @@ class Problem : public std::enable_shared_from_this<Problem>
         mutable std::mutex mut_transform_;
 
       private:  // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
-        Problem(const TypeComposite& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
-        Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
+        Problem(const TypeComposite& _frame_structure, SizeEigen _dim); // USE create() below !!
+        Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !!
         void setup();
 
     public:
         static ProblemPtr create(const TypeComposite& _frame_structure, 
-                                 SizeEigen _dim, 
-                                 MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR!
+                                 SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
         static ProblemPtr create(const std::string& _frame_structure, 
-                                 SizeEigen _dim, 
-                                 MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR!
+                                 SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
         static ProblemPtr autoSetup(const std::string& _input_yaml_file, 
                                     const std::vector<std::string>& _primary_schema_folders={});
         virtual ~Problem();
@@ -112,11 +110,12 @@ class Problem : public std::enable_shared_from_this<Problem>
         // Properties -----------------------------------------
     public:
         SizeEigen getDim() const;
-        const StateStructure& getFrameStructure() const;
+        StateKeys getFrameKeys() const;
+        TypeComposite getFrameTypes(StateKeys _keys = "") const;
 
     protected:
-        //void appendToStructure(const StateStructure& _structure);
-        void appendToStructure(const TypeComposite& _structure);
+        //void appendToFrameTypes(const StateKeys& _structure);
+        void appendToFrameTypes(const TypeComposite& _structure);
 
 
 
@@ -201,101 +200,64 @@ class Problem : public std::enable_shared_from_this<Problem>
         FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
         FrameBasePtr setPrior(const SpecComposite& priors, const TimeStamp& _ts);
 
-        // /** \brief Emplace frame from string frame_structure, dimension and vector
-        //  * \param _time_stamp Time stamp of the frame
-        //  * \param _frame_structure String indicating the frame structure.
-        //  * \param _dim variable indicating the dimension of the problem
-        //  * \param _frame_state State vector; must match the size and format of the chosen frame structure
-        //  *
-        //  * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
-        //  *   - Create a Frame
-        //  *   - Add it to Trajectory
-        //  *   - If it is key-frame, update state-block lists in Problem
-        //  */
-        // FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
-        //                           const StateStructure& _frame_structure, //
-        //                           const SizeEigen _dim, //
-        //                           const Eigen::VectorXd& _frame_state);
-
-        /** \brief Emplace frame from string frame_structure and state
+        /** \brief Emplace frame from timestamp, types and and state
          * \param _time_stamp Time stamp of the frame
-         * \param _frame_structure String indicating the frame structure.
+         * \param _frame_types TypeComposite indicating the types of each key.
          * \param _frame_state State vector; must match the size and format of the chosen frame structure
          *
-         * - The dimension is taken from Problem
-         *
          * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
          *   - Create a Frame
          *   - Add it to Trajectory
          *   - If it is key-frame, update state-block lists in Problem
          */
-        // FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
-        //                           const StateStructure& _frame_structure, //
-        //                           const VectorComposite& _frame_state);
-        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
-                                  const TypeComposite& _frame_structure, //
+        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp,
+                                  const TypeComposite& _frame_types,
                                   const VectorComposite& _frame_state);
 
-        /** \brief Emplace frame from state
+        /** \brief Emplace frame from timestamp and keys
          * \param _time_stamp Time stamp of the frame
-         * \param _frame_state State; must be part of the problem's frame structure
-         *
-         * - The structure is taken from Problem
-         * - The dimension is taken from Problem
+         * \param _frame_keys String containing the keys of the desired frame. Empty means all keys available.
          *
+         * - The frame_types are taken from Problem
+         * - The states are taken from Problem
+         * 
          * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
          *   - Create a Frame
          *   - Add it to Trajectory
          *   - If it is key-frame, update state-block lists in Problem
          */
-        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
-                                  const VectorComposite& _frame_state);
+        FrameBasePtr emplaceFrame (const TimeStamp& _time_stamp,
+                                   const StateKeys& _frame_keys="");
 
-        // /** \brief Emplace frame from string frame_structure and dimension
-        //  * \param _time_stamp Time stamp of the frame
-        //  * \param _frame_structure String indicating the frame structure.
-        //  * \param _dim variable indicating the dimension of the problem
-        //  *
-        //  * - The dimension is taken from Problem
-        //  * - The state is taken from Problem
-        //  *
-        //  * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
-        //  *   - Create a Frame
-        //  *   - Add it to Trajectory
-        //  *   - If it is key-frame, update state-block lists in Problem
-        //  */
-        // FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
-        //                           const StateStructure& _frame_structure, //
-        //                           const SizeEigen _dim);
-
-        /** \brief Emplace frame from state vector
+        /** \brief Emplace frame from timestamps and state
          * \param _time_stamp Time stamp of the frame
-         * \param _frame_state State vector; must match the size and format of the chosen frame structure
+         * \param _frame_state State; must be part of the problem's frame structure
          *
-         * - The structure is taken from Problem
-         * - The dimension is taken from Problem
+         * - The frame_types are taken from Problem
          *
          * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
          *   - Create a Frame
          *   - Add it to Trajectory
          *   - If it is key-frame, update state-block lists in Problem
          */
-        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
-                                  const Eigen::VectorXd& _frame_state);
+        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp,
+                                  const VectorComposite& _frame_state);
 
-        /** \brief Emplace frame, guess all values
+        /** \brief Emplace frame from timestamp and state vector
          * \param _time_stamp Time stamp of the frame
+         * \param _frame_keys String containing the keys of the desired frame. Empty means all keys available.
+         * \param _frame_state State vector; must match the size and format of the chosen frame structure
          *
-         * - The structure is taken from Problem
-         * - The dimension is taken from Problem
-         * - The state is taken from Problem
+         * - The frame_types are taken from Problem
          *
          * This acts as a Frame factory, but also takes care to update related lists in WolfProblem:
          *   - Create a Frame
          *   - Add it to Trajectory
          *   - If it is key-frame, update state-block lists in Problem
          */
-        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp);
+        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp,
+                                  const StateKeys& _frame_keys,
+                                  const Eigen::VectorXd& _frame_state);
 
         /** \brief Emplace frame from state specs composite
          * \param _time_stamp Time stamp of the frame
@@ -310,7 +272,7 @@ class Problem : public std::enable_shared_from_this<Problem>
          *   - Add it to Trajectory
          *   - If it is key-frame, update state-block lists in Problem
          */
-        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
+        FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp,
                                   const SpecComposite& _frame_spec_composite);
 
         // Frame getters
@@ -331,17 +293,18 @@ class Problem : public std::enable_shared_from_this<Problem>
          *
          * New key frame callback: It should be called by any processor that creates a new key frame. It calls the keyFrameCallback of the rest of processors.
          */
-        void keyFrameCallback(FrameBasePtr _keyframe_ptr, //
+        void keyFrameCallback(FrameBasePtr _keyframe_ptr,
                               ProcessorBasePtr _processor_ptr);
 
         // State getters
         TimeStamp       getTimeStamp    ( ) const;
-        VectorComposite getState        ( const StateStructure& _structure = "" ) const;
-        VectorComposite getState        ( const TimeStamp& _ts, const StateStructure& _structure = "" ) const;
-        VectorComposite getOdometry     ( const StateStructure& _structure = "" ) const;
+        VectorComposite getState        ( const StateKeys& _structure = "" ) const;
+        VectorComposite getState        ( const TimeStamp& _ts, const StateKeys& _structure = "" ) const;
+        VectorComposite getOdometry     ( const StateKeys& _structure = "" ) const;
 
         // Zero state provider
-        VectorComposite stateZero       ( const StateStructure& _structure = "" ) const;
+        VectorComposite stateZero       ( TypeComposite _types = {} ) const;
+        VectorComposite stateZero       ( const StateKeys& _structure) const;
 
 
 
@@ -351,7 +314,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         MapBasePtr getMap();
         void setMap(MapBasePtr);
         void loadMap(const std::string& _filename_dot_yaml);
-        void saveMap(const std::string& _filename_dot_yaml, //
+        void saveMap(const std::string& _filename_dot_yaml,
                      const std::string& _map_name = "Map automatically saved by Wolf");
 
 
diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h
index b432824fbbae94391460334009fc09191057acca..9ce372e6503f37423f9e7237aa64e14e133b8bfa 100644
--- a/include/core/processor/factory_processor.h
+++ b/include/core/processor/factory_processor.h
@@ -117,7 +117,7 @@ namespace wolf
  *     \endcode
  *
  */
-typedef Factory<ProcessorBase,
+typedef Factory<std::shared_ptr<ProcessorBase>,
                 const YAML::Node&> FactoryProcessor;
 template<>
 inline std::string FactoryProcessor::getClass() const
@@ -125,7 +125,7 @@ inline std::string FactoryProcessor::getClass() const
     return "FactoryProcessor";
 }
 
-typedef Factory<ProcessorBase,
+typedef Factory<std::shared_ptr<ProcessorBase>,
                 const std::string&,
                 const std::vector<std::string>&> FactoryProcessorYaml;
 template<>
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index faa85b47e8ff3588ffdeebdf57ad1b77110c6498..ae25dbf4cc08e45de3a241d0764d8bee5135fa0b 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -23,6 +23,7 @@
 
 #include "core/common/wolf.h"
 #include "core/state_block/vector_composite.h"
+#include "core/state_block/type_composite.h"
 #include "core/common/params_base.h" // for toString
 #include "yaml-cpp/yaml.h"
 
@@ -34,19 +35,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProvider);
 struct ParamsMotionProvider
 {
     bool state_provider = true;
-    int state_order = 1;
+    int state_provider_order = 1;
 
     ParamsMotionProvider() = default;
     ParamsMotionProvider(const YAML::Node& _n)
     {
         state_provider    = _n["state_provider"].as<bool>();
         if (state_provider)
-            state_order   = _n["state_order"].as<double>();
+            state_provider_order   = _n["state_provider_order"].as<double>();
     }
     std::string print() const
     {
       return  "state_provider: "   + toString(state_provider)   + "\n"
-            + "state_provider_order: " + toString(state_order) + "\n";
+            + "state_provider_order: " + toString(state_provider_order) + "\n";
     }
 
 };
@@ -58,14 +59,13 @@ class MotionProvider
 {
     public:
 
-        //MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params);
-        MotionProvider(const TypeComposite& _structure, ParamsMotionProviderPtr _params);
+        MotionProvider(const TypeComposite& _state_types, ParamsMotionProviderPtr _params);
         virtual ~MotionProvider();
 
         // Queries to the processor:
         virtual TimeStamp       getTimeStamp() const = 0;
-        virtual VectorComposite getState(const StateStructure& _structure = "") const = 0;
-        virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0;
+        virtual VectorComposite getState(const StateKeys& _structure = "") const = 0;
+        virtual VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const = 0;
 
         VectorComposite getOdometry ( ) const;
         void setOdometry(const VectorComposite&);
@@ -75,25 +75,23 @@ class MotionProvider
         void setOrder(int);
 
     public:
-        //const StateStructure& getStateStructure ( ) const { return state_structure_; };
-        const TypeComposite& getStateStructure ( ) const { return state_structure_; };
-        //void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
-        void setStateStructure(TypeComposite _state_structure) { state_structure_ = _state_structure; };
+        TypeComposite getStateTypes ( ) const { return state_types_; };
+        StateKeys getStateKeys ( ) const { return state_types_.getKeys(); };
+        void setStateTypes(const TypeComposite& _state_structure) { state_types_ = _state_structure; };
         void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr);
 
     protected:
-        //StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
-        TypeComposite state_structure_;
+        TypeComposite state_types_;
         VectorComposite odometry_;
         ParamsMotionProviderPtr params_motion_provider_;
 };
 
-// inline MotionProvider::MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params) :
-inline MotionProvider::MotionProvider(const TypeComposite& _structure, ParamsMotionProviderPtr _params) :
-        state_structure_(_structure),
+// inline MotionProvider::MotionProvider(const StateKeys& _structure, ParamsMotionProviderPtr _params) :
+inline MotionProvider::MotionProvider(const TypeComposite& _state_types, ParamsMotionProviderPtr _params) :
+        state_types_(_state_types),
         params_motion_provider_(_params)
 {
-    //
+    checkTypeComposite(_state_types);
 }
 
 inline wolf::VectorComposite MotionProvider::getOdometry ( ) const
@@ -117,12 +115,12 @@ inline bool MotionProvider::isStateGetter() const
 
 inline int MotionProvider::getOrder() const
 {
-    return params_motion_provider_->state_order;
+    return params_motion_provider_->state_provider_order;
 }
 
 inline void MotionProvider::setOrder(int _order)
 {
-    params_motion_provider_->state_order = _order;
+    params_motion_provider_->state_provider_order = _order;
 }
 
 } /* namespace wolf */
\ No newline at end of file
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 3a2e7a60c1430e42cd19ab31352984f2c1515199..dbc86ff01abcb16fa27605781326fb66e30a62b3 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -48,11 +48,11 @@ struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsMotionPr
             ParamsProcessorBase(_n),
             ParamsMotionProvider(_n)
         {
-          max_time_span               = _n["keyframe_vote"]["max_time_span"].as<double>();
-          max_buff_length             = _n["keyframe_vote"]["max_buff_length"].as<unsigned int>();
-          dist_traveled               = _n["keyframe_vote"]["dist_traveled"].as<double>();
-          angle_turned                = _n["keyframe_vote"]["angle_turned"].as<double>();
-          unmeasured_perturbation_std = _n["unmeasured_perturbation_std"].as<double>();
+            max_time_span               = _n["keyframe_vote"]["max_time_span"].as<double>();
+            max_buff_length             = _n["keyframe_vote"]["max_buff_length"].as<unsigned int>();
+            dist_traveled               = _n["keyframe_vote"]["dist_traveled"].as<double>();
+            angle_turned                = _n["keyframe_vote"]["angle_turned"].as<double>();
+            unmeasured_perturbation_std = _n["unmeasured_perturbation_std"].as<double>();
         }
         std::string print() const override
         {
@@ -166,7 +166,7 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
         // This is the main public interface
       public:
         ProcessorMotion(const std::string& _type,
-                        std::string _state_structure,
+                        TypeComposite _state_types,
                         int _dim,
                         SizeEigen _state_size,
                         SizeEigen _delta_size,
@@ -181,8 +181,8 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
 
         // Queries to the processor:
         TimeStamp       getTimeStamp() const override;
-        VectorComposite getState(const StateStructure& _structure = "") const override;
-        VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override;
+        VectorComposite getState(const StateKeys& _structure = "") const override;
+        VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override;
 
         /** \brief Gets the delta preintegrated covariance from all integrations so far
          * \return the delta preintegrated covariance matrix
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index b4461c7e101f031a768d24b4912a06d0a1a5dad4..faf162f88fcefe61c7643cf4ef58da58427edc01 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -119,16 +119,16 @@ class ProcessorTracker : public ProcessorBase
         FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
         FeatureBasePtrList known_features_last_; ///< list of the known features in previous captures successfully tracked in \b last
         FeatureBasePtrList known_features_incoming_; ///< list of the known features in \b last successfully tracked in \b incoming
-        StateStructure state_structure_;         ///< Structure of frames created or used by this processor
+        StateKeys state_keys_;         ///< Keys of frames created or used by this processor
 
     public:
         ProcessorTracker(const std::string& _type,
-                         const StateStructure& _structure,
+                         const StateKeys& _structure,
                          int _dim,
                          ParamsProcessorTrackerPtr _params_tracker);
         ~ProcessorTracker() override;
 
-        const StateStructure& getStateStructure() const;
+        const StateKeys& getStateKeys() const;
 
         virtual CaptureBaseConstPtr getOrigin() const;
         virtual CaptureBasePtr getOrigin();
@@ -305,9 +305,9 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming()
     return new_features_incoming_;
 }
 
-inline const StateStructure& ProcessorTracker::getStateStructure ( ) const
+inline const StateKeys& ProcessorTracker::getStateKeys ( ) const
 {
-    return state_structure_;
+    return state_keys_;
 }
 
 inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index e76d8103fea857d725c52e9bafc365940907cd61..18e724e4ed582a04d5c7d48bbf8f924f64649460 100644
--- a/include/core/processor/processor_tracker_feature.h
+++ b/include/core/processor/processor_tracker_feature.h
@@ -98,7 +98,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
         /** \brief Constructor with type
          */
         ProcessorTrackerFeature(const std::string& _type,
-                                const StateStructure& _structure,
+                                const StateKeys& _structure,
                                 int _dim,
                                 ParamsProcessorTrackerFeaturePtr _params_tracker_feature);
         ~ProcessorTrackerFeature() override;
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index b11ae12b524dd1ce31c3e7c938b164073ce6953d..c4563d754f01253be8bcf386d59a68a35341029f 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -94,7 +94,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker
 {
     public:
         ProcessorTrackerLandmark(const std::string& _type,
-                                 const StateStructure& _structure,
+                                 const StateKeys& _structure,
                                  ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark);
         ~ProcessorTrackerLandmark() override;
 
diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
index 745b2d839759a8608072c663140589298d8c153b..82aff821af7aa7ef9a5dfa656fb1b8825e48e1e7 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.h
@@ -209,10 +209,10 @@ namespace wolf
  *  - Problem::installSensor() : to install sensors in WOLF Problem.
  */
 
-typedef Factory<SensorBase,
+typedef Factory<std::shared_ptr<SensorBase>,
                 const YAML::Node&> FactorySensor;
 
-typedef Factory<SensorBase,
+typedef Factory<std::shared_ptr<SensorBase>,
                 const std::string&,
                 const std::string&,
                 const std::vector<std::string>&> FactorySensorYaml;
diff --git a/include/core/solver/factory_solver.h b/include/core/solver/factory_solver.h
index 72716cf487baf347a26f47b15b28f82c047a90e3..860ede1ff3bfd123e0f9dfee2115a6e3f275f5c8 100644
--- a/include/core/solver/factory_solver.h
+++ b/include/core/solver/factory_solver.h
@@ -40,7 +40,7 @@ namespace wolf
  *
  */
 
-typedef Factory<SolverManager,
+typedef Factory<std::shared_ptr<SolverManager>,
                 const ProblemPtr&,
                 const YAML::Node&> FactorySolver;
 
diff --git a/include/core/state_block/composite.h b/include/core/state_block/composite.h
index 83f73f5ab3bde507cd9c285cea9d24ae26ca2e8a..2e1abc2dd54cb29c2fb96139ff381289cda690b0 100644
--- a/include/core/state_block/composite.h
+++ b/include/core/state_block/composite.h
@@ -22,22 +22,23 @@
 #pragma once
 
 #include <string>
-#include <unordered_map>
+#include <map>
+#include <algorithm>
 #include "yaml-cpp/yaml.h"
 
 namespace wolf
 {
+using std::map;
 
-typedef std::string StateStructure;
-using std::unordered_map;
+typedef std::string StateKeys;
 
 template <typename T>
-class Composite : public unordered_map<char, T>
+class Composite : public map<char, T>
 {
     public:
         using CompositeType = T;
-        using unordered_map<char, T>::unordered_map;
 
+        using map<char,T>::map;
         Composite(const YAML::Node& _n);
         virtual ~Composite() = default;
 
@@ -45,12 +46,12 @@ class Composite : public unordered_map<char, T>
         CompositeOther cast() const;
 
         bool has(char _key) const;
-        bool has(const StateStructure &_structure) const;
-        StateStructure getStructure() const;
-        
+        bool has(const StateKeys &_structure) const;
+        StateKeys getKeys() const;
+
         friend std::ostream& operator<<(std::ostream &_os, const Composite<T> &_x)
         {
-            for (const auto &pair : _x)
+            for (auto&& pair : _x)
             {
                 _os << pair.first << ": " << pair.second;
             }
@@ -63,7 +64,7 @@ template <typename CompositeOther>
 inline CompositeOther Composite<T>::cast() const
 {
     CompositeOther casted_composite;
-    for (auto pair : *this)
+    for (auto&& pair : *this)
     {
         casted_composite.emplace(pair.first, static_cast<typename CompositeOther::CompositeType>(pair.second));
     }
@@ -77,6 +78,7 @@ inline Composite<T>::Composite(const YAML::Node& _n)
     {
         throw std::runtime_error("SpecComposite: constructor with a non-map yaml node");
     }
+
     for (auto spec_pair : _n)
     {
         this->emplace(spec_pair.first.as<char>(), T(spec_pair.second));
@@ -90,9 +92,9 @@ inline bool Composite<T>::has(char _key) const
 }
 
 template <typename T>
-inline bool Composite<T>::has(const StateStructure &_structure) const
+inline bool Composite<T>::has(const StateKeys &_query_keys) const
 {
-    for (auto key : _structure)
+    for (auto&& key : _query_keys)
     {
         if (not has(key))
         {
@@ -103,14 +105,13 @@ inline bool Composite<T>::has(const StateStructure &_structure) const
 }
 
 template <typename T>
-inline std::string Composite<T>::getStructure() const
+inline StateKeys Composite<T>::getKeys() const
 {
-    std::string structure;
-    for (auto pair : *this)
-    {
-        structure.push_back(pair.first);
-    }
-    return structure;
+    StateKeys keys;
+    for (auto&& pair : *this)
+        keys.push_back(pair.first);
+
+    return keys;
 }
 
 }  // namespace wolf
\ No newline at end of file
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
index 578a8b0acdbf2fcfeb9b8b26656c676774fb67c2..31b87e960309f464278f202f1c0557730948e611 100644
--- a/include/core/state_block/factory_state_block.h
+++ b/include/core/state_block/factory_state_block.h
@@ -121,24 +121,33 @@ namespace wolf
  *
  * Note: for base state blocks, the size is determined by the size of the provided vector parameter `VectorXd& _state`.
  */
-typedef Factory<StateBlock, const Eigen::VectorXd&, bool> FactoryStateBlock;
+typedef Factory<std::shared_ptr<StateBlock>, const Eigen::VectorXd&, bool> FactoryStateBlock;
 template<>
 inline std::string FactoryStateBlock::getClass() const
 {
     return "FactoryStateBlock";
 }
+typedef Factory<std::shared_ptr<StateBlock>> FactoryStateBlockIdentity;
+template<>
+inline std::string FactoryStateBlockIdentity::getClass() const
+{
+    return "FactoryStateBlockIdentity";
+}
+typedef Factory<Eigen::VectorXd> FactoryStateBlockIdentityVector;
+template<>
+inline std::string FactoryStateBlockIdentityVector::getClass() const
+{
+    return "FactoryStateBlockIdentityVector";
+}
 
-#define WOLF_REGISTER_STATEBLOCK(StateBlockType)                                                                      \
-    namespace                                                                                                         \
-    {                                                                                                                 \
-    const bool WOLF_UNUSED StateBlockType##Registered =                                                               \
-        FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create);                                  \
-    }
-
-#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType)                                                        \
-    namespace                                                                                                         \
-    {                                                                                                                 \
-    const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key =                                                      \
-        FactoryStateBlock::registerCreator(#Key, StateBlockType::create);                                             \
+#define WOLF_REGISTER_STATEBLOCK(StateBlockType)                                                        \
+    namespace                                                                                           \
+    {                                                                                                   \
+    const bool WOLF_UNUSED StateBlockType##Registered =                                                 \
+        FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create);                    \
+    const bool WOLF_UNUSED StateBlockType##IdentityRegistered =                                         \
+        FactoryStateBlockIdentity::registerCreator(#StateBlockType, StateBlockType::createIdentity);    \
+    const bool WOLF_UNUSED StateBlockType##IdentityVectorRegistered =                                   \
+        FactoryStateBlockIdentityVector::registerCreator(#StateBlockType, StateBlockType::Identity);    \
     }
 }
\ No newline at end of file
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index f728227f91b0fd0f37c60bf1d4412c9540f3cf08..d143262215279bf247464e017a7cb842423c351d 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -37,14 +37,14 @@ class HasStateBlocks
 
     public:
         HasStateBlocks();
-        HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
+        HasStateBlocks(const StateKeys& _keys) : keys_order_(_keys), state_block_map_() {}
         HasStateBlocks(const SpecComposite& _specs);
         HasStateBlocks(const TypeComposite& _types, const VectorComposite& _vectors);
         virtual ~HasStateBlocks();
 
-        const StateStructure& getStructure() const { return structure_; }
-        void appendToStructure(const char& _new_key){ structure_ += _new_key; }
-        bool isInStructure(const char& _sb_key) const { return structure_.find(_sb_key) != std::string::npos; }
+        StateKeys getKeys() const { return keys_order_; }
+        void appendToStructure(const char& _new_key){ keys_order_ += _new_key; }
+        bool isInStructure(const char& _sb_key) const { return keys_order_.find(_sb_key) != std::string::npos; }
 
         std::unordered_map<char, StateBlockConstPtr> getStateBlockMap() const;
         std::unordered_map<char, StateBlockPtr> getStateBlockMap();
@@ -82,15 +82,15 @@ class HasStateBlocks
         virtual void removeStateBlocks(ProblemPtr _problem);
 
         // States
-        VectorComposite getState(const StateStructure& structure="") const;
+        VectorComposite getState(const StateKeys& _keys="") const;
         void setState(const VectorComposite& _state, const bool _notify = true);
-        void setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const std::list<SizeEigen>& _sizes, const bool _notify = true);
-        void setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const bool _notify = true);
-        void setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify = true);
+        void setState(const Eigen::VectorXd& _state, const StateKeys& _keys, const std::list<SizeEigen>& _sizes, const bool _notify = true);
+        void setState(const Eigen::VectorXd& _state, const StateKeys& _keys, const bool _notify = true);
+        void setState(const StateKeys& _keys, const std::list<VectorXd>& _vectors, const bool _notify = true);
 
-        VectorXd getStateVector(const StateStructure& structure) const;
-        unsigned int getSize(const StateStructure& _structure="") const;
-        unsigned int getLocalSize(const StateStructure& _structure="") const;
+        VectorXd getStateVector(const StateKeys& _keys) const;
+        unsigned int getSize(const StateKeys& _keys="") const;
+        unsigned int getLocalSize(const StateKeys& _keys="") const;
 
         // Perturb state
         void perturb(double amplitude = 0.01);        
@@ -102,7 +102,7 @@ class HasStateBlocks
         void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
 
     private:
-        StateStructure structure_;
+        StateKeys keys_order_;
         std::unordered_map<char, StateBlockPtr> state_block_map_;
 
 };
@@ -117,7 +117,7 @@ class HasStateBlocks
 namespace wolf{
 
 inline HasStateBlocks::HasStateBlocks() :
-        structure_(std::string("")),
+        keys_order_(std::string("")),
         state_block_map_()
 {
     //
@@ -144,7 +144,7 @@ inline std::unordered_map<char, StateBlockPtr> HasStateBlocks::getStateBlockMap(
 inline std::vector<StateBlockConstPtr> HasStateBlocks::getStateBlockVec() const
 {
     std::vector<StateBlockConstPtr> sbv;
-    for (auto& key : structure_)
+    for (auto& key : keys_order_)
     {
         sbv.push_back(getStateBlock(key));
     }
@@ -154,7 +154,7 @@ inline std::vector<StateBlockConstPtr> HasStateBlocks::getStateBlockVec() const
 inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec()
 {
     std::vector<StateBlockPtr> sbv;
-    for (auto& key : structure_)
+    for (auto& key : keys_order_)
     {
         sbv.push_back(getStateBlock(key));
     }
@@ -179,7 +179,7 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_key
 
 inline bool HasStateBlocks::setStateBlock(const char _sb_key, const StateBlockPtr& _sb)
 {
-    assert (structure_.find(_sb_key) > 0 and "Cannot set a state block out of the state structure! Use addStateBlock instead.");
+    assert (keys_order_.find(_sb_key) > 0 and "Cannot set a state block out of the state keys! Use addStateBlock instead.");
     assert ( state_block_map_.count(_sb_key) > 0 and "Cannot set an inexistent state block! Use addStateBlock instead.");
     state_block_map_.at(_sb_key) = _sb;
     return true; // success
@@ -256,29 +256,29 @@ inline void HasStateBlocks::setState(const VectorComposite& _state, const bool _
         const auto& vec = pair_key_vec.second;
         const auto& sb = getStateBlock(key);
         if (!sb)
-            WOLF_ERROR("Stateblock key ", key, " not in the structure");
-        assert (sb and "Stateblock key not in the structure");
+            WOLF_ERROR("Stateblock key ", key, " not in the keys");
+        assert (sb and "Stateblock key not in the keys");
 
         sb->setState(vec, _notify);
     }
 }
 
-inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const bool _notify)
+inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateKeys& _keys, const bool _notify)
 {
-    StateStructure structure;
-    if (_structure == "")
-        structure = structure_;
+    StateKeys keys;
+    if (_keys == "")
+        keys = keys_order_;
     else
-        structure = _structure;
+        keys = _keys;
 
-    int size = getSize(structure);
+    int size = getSize(keys);
     assert(_state.size() == size and "In FrameBase::setState wrong state size");
 
     unsigned int index = 0;
-    for (const char key : structure)
+    for (const char key : keys)
     {
         const auto& sb  = getStateBlock(key);
-        assert (sb and "Stateblock key not in the structure");
+        assert (sb and "Stateblock key not in the keys");
 
         sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver
         index += sb->getSize();
@@ -287,16 +287,16 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateS
 }
 
 inline void HasStateBlocks::setState (const Eigen::VectorXd& _state,
-                                      const StateStructure& _structure,
+                                      const StateKeys& _keys,
                                       const std::list<SizeEigen>& _sizes,
                                       const bool _notify)
 {
     SizeEigen index = 0;
     auto size_it = _sizes.begin();
-    for (const auto& key : _structure)
+    for (const auto& key : _keys)
     {
         const auto& sb = getStateBlock(key);
-        assert (sb and "Stateblock key not in the structure");
+        assert (sb and "Stateblock key not in the keys");
         assert(*size_it == sb->getSize() and "State block size mismatch");
 
         sb->setState(_state.segment(index, *size_it), _notify);
@@ -306,13 +306,13 @@ inline void HasStateBlocks::setState (const Eigen::VectorXd& _state,
 }
 
 
-inline void HasStateBlocks::setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify)
+inline void HasStateBlocks::setState(const StateKeys& _keys, const std::list<VectorXd>& _vectors, const bool _notify)
 {
     auto vec_it = _vectors.begin();
-    for (const auto key : _structure)
+    for (const auto key : _keys)
     {
         const auto& sb = getStateBlock(key);
-        assert (sb and "Stateblock key not in the structure");
+        assert (sb and "Stateblock key not in the keys");
         assert(vec_it->size() == sb->getSize() and "State block size mismatch");
 
         sb->setState(*vec_it, _notify);
@@ -321,23 +321,23 @@ inline void HasStateBlocks::setState(const StateStructure& _structure, const std
 }
 
 
-//// _structure can be either stateblock structure of the node or a subset of this structure
-inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _structure) const
+//// _keys can be either stateblock keys of the node or a subset of this keys
+inline VectorXd HasStateBlocks::getStateVector(const StateKeys& _keys) const
 {
-    StateStructure structure;
-    if (_structure == "")
-        structure = structure_;
+    StateKeys keys;
+    if (_keys == "")
+        keys = keys_order_;
     else
-        structure = _structure;
+        keys = _keys;
 
-    VectorXd state(getSize(structure));
+    VectorXd state(getSize(keys));
 
     unsigned int index = 0;
-    for (const char key : structure)
+    for (const char key : keys)
     {
         const auto& sb = getStateBlock(key);
 
-        assert(sb != nullptr and "Requested StateBlock key not in the structure");
+        assert(sb != nullptr and "Requested StateBlock key not in the keys");
 
         state.segment(index,sb->getSize()) = sb->getState();
         index += sb->getSize();
@@ -345,13 +345,13 @@ inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _structure)
     return state;
 }
 
-inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure) const
+inline VectorComposite HasStateBlocks::getState(const StateKeys& _keys) const
 {
-    const StateStructure& structure = (_structure == "" ? structure_ : _structure);
+    const StateKeys& keys = (_keys == "" ? keys_order_ : _keys);
 
     VectorComposite state;
 
-    for (const auto key : structure)
+    for (const auto key : keys)
     {
         auto state_it = state_block_map_.find(key);
 
@@ -363,16 +363,16 @@ inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure
     return state;
 }
 
-inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) const
+inline unsigned int HasStateBlocks::getSize(const StateKeys& _keys) const
 {
-    const StateStructure& structure = (_structure == "" ? structure_ : _structure);
+    const StateKeys& keys = (_keys == "" ? keys_order_ : _keys);
 
     unsigned int size = 0;
-    for (const char key : structure)
+    for (const char key : keys)
     {
         const auto& sb = getStateBlock(key);
         if (!sb){
-            WOLF_ERROR("Stateblock key ", key, " not in the structure");
+            WOLF_ERROR("Stateblock key ", key, " not in the keys");
         }
         size += sb->getSize();
     }
@@ -441,20 +441,20 @@ inline bool HasStateBlocks::stateBlockKey(const StateBlockConstPtr &_sb, char& _
     }
 }
 
-inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _structure) const
+inline unsigned int HasStateBlocks::getLocalSize(const StateKeys& _keys) const
 {
-    StateStructure structure;
-    if (_structure == "")
-        structure = structure_;
+    StateKeys keys;
+    if (_keys == "")
+        keys = keys_order_;
     else
-        structure = _structure;
+        keys = _keys;
 
     unsigned int size = 0;
-    for (const char key : structure)
+    for (const char key : keys)
     {
         const auto& sb = getStateBlock(key);
         if (!sb){
-            WOLF_ERROR("Stateblock key ", key, " not in the structure");
+            WOLF_ERROR("Stateblock key ", key, " not in the keys");
         }
         size += sb->getLocalSize();
     }
diff --git a/include/core/state_block/spec_composite.h b/include/core/state_block/spec_composite.h
index 05fa48c3a0150bfba804d9428d8023a5b521faf0..688caea505b38e199f24bbd51c370b21cad76f47 100644
--- a/include/core/state_block/spec_composite.h
+++ b/include/core/state_block/spec_composite.h
@@ -22,6 +22,7 @@
 #pragma once
 
 #include "core/state_block/composite.h"
+#include "core/state_block/type_composite.h"
 #include "core/state_block/vector_composite.h"
 
 #include <string>
@@ -32,8 +33,6 @@
 namespace wolf
 {
 
-using std::unordered_map;
-
 class SpecState
 {
     protected:
@@ -67,8 +66,6 @@ class SpecState
         friend std::ostream& operator <<(std::ostream &_os, const wolf::SpecState &_x);
 };
 
-typedef Composite<std::string> TypeComposite;
-
 template <typename T>
 class SpecBaseComposite : public Composite<T>
 {
@@ -86,9 +83,9 @@ template <>
 inline VectorComposite SpecBaseComposite<SpecState>::getVectorComposite() const
 {
     VectorComposite states;
-    for (auto spec_pair : *this)
+    for (auto&& pair : *this)
     {
-        states.emplace(spec_pair.first, spec_pair.second.getState());
+        states.emplace(pair.first, pair.second.getState());
     }
     return states;
 }
@@ -103,9 +100,9 @@ template <>
 inline TypeComposite SpecBaseComposite<SpecState>::getTypeComposite() const
 {
     TypeComposite types;
-    for (auto spec_pair : *this)
+    for (auto&& pair : *this)
     {
-        types.emplace(spec_pair.first, spec_pair.second.getType());
+        types.emplace(pair.first, pair.second.getType());
     }
     return types;
 }
@@ -116,7 +113,6 @@ inline TypeComposite SpecBaseComposite<T>::getTypeComposite() const
     throw std::runtime_error("Impossible to build a TypeComposite from this Composite");
 }
 
-
 inline const std::string& SpecState::getType() const { return type_; }
 
 inline const std::string& SpecState::getMode() const { return mode_; }
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index cce71edc7b2cd3d105ea228ef099ab6de3281d4f..d33f53a1a923b1380c8a04c1a4266cb434cf1aa1 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -30,14 +30,16 @@ namespace wolf
 class StateAngle : public StateBlock
 {
     public:
-        StateAngle(double _angle = 0.0, bool _fixed = false, bool _transformable = true);
-        StateAngle(const Vector1d& _angle, bool _fixed = false, bool _transformable = true);
+        StateAngle(double _angle, bool _fixed = false, bool _transformable = true);
+        StateAngle(const VectorXd& _angle, bool _fixed = false, bool _transformable = true);
+        
+        static Eigen::VectorXd Identity();
+
+        WOLF_STATE_BLOCK_CREATE(StateAngle);
 
         ~StateAngle() override;
 
         virtual void transform(const VectorComposite& _transformation) override;
-
-        static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) :
@@ -46,23 +48,23 @@ inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) :
     state_(0) = pi2pi(_angle);
 }
 
-inline StateAngle::StateAngle(const Vector1d& _angle, bool _fixed, bool _transformable) :
+inline StateAngle::StateAngle(const VectorXd& _angle, bool _fixed, bool _transformable) :
         StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
 {
+    if(_angle.size() != 1)
+        throw std::runtime_error("The angle state vector must be of size 1!");
+
     state_(0) = pi2pi(_angle(0));
 }
 
-inline StateAngle::~StateAngle()
+inline Eigen::VectorXd StateAngle::Identity()
 {
-    //
+    return Eigen::VectorXd::Zero(1);
 }
 
-inline StateBlockPtr StateAngle::create (const Eigen::VectorXd& _state, bool _fixed)
+inline StateAngle::~StateAngle()
 {
-    if(_state.size() != 1)
-        throw std::runtime_error("The angle state vector must be of size 1!");
-    
-    return std::make_shared<StateAngle>(_state(0), _fixed);
+    //
 }
 
 inline void StateAngle::transform(const VectorComposite& _transformation)
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index ca355e307206f6f74dbb25e10e5035a14e332385..d19d3155a28d8b95e273a3f8f9e9bbde97a8564b 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -21,6 +21,31 @@
 //--------LICENSE_END--------
 #pragma once
 
+/*
+ * Macro for defining StateBlocks creators.
+ *
+ * Place a call to this macro inside your class declaration 
+ * (in the state_block_class.h file), preferably just after the constructors.
+ *
+ * In order to use this macro, the derived state_block class, StateBlockClass,
+ * must have the constructor callable as:
+ *
+ *   StateBlockClass(const Eigen::VectorXd& _state, bool _fixed)
+ * 
+ * And the static method:
+ * 
+ *   static Eigen::VectorXd Identity()
+ */
+#define WOLF_STATE_BLOCK_CREATE(StateBlockClass)                                    \
+static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed)             \
+{                                                                                   \
+    return std::make_shared<StateBlockClass>(_state, _fixed);                       \
+}                                                                                   \
+static StateBlockPtr createIdentity()                                               \
+{                                                                                   \
+    return std::make_shared<StateBlockClass>(StateBlockClass::Identity(), false);   \
+}                                                                                   \
+
 // Fwd references
 namespace wolf{
 class NodeBase;
@@ -341,7 +366,8 @@ inline Eigen::VectorXd StateBlock::identity() const
 {
     return Eigen::VectorXd::Zero(state_size_);
 }
-inline Eigen::VectorXd StateBlock::zero()     const
+
+inline Eigen::VectorXd StateBlock::zero() const
 {
     return identity();
 }
diff --git a/include/core/state_block/state_block_derived.h b/include/core/state_block/state_block_derived.h
index a4c34c448563e7126198f0218effe1a1d16ce0c8..6f0c50feeb7301ae6e90ea076cf5d311b07418cd 100644
--- a/include/core/state_block/state_block_derived.h
+++ b/include/core/state_block/state_block_derived.h
@@ -39,12 +39,21 @@ template <size_t size>
 class StateParams : public StateBlock
 {
   public:
-    StateParams(const Eigen::VectorXd& _state, bool _fixed = false) : StateBlock(_state, _fixed, nullptr, false) {}
+    StateParams(const Eigen::VectorXd& _state, bool _fixed = false) : 
+        StateBlock(_state, _fixed, nullptr, false) 
+    {
+        if (_state.size() != size) 
+            throw std::runtime_error("Wrong vector size for StateParams.");
+    }
+    static Eigen::VectorXd Identity()
+    {
+        return Eigen::Matrix<double,size,1>::Zero();
+    }
+    WOLF_STATE_BLOCK_CREATE(StateParams<size>);
     void transform(const VectorComposite& _transformation) override
     {
         // non transformable! --> do nothing
     }
-    static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 typedef StateParams<1>  StateParams1;
@@ -73,12 +82,18 @@ class StatePoint2d : public StateBlock
     StatePoint2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
         : StateBlock(_state, _fixed, nullptr, _transformable)
     {
+        if (_state.size() != 2) 
+            throw std::runtime_error("Wrong vector size for StatePoint2d.");
+    }
+    static Eigen::VectorXd Identity()
+    {
+        return Eigen::Vector2d::Zero();
     }
+    WOLF_STATE_BLOCK_CREATE(StatePoint2d);
     void transform(const VectorComposite& _transformation) override
     {
         if (transformable_) setState(_transformation.at('P') + Rotation2Dd(_transformation.at('O')(0)) * getState());
     }
-    static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 /**
@@ -97,12 +112,18 @@ class StateVector2d : public StateBlock
     StateVector2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
         : StateBlock(_state, _fixed, nullptr, _transformable)
     {
+        if (_state.size() != 2) 
+            throw std::runtime_error("Wrong vector size for StateVector2d.");
     }
+    static Eigen::VectorXd Identity()
+    {
+        return Eigen::Vector2d::Zero();
+    }
+    WOLF_STATE_BLOCK_CREATE(StateVector2d);
     void transform(const VectorComposite& _transformation) override
     {
         if (transformable_) setState(Rotation2Dd(_transformation.at('O')(0)) * getState());
     }
-    static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 /**
@@ -117,16 +138,24 @@ class StateVector2d : public StateBlock
 class StatePoint3d : public StateBlock
 {
   public:
-    StatePoint3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
+    StatePoint3d(const Eigen::VectorXd& _state, 
+                 bool _fixed = false, 
+                 bool _transformable = true)
         : StateBlock(_state, _fixed, nullptr, _transformable)
     {
+        if (_state.size() != 3) 
+            throw std::runtime_error("Wrong vector size for StatePoint3d.");
+    }
+    static Eigen::VectorXd Identity()
+    {
+        return Eigen::Vector3d::Zero();
     }
+    WOLF_STATE_BLOCK_CREATE(StatePoint3d);
     void transform(const VectorComposite& _transformation) override
     {
         if (transformable_)
             setState(_transformation.at('P') + Quaterniond(_transformation.at('O').data()) * getState());
     }
-    static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 /**
@@ -145,12 +174,18 @@ class StateVector3d : public StateBlock
     StateVector3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
         : StateBlock(_state, _fixed, nullptr, _transformable)
     {
+        if (_state.size() != 3) 
+            throw std::runtime_error("Wrong vector size for StateVector3d.");
+    }
+    static Eigen::VectorXd Identity()
+    {
+        return Eigen::Vector3d::Zero();
     }
+    WOLF_STATE_BLOCK_CREATE(StateVector3d);
     void transform(const VectorComposite& _transformation) override
     {
         if (transformable_) setState(Quaterniond(_transformation.at('O').data()) * getState());
     }
-    static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 }  // namespace wolf
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index 7307b8afb4e48e785a48eb9461a13ba0eda2707d..f6d851b488b1147514a3a7f76da68d1c325cae58 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -44,8 +44,8 @@ class MatrixComposite : public std::unordered_map < char, std::unordered_map < c
         std::unordered_map < char, unsigned int> size_rows_, size_cols_;
         unsigned int    rows() const;
         unsigned int    cols() const;
-        void            sizeAndIndices(const StateStructure & _struct_rows,
-                                       const StateStructure & _struct_cols,
+        void            sizeAndIndices(const StateKeys & _struct_rows,
+                                       const StateKeys & _struct_cols,
                                        std::unordered_map < char, unsigned int>& _indices_rows,
                                        std::unordered_map < char, unsigned int>& _indices_cols,
                                        unsigned int& _rows,
@@ -53,11 +53,11 @@ class MatrixComposite : public std::unordered_map < char, std::unordered_map < c
 
     public:
         MatrixComposite() {};
-        MatrixComposite(const StateStructure& _row_structure,
-                        const StateStructure& _col_structure);
-        MatrixComposite(const StateStructure& _row_structure,
+        MatrixComposite(const StateKeys& _row_structure,
+                        const StateKeys& _col_structure);
+        MatrixComposite(const StateKeys& _row_structure,
                         const std::list<int>& _row_sizes,
-                        const StateStructure& _col_structure,
+                        const StateKeys& _col_structure,
                         const std::list<int>& _col_sizes);
 
         /**
@@ -75,19 +75,19 @@ class MatrixComposite : public std::unordered_map < char, std::unordered_map < c
          *   vec_comp["b"].transpose(); // = (3,4,5);
          */
         MatrixComposite(const MatrixXd& _m,
-                        const StateStructure& _row_structure,
+                        const StateKeys& _row_structure,
                         const std::list<int>& _row_sizes,
-                        const StateStructure& _col_structure,
+                        const StateKeys& _col_structure,
                         const std::list<int>& _col_sizes);
         ~MatrixComposite()  = default;
 
         bool check() const;
-        static MatrixComposite zero(const StateStructure& _row_structure,
+        static MatrixComposite zero(const StateKeys& _row_structure,
                                     const std::list<int>& _row_sizes,
-                                    const StateStructure& _col_structure,
+                                    const StateKeys& _col_structure,
                                     const std::list<int>& _col_sizes);
 
-        static MatrixComposite identity(const StateStructure& _structure,
+        static MatrixComposite identity(const StateKeys& _structure,
                                         const std::list<int>& _sizes);
 
         unsigned int    count(const char &_row,
@@ -111,8 +111,8 @@ class MatrixComposite : public std::unordered_map < char, std::unordered_map < c
         using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::count;
 
 
-        MatrixXd        matrix(const StateStructure & _struct_rows,
-                               const StateStructure & _struct_cols) const;
+        MatrixXd        matrix(const StateKeys & _struct_rows,
+                               const StateKeys & _struct_cols) const;
 
         MatrixComposite operator * (double scalar_right) const;
         MatrixComposite operator + (const MatrixComposite & _second) const;
@@ -239,7 +239,7 @@ class StateBlockComposite
         StateBlockPtr           at(const char& _sb_type) const;
         void                    set(const char& _sb_type, const StateBlockPtr& _sb);
         void                    set(const char& _sb_type, const VectorXd& _vec);
-        void                    setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors);
+        void                    setVectors(const StateKeys& _structure, const std::list<VectorXd> &_vectors);
         bool                    key(const StateBlockPtr& _sb, char& _key) const;
         char             key(const StateBlockPtr& _sb) const;
         StateBlockMapCIter      find(const StateBlockPtr& _sb) const;
@@ -268,9 +268,9 @@ class StateBlockComposite
 
         // Get compact Eigen vector and related things
         SizeEigen               stateSize() const;
-        SizeEigen               stateSize(const StateStructure& _structure) const;
-        VectorXd                stateVector(const StateStructure& _structure) const;
-        void                    stateVector(const StateStructure& _structure, VectorXd& _vector) const;
+        SizeEigen               stateSize(const StateKeys& _structure) const;
+        VectorXd                stateVector(const StateKeys& _structure) const;
+        void                    stateVector(const StateKeys& _structure, VectorXd& _vector) const;
 
     private:
         StateBlockMap state_block_map_;
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index a576fa0ea2f4beb7aec0b8c49bfa8438e674a6d2..357a976044f3574e17d9c405e0119aa338725e22 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -32,19 +32,20 @@ class StateHomogeneous3d : public StateBlock
         StateHomogeneous3d(bool _fixed = false, bool _transformable = true);
         StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false, bool _transformable = true);
         ~StateHomogeneous3d() override;
+        static Eigen::VectorXd Identity();
+        WOLF_STATE_BLOCK_CREATE(StateHomogeneous3d);
 
         void setIdentity(bool _notify = true) override;
         Eigen::VectorXd identity() const override;
 
         virtual void transform(const VectorComposite& _transformation) override;
-
-        static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed, bool _transformable) :
         StateBlock(_state, _fixed, nullptr, _transformable)
 {
-    assert(_state.size() == 4 && "Homogeneous 3d must be size 4.");
+    if(_state.size() != 4)
+        throw std::runtime_error("Homogeneous 3d must be size 4.");
     local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
 }
 
@@ -60,7 +61,6 @@ inline StateHomogeneous3d::~StateHomogeneous3d()
     // The local_param_ptr_ pointer is already removed by the base class
 }
 
-
 inline void StateHomogeneous3d::setIdentity(bool _notify)
 {
     setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
@@ -71,16 +71,15 @@ inline Eigen::VectorXd StateHomogeneous3d::identity() const
     return Eigen::Quaterniond::Identity().coeffs();
 }
 
-inline void StateHomogeneous3d::transform(const VectorComposite& _transformation)
+inline Eigen::VectorXd StateHomogeneous3d::Identity()
 {
-    if (transformable_)
-        setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs()); // TODO is this correct?????? probably not!!!
+    return Eigen::Quaterniond::Identity().coeffs();
 }
 
-inline StateBlockPtr StateHomogeneous3d::create (const Eigen::VectorXd& _state, bool _fixed)
+inline void StateHomogeneous3d::transform(const VectorComposite& _transformation)
 {
-    MatrixSizeCheck<4,1>::check(_state);
-    return std::make_shared<StateHomogeneous3d>(_state, _fixed);
+    if (transformable_)
+        setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs()); // TODO is this correct?????? probably not!!!
 }
 
 } // namespace wolf
\ No newline at end of file
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 718f3965da56e0258c0fd02f93edb3fea6dd4f68..13632c59ee2d6f1cd102a9cb7996ffcbff85d1a3 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -33,13 +33,13 @@ class StateQuaternion : public StateBlock
         StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true);
         StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false, bool _transformable = true);
         ~StateQuaternion() override;
+        static Eigen::VectorXd Identity();
+        WOLF_STATE_BLOCK_CREATE(StateQuaternion);
 
         void setIdentity(bool _notify = true) override;
         Eigen::VectorXd identity() const override;
 
         virtual void transform(const VectorComposite& _transformation) override;
-
-        static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable) :
@@ -77,12 +77,16 @@ inline StateQuaternion::~StateQuaternion()
     // The local_param_ptr_ pointer is already removed by the base class
 }
 
-
 inline void StateQuaternion::setIdentity(bool _notify)
 {
     setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
 }
 
+inline Eigen::VectorXd StateQuaternion::Identity()
+{
+    return Eigen::Quaterniond::Identity().coeffs();
+}
+
 inline Eigen::VectorXd StateQuaternion::identity() const
 {
     return Eigen::Quaterniond::Identity().coeffs();
@@ -94,9 +98,4 @@ inline void StateQuaternion::transform(const VectorComposite& _transformation)
         setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs());
 }
 
-inline StateBlockPtr StateQuaternion::create (const Eigen::VectorXd& _state, bool _fixed)
-{
-    return std::make_shared<StateQuaternion>(_state, _fixed);
-}
-
 } // namespace wolf
\ No newline at end of file
diff --git a/include/core/state_block/type_composite.h b/include/core/state_block/type_composite.h
new file mode 100644
index 0000000000000000000000000000000000000000..5835e2e2b232434475249a88ee71feb8afec9079
--- /dev/null
+++ b/include/core/state_block/type_composite.h
@@ -0,0 +1,114 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+#include "core/common/wolf.h"
+#include "core/state_block/composite.h"
+
+namespace wolf
+{
+
+typedef Composite<std::string> TypeComposite;
+void checkTypeComposite(const TypeComposite& _types, SizeEigen _dim = 0);
+bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim = 0);
+
+template <>
+inline Composite<std::string>::Composite(const YAML::Node& _n)
+{
+    if (not _n.IsMap())
+    {
+        throw std::runtime_error("SpecComposite: constructor with a non-map yaml node");
+    }
+
+    for (auto spec_pair : _n)
+    {
+        this->emplace(spec_pair.first.as<char>(), spec_pair.second.as<std::string>());
+    }
+}
+
+inline bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim)
+{
+    try
+    {
+        checkTypeComposite(_types, _dim);
+    }
+    catch(const std::exception& e)
+    {
+        return false;
+    }
+    return true;
+}
+
+inline void checkTypeComposite(const TypeComposite& _types, SizeEigen _dim)
+{
+    if (_dim == 2)
+    {
+        if (_types.count('P') != 0 and _types.at('P') != "StatePoint2d")
+        {
+            throw std::runtime_error("Bad TypeComposite: Type for 'P' in 2D should be 'StatePoint2d'");
+        }
+        if (_types.count('O') != 0 and _types.at('O') != "StateAngle")
+        {
+            throw std::runtime_error("Bad TypeComposite: Type for 'O' in 2D should be 'StateAngle'");
+        }
+        if (_types.count('V') != 0 and _types.at('V') != "StateVector2d")
+        {
+            throw std::runtime_error("Bad TypeComposite: Type for 'V' in 2D should be 'StateVector2d'");
+        }
+    }
+    else if (_dim == 3)
+    {
+        if (_types.count('P') != 0 and _types.at('P') != "StatePoint3d")
+        {
+            throw std::runtime_error("Bad TypeComposite: Type for 'P' in 3D should be 'StatePoint3d'");
+        }
+        if (_types.count('O') != 0 and _types.at('O') != "StateQuaternion")
+        {
+            throw std::runtime_error("Bad TypeComposite: Type for 'O' in 3D should be 'StateQuaternion'");
+        }
+        if (_types.count('V') != 0 and _types.at('V') != "StateVector3d")
+        {
+            throw std::runtime_error("Bad TypeComposite: Type for 'V' in 3D should be 'StateVector3d'");
+        }
+    }
+    else if (_dim == 0)
+    {
+        if (_types.count('P') != 0 and _types.at('P') != "StatePoint2d" and _types.at('P') != "StatePoint3d")
+        {
+            throw std::runtime_error("Bad TypeComposite: Type for 'P' should be 'StatePoint2d' or 'StatePoint3d'");
+        }
+        if (_types.count('O') != 0 and _types.at('O') != "StateAngle" and _types.at('O') != "StateQuaternion")
+        {
+            throw std::runtime_error("Bad TypeComposite: Type for 'O' should be 'StateAngle' or 'StateQuaternion'");
+        }
+        if (_types.count('V') != 0 and _types.at('V') != "StateVector2d" and _types.at('V') != "StateVector3d")
+        {
+            throw std::runtime_error("Bad TypeComposite: Type for 'V' should be 'StateVector2d' or 'StateVector3d'");
+        }
+    }
+    else
+    {
+        throw std::runtime_error("checkTypeComposite(_types, _dim) bad value for '_dim', should be 2 for 2D, 3 for 3D, or 0 for not defined");
+    }
+}
+
+}  // namespace wolf
\ No newline at end of file
diff --git a/include/core/state_block/vector_composite.h b/include/core/state_block/vector_composite.h
index f5a87bceae193ba31e6d203667443370d49ae451..ebed1706395b60e87650c1d621bad052a8c44b45 100644
--- a/include/core/state_block/vector_composite.h
+++ b/include/core/state_block/vector_composite.h
@@ -37,11 +37,9 @@ class VectorComposite : public Composite<Eigen::VectorXd>
     public:
         using Composite<Eigen::VectorXd>::Composite;
         
-        VectorComposite() {};
-        VectorComposite(const StateStructure& _s);
-        VectorComposite(const StateStructure& _s, const std::list<int>& _sizes);
+        VectorComposite();
         /**
-         * \brief Construct from Eigen::VectorXd and structure
+         * \brief Construct from Eigen::VectorXd and keys
          *
          * Usage:
          *
@@ -54,13 +52,13 @@ class VectorComposite : public Composite<Eigen::VectorXd>
          *   vec_comp["a"].transpose(); // = (1,2);
          *   vec_comp["b"].transpose(); // = (3,4,5);
          */
-        VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
-        VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors);
+        VectorComposite(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes);
+        VectorComposite(const StateKeys& _keys, const std::list<VectorXd>& _vectors);
 
-        Eigen::VectorXd vector(const StateStructure& _structure) const;
+        Eigen::VectorXd vector(const StateKeys& _keys) const;
 
         /**
-         * \brief set from Eigen::VectorXd and structure
+         * \brief set from Eigen::VectorXd and keys
          *
          * Usage:
          *
@@ -74,7 +72,7 @@ class VectorComposite : public Composite<Eigen::VectorXd>
          *   vec_comp["a"].transpose(); // = (1,2);
          *   vec_comp["b"].transpose(); // = (3,4,5);
          */
-        void set(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
+        void set(const VectorXd& _v, const StateKeys& _keys, const std::list<int>& _sizes);
         void setZero();
 
         friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 194c942d76f97e88890855ab406ebb3add8306ff..4c16c1e0c094e9b822a547ae6fca5484ee0f13da 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -43,7 +43,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
          FramePtrMap frame_map_;
 
     protected:
-         StateStructure frame_structure_;  // Defines the structure of the Frames in the Trajectory.
+         StateKeys frame_structure_;  // Defines the structure of the Frames in the Trajectory.
         
     public:
         TrajectoryBase();
diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
index c4f4e4d5655c6f3519fb75e91cfab8123ac40c0c..8c3df91f865ae1136d97a495452dff1f550910ba 100644
--- a/include/core/tree_manager/factory_tree_manager.h
+++ b/include/core/tree_manager/factory_tree_manager.h
@@ -35,7 +35,7 @@ struct ParamsTreeManagerBase;
 namespace wolf
 {
 
-typedef Factory<TreeManagerBase,
+typedef Factory<std::shared_ptr<TreeManagerBase>,
                 const YAML::Node&> FactoryTreeManager;
 template<>
 inline std::string FactoryTreeManager::getClass() const
@@ -43,7 +43,7 @@ inline std::string FactoryTreeManager::getClass() const
     return "FactoryTreeManager";
 }
 
-typedef Factory<TreeManagerBase,
+typedef Factory<std::shared_ptr<TreeManagerBase>,
                 const std::string&,
                 const std::vector<std::string>&> FactoryTreeManagerYaml;
 template<>
diff --git a/schema/problem/Problem.schema b/schema/problem/Problem.schema
index 0a7943acdbb1beca7e2946d4d0f90e1a08899d99..7b24224127a6efef1b12f83ec451026f897f2210 100644
--- a/schema/problem/Problem.schema
+++ b/schema/problem/Problem.schema
@@ -5,11 +5,6 @@ problem:
     yaml_type: scalar
     mandatory: true
     doc: Tree manager parameters
-  frame_structure:
-    yaml_type: scalar
-    type: string
-    mandatory: true
-    doc: The keys of the default frames in problems
   dimension:
     yaml_type: scalar
     type: int
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index c83686ef4100dfa4cee61e576f99fa0759a1e94a..83368de1ca47b17f139872eed0cce274d7f69f8a 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -119,7 +119,7 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
             std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
             //frame state blocks
             for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
-                for (auto key : fr_pair.second->getStructure())
+                for (auto key : fr_pair.second->getKeys())
                 {
                     const auto& sb = fr_pair.second->getStateBlock(key);
                     all_state_blocks.push_back(sb);
@@ -127,7 +127,7 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
 
             // landmark state blocks
             for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
-                for (auto key : l_ptr->getStructure())
+                for (auto key : l_ptr->getKeys())
                 {
                     const auto& sb = l_ptr->getStateBlock(key);
                     all_state_blocks.push_back(sb);
@@ -152,12 +152,12 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
         {
             // first create a vector containing all state blocks
             for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
-                for (auto key1 : fr_pair.second->getStructure())
+                for (auto key1 : fr_pair.second->getKeys())
                 {
                     const auto& sb1 = fr_pair.second->getStateBlock(key1);
                     assert(isStateBlockRegisteredDerived(sb1));
 
-                    for (auto key2 : fr_pair.second->getStructure())
+                    for (auto key2 : fr_pair.second->getKeys())
                     {
                         const auto& sb2 = fr_pair.second->getStateBlock(key2);
                         assert(isStateBlockRegisteredDerived(sb2));
@@ -171,12 +171,12 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
 
             // landmark state blocks
             for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
-                for (auto key1 : l_ptr->getStructure())
+                for (auto key1 : l_ptr->getKeys())
                 {
                     const auto& sb1 = l_ptr->getStateBlock(key1);
                     assert(isStateBlockRegisteredDerived(sb1));
 
-                    for (auto key2 : l_ptr->getStructure())
+                    for (auto key2 : l_ptr->getKeys())
                     {
                         const auto& sb2 = l_ptr->getStateBlock(key2);
                         assert(isStateBlockRegisteredDerived(sb2));
@@ -211,7 +211,7 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
 
             // landmarks
             for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
-                for (auto key : l_ptr->getStructure())
+                for (auto key : l_ptr->getKeys())
                 {
                     const auto& sb = l_ptr->getStateBlock(key);
                     assert(isStateBlockRegisteredDerived(sb));
@@ -225,7 +225,7 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
                                               getAssociatedMemBlockPtr(sb));
 
                     // landmark marginal
-                    for (auto key2 : l_ptr->getStructure())
+                    for (auto key2 : l_ptr->getKeys())
                     {
                         const auto& sb2 = l_ptr->getStateBlock(key2);
                         assert(isStateBlockRegisteredDerived(sb2));
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index a681f73a25a76e506b151fca743cf4ef181dec43..0208dd62c388c6a9856c122b5c05ebacec1445bd 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -34,7 +34,7 @@ namespace wolf {
 unsigned int FrameBase::frame_id_count_ = 0;
 
 // FrameBase::FrameBase(const TimeStamp& _ts,
-//                      const StateStructure& _frame_structure,
+//                      const StateKeys& _frame_structure,
 //                      const VectorComposite& _state) :
 //         NodeBase("FRAME", "FrameBase"),
 //         HasStateBlocks(_frame_structure),
@@ -44,7 +44,7 @@ unsigned int FrameBase::frame_id_count_ = 0;
 // {
 //     assert(_state.has(_frame_structure) && "_state does not include all keys of _frame_structure");
 
-//     for (auto key : getStructure())
+//     for (auto key : getKeys())
 //     {
 //         StateBlockPtr sb = FactoryStateBlock::create(std::string(1,key), _state.at(key), false); // false = non fixed
 //         addStateBlock(key, sb, getProblem());
@@ -53,7 +53,7 @@ unsigned int FrameBase::frame_id_count_ = 0;
 
 
 // FrameBase::FrameBase(const TimeStamp& _ts,
-//                      const StateStructure& _frame_structure,
+//                      const StateKeys& _frame_structure,
 //                      const std::list<VectorXd>& _vectors) :
 //                 NodeBase("FRAME", "FrameBase"),
 //                 HasStateBlocks(_frame_structure),
@@ -440,7 +440,7 @@ void FrameBase::setProblem(ProblemPtr _problem)
 void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
 {
     _stream << _tabs << "Frm" << id()
-            << " " << getStructure()
+            << " " << getKeys()
             << " ts = " << std::setprecision(3) << getTimeStamp()
             << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C  " : "");
     if (_constr_by)
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 72b1834617074da8a53a89a1097d28d8cdfeebb3..82f91377428bb34ab7b46c29bac5b1adc5890626 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -90,7 +90,7 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
 //     std::vector<StateBlockConstPtr> used_state_block_vec(0);
 
 //     // normal state blocks in {P,O,V,W}
-//     for (auto key : getStructure())
+//     for (auto key : getKeys())
 //     {
 //         const auto sbp = getStateBlock(key);
 //         if (sbp)
@@ -110,7 +110,7 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
 //     std::vector<StateBlockPtr> used_state_block_vec(0);
 
 //     // normal state blocks in {P,O,V,W}
-//     for (auto key : getStructure())
+//     for (auto key : getKeys())
 //     {
 //         auto sbp = getStateBlock(key);
 //         if (sbp)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index acab5763aad76e554d256b8a31d52ea98da1c19e..8712f0da95f856e2f21853924bc58a31e62ed1bc 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -28,6 +28,7 @@
 #include "core/capture/capture_void.h"
 #include "core/factor/factor_block_absolute.h"
 #include "core/factor/factor_quaternion_absolute.h"
+#include "core/state_block/factory_state_block.h"
 #include "core/state_block/state_quaternion.h"
 #include "core/state_block/state_angle.h"
 #include "core/tree_manager/factory_tree_manager.h"
@@ -41,71 +42,52 @@
 namespace wolf
 {
 
-Problem::Problem(const TypeComposite& _frame_structure, SizeEigen _dim, MapBasePtr _map) :
+Problem::Problem(const TypeComposite& _frame_types, SizeEigen _dim) :
         tree_manager_(nullptr),
         hardware_ptr_(std::make_shared<HardwareBase>()),
         trajectory_ptr_(std::make_shared<TrajectoryBase>()),
-        map_ptr_(_map),
+        map_ptr_(std::make_shared<MapBase>()),
         motion_provider_map_(),
-        frame_structure_(_frame_structure),
+        dim_(_dim),
+        frame_types_(_frame_types),
         prior_options_(),
         prior_applied_(false),
         transformed_(false)
 {
+    checkTypeComposite(frame_types_, dim_);
 }
 
-Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) :
+Problem::Problem(const StateKeys& _frame_keys, SizeEigen _dim) :
         tree_manager_(nullptr),
         hardware_ptr_(std::make_shared<HardwareBase>()),
         trajectory_ptr_(std::make_shared<TrajectoryBase>()),
-        map_ptr_(_map),
+        map_ptr_(std::make_shared<MapBase>()),
         motion_provider_map_(),
-        //frame_structure_(_frame_structure),
-        frame_structure_(),
+        dim_(_dim),
+        frame_types_(),
         prior_options_(),
         prior_applied_(false),
         transformed_(false)
 {
-    dim_ = _dim;
-    for (auto key : _frame_structure)
+    for (auto key : _frame_keys)
     {
         if (key == 'P')
         {
-            appendToStructure({{'P', (dim_ == 2 ? "StatePoint2d" : "StatePoint3d")}});
+            appendToFrameTypes({{'P', (dim_ == 2 ? "StatePoint2d" : "StatePoint3d")}});
         }
         else if (key == 'O')
         {
-            appendToStructure({{'P', (dim_ == 2 ? "StateAngle" : "StateQuaternion")}});
+            appendToFrameTypes({{'O', (dim_ == 2 ? "StateAngle" : "StateQuaternion")}});
         }
         else if (key == 'V')
         {
-            appendToStructure({{'P', (dim_ == 2 ? "StateVector2d" : "StateVector3d")}});
+            appendToFrameTypes({{'V', (dim_ == 2 ? "StateVector2d" : "StateVector3d")}});
         }
         else
         {
             throw std::runtime_error("Problem::Problem: Unknown type for key " + std::string(1,key) + " use the constructor via TypeComposite");
         }
     }
-
-    // if (_frame_structure == "PO" and _dim == 2)
-    // {
-    //     state_size_ = 3;
-    //     state_cov_size_ = 3;
-    // }else if (_frame_structure == "PO" and _dim == 3)
-    // {
-    //     state_size_ = 7;
-    //     state_cov_size_ = 6;
-    // }else if (_frame_structure == "POV" and _dim == 3)
-    // {
-    //     state_size_ = 10;
-    //     state_cov_size_ = 9;
-    // }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.");
 }
 
 void Problem::setup()
@@ -116,16 +98,16 @@ void Problem::setup()
         map_ptr_   -> setProblem(shared_from_this());
 }
 
-ProblemPtr Problem::create(const TypeComposite& _frame_structure, SizeEigen _dim, MapBasePtr _map)
+ProblemPtr Problem::create(const TypeComposite& _frame_types, SizeEigen _dim)
 {
-    ProblemPtr p(new Problem(_frame_structure, _dim, _map)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
+    ProblemPtr p(new Problem(_frame_types, _dim)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
     p->setup();
     return p->shared_from_this();
 }
 
-ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map)
+ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
 {
-    ProblemPtr p(new Problem(_frame_structure, _dim, _map)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
+    ProblemPtr p(new Problem(_frame_structure, _dim)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
     p->setup();
     return p->shared_from_this();
 }
@@ -188,14 +170,11 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
     // structure and dimension
     WOLF_INFO("Loading problem parameters");
     YAML::Node problem_node = param_node["problem"];
-    std::string frame_structure;
-    if (problem_node["frame_structure"])
-    {
-        // TODO: accept types!
-        frame_structure = problem_node["frame_structure"].as<std::string>();
-    }
     int  dim     = problem_node["dimension"].as<int>();
-    auto problem = Problem::create(frame_structure, dim, nullptr);
+    TypeComposite frame_types;
+    if (problem_node["frame_types"])
+        frame_types = TypeComposite(problem_node["frame_types"]);
+    auto problem = Problem::create(frame_types, dim);
     
     // Tree manager
     WOLF_INFO("Loading tree manager parameters");
@@ -393,140 +372,119 @@ ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name)
     return nullptr;
 }
 
-// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
-//                                    const StateStructure& _frame_structure, //
-//                                    const SizeEigen _dim, //
-//                                    const Eigen::VectorXd& _frame_state)
-// {
-//     VectorComposite state;
-//     SizeEigen index = 0;
-//     SizeEigen size = 0;
-//     for (auto key : _frame_structure)
-//     {
-//         if (key == 'O')
-//         {
-//             if (_dim == 2) size = 1;
-//             else size = 4;
-//         }
-//         else
-//             size = _dim;
-
-//         assert (_frame_state.size() >= index+size && "State vector incompatible with given structure and dimension! Vector too small.");
-
-//         state.emplace(key, _frame_state.segment(index, size));
-
-//         // append new key to frame structure
-//         if (frame_structure_.find(key) == std::string::npos) // not found
-//             frame_structure_ += std::string(1,key);
-
-//         index += size;
-//     }
-
-//     assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large.");
-
-//     auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_,
-//                                              _time_stamp,
-//                                              _frame_structure,
-//                                              state);
-
-//     return frm;
-// }
-
-// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
-//                                    const StateStructure& _frame_structure, //
-//                                    const SizeEigen _dim)
-// {
-//     return emplaceFrame(_time_stamp,
-//                         _frame_structure,
-//                         getState(_time_stamp));
-// }
-
-FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
-                                    //const StateStructure& _frame_structure, //
-                                    const TypeComposite& _frame_structure, //
+FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp,
+                                    const TypeComposite& _frame_types,
                                     const VectorComposite& _frame_state)
 {
+    // check input _frame_specs
+    checkTypeComposite(_frame_types);
+
+    // check same keys in state and types
+    if (_frame_types.getKeys() != _frame_state.getKeys())
+    {
+        throw std::runtime_error("Problem::emplaceFrame: Different keys in given TypeComposite and VectorComposite. TypeComposite: " + 
+                                 _frame_types.getKeys() + 
+                                 "VectorComposite: " + 
+                                 _frame_state.getKeys());
+    }
+
     return FrameBase::emplace<FrameBase>(getTrajectory(),
                                          _time_stamp,
-                                         _frame_structure,
+                                         _frame_types,
                                          _frame_state);
 }
 
-FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, //
-                                    const VectorComposite& _frame_state)
+FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp,
+                                    const StateKeys& _frame_keys)
 {
-    // // append new keys to frame structure
-    // for (auto pair_key_vec : _frame_state)
-    // {
-    //     if (not frame_structure_.has(pair_key_vec.first))
-    //     auto key = pair_key_vec.first;
-    //     if (frame_structure_.find(key) == std::string::npos) // not found
-    //         frame_structure_ += std::string(1,key);
-    // }
-
-    if (not frame_structure_.has(_frame_state.getStructure()))
+    if (not frame_types_.has(_frame_keys))
     {
-        throw std::runtime_error("Problem::emplaceFrame: the given VectorComposite has a state with an unknown key");
+        throw std::runtime_error("Problem::emplaceFrame: any unknown key... asked for " + _frame_keys + " but only have " + frame_types_.getKeys());
     }
 
-    return FrameBase::emplace<FrameBase>(getTrajectory(),
-                                         _time_stamp,
-                                         getFrameStructure(),
-                                         _frame_state);
+    auto state = getState(_time_stamp, _frame_keys);
+
+    return emplaceFrame (_time_stamp,
+                         getFrameTypes(state.getKeys()),
+                         state);
 }
 
-// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, //
-//                                    const Eigen::VectorXd& _frame_state)
-// {
-//     // return emplaceFrame(_time_stamp,
-//     //                     this->getFrameStructure(),
-//     //                     this->getDim(),
-//     //                     _frame_state);
-//     return emplaceFrame(_time_stamp,
-//                         this->getFrameStructure(),
-//                         this->getDim(),
-//                         _frame_state);
-// }
-
-// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp)
-// {
-//     return emplaceFrame(_time_stamp,
-//                         this->getFrameStructure(),
-//                         this->getDim());
-// }
+FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp,
+                                    const VectorComposite& _frame_state)
+{
+    if (not frame_types_.has(_frame_state.getKeys()))
+    {
+        throw std::runtime_error("Problem::emplaceFrame: any unknown key... asked for " + _frame_state.getKeys() + " but only have " + frame_types_.getKeys());
+    }
+
+    return Problem::emplaceFrame(_time_stamp,
+                                 getFrameTypes(_frame_state.getKeys()),
+                                 _frame_state);
+}
 
 FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp,
-                                   const SpecComposite& _frame_spec_composite)
+                                   const StateKeys& _frame_keys,
+                                   const Eigen::VectorXd& _frame_state)
 {
-    // Checks
-    if (_frame_spec_composite.count('P') == 0 or _frame_spec_composite.count('O') == 0)
+    if (not frame_types_.has(_frame_keys))
     {
-        WOLF_INFO(_frame_spec_composite);
-        throw std::runtime_error("Problem::emplaceFrame: emplacing a frame without P or O");
+        throw std::runtime_error("Problem::emplaceFrame: any unknown key... asked for " + _frame_keys + " but only have " + frame_types_.getKeys());
     }
-    if (_frame_spec_composite.at('P').getType() != "P" and 
-        _frame_spec_composite.at('P').getType() != (dim_ == 2 ? "StatePoint2d" : "StatePoint3d"))
+
+    VectorComposite vec_composite;
+
+    int next_state_index = 0;
+    for (auto key : _frame_keys)
     {
-        throw std::runtime_error("Problem::emplaceFrame: State type of key P should be 'StatePoint2d' or 'StatePoint3d' for 2D and 3D problem, respectively");
+        auto state_size = FactoryStateBlockIdentityVector::create(frame_types_.at(key)).size();
+
+        if (_frame_state.size() < next_state_index + state_size)
+        {
+            throw std::runtime_error("Problem::emplaceFrame: the provided vector _frame_state is too short");
+        }
+
+        vec_composite[key] = _frame_state.segment(next_state_index, state_size);
+        next_state_index += state_size;
     }
-    if (_frame_spec_composite.at('O').getType() != "O" and 
-        _frame_spec_composite.at('O').getType() != (dim_ == 2 ? "StateAngle" : "StateQuaternion"))
+    if (_frame_state.size() != next_state_index)
     {
-        throw std::runtime_error("Problem::emplaceFrame: State type of key O should be 'StateAngle' or 'StateQuaternion' for 2D and 3D problem, respectively");
+        throw std::runtime_error("Problem::emplaceFrame: the provided vector _frame_state is too long");
     }
-    if (_frame_spec_composite.count('V') != 0 and 
-        _frame_spec_composite.at('V').getType() != "V" and 
-        _frame_spec_composite.at('V').getType() != (dim_ == 2 ? "StateVector2d" : "StateVector3d"))
+
+    return Problem::emplaceFrame(_time_stamp,
+                                 getFrameTypes(vec_composite.getKeys()),
+                                 vec_composite);
+}
+
+FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp,
+                                   const SpecComposite& _frame_specs)
+{
+    // check input _frame_specs
+    checkTypeComposite(_frame_specs.getTypeComposite());
+
+    // Check _frame_specs are consistent with frame_types_
+    auto input_types = _frame_specs.getTypeComposite();
+    for (auto type_pair : frame_types_)
     {
-        throw std::runtime_error("Problem::emplaceFrame: State type of key O should be 'StateAngle' or 'StateQuaternion' for 2D and 3D problem, respectively");
+        if (input_types.count(type_pair.first) and input_types.at(type_pair.first) != input_types.at(type_pair.first))
+        {
+            throw std::runtime_error("Problem::emplaceFrame: provided _frame_specs specify the type " + 
+                                     input_types.at(type_pair.first) + 
+                                     " for key " +
+                                     std::string(1, type_pair.first) + 
+                                     " but in problem, this key is specified as: " +
+                                     input_types.at(type_pair.first));
+        }
     }
 
+    // add the types of the keys that are not in frame_types_
+    appendToFrameTypes(_frame_specs.getTypeComposite());
+
     return FrameBase::emplace<FrameBase>(getTrajectory(),
                                          _time_stamp,
-                                         _frame_spec_composite);
+                                         _frame_specs);
 }
 
-
 TimeStamp Problem::getTimeStamp ( ) const
 {
     TimeStamp  ts = TimeStamp::Invalid();
@@ -552,48 +510,47 @@ TimeStamp Problem::getTimeStamp ( ) const
 }
 
 
-VectorComposite Problem::getState(const StateStructure& _structure) const
+VectorComposite Problem::getState(const StateKeys& _keys) const
 {
-    StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+    StateKeys keys = (_keys == "" ? getFrameKeys() : _keys);
 
     VectorComposite state;
 
     // compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
     for (auto prc_pair : motion_provider_map_)
     {
-        auto prc_state = prc_pair.second->getState(structure);
+        auto prc_state = prc_pair.second->getState(keys);
 
         // transfer processor vector blocks to problem state
         for (auto pair_key_vec : prc_state)
         {
-            if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
+            if (not state.has(pair_key_vec.first)) // Only write once. This gives priority to processors with more priority
             {  
-              state.insert(pair_key_vec);
+                state.insert(pair_key_vec);
             }
         }
 
         //If all keys are filled return
-        if (state.size() == structure.size())
+        if (state.has(keys))
         {
             return state;
         }
     }
 
     // check for empty blocks and fill them with the last KF, with the prior, or with zeros in the worst case
-
     VectorComposite state_last;
 
     auto last_kf = trajectory_ptr_->getLastFrame();
 
     if (last_kf)
-        state_last = last_kf->getState(structure);
+        state_last = last_kf->getState(keys);
 
     else if (not prior_options_.empty())
         state_last = prior_options_.getVectorComposite();
 
-    for (auto key : structure)
+    for (auto key : keys)
     {
-        if (state.count(key) == 0)
+        if (state.has(key) == 0)
         {
             auto state_last_it = state_last.find(key);
 
@@ -608,9 +565,9 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
     return state;
 }
 
-VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _structure) const
+VectorComposite Problem::getState (const TimeStamp& _ts, const StateKeys& _structure) const
 {
-    StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+    StateKeys structure = (_structure == "" ? getFrameKeys() : _structure);
 
     VectorComposite state;
 
@@ -662,9 +619,9 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
     return state;
 }
 
-VectorComposite Problem::getOdometry(const StateStructure& _structure) const
+VectorComposite Problem::getOdometry(const StateKeys& _structure) const
 {
-    StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+    StateKeys structure = (_structure == "" ? getFrameKeys() : _structure);
 
     VectorComposite odom_state;
 
@@ -710,24 +667,28 @@ SizeEigen Problem::getDim() const
     return dim_;
 }
 
-const StateStructure& Problem::getFrameStructure() const
+StateKeys Problem::getFrameKeys() const
 {
-    // return frame_structure_;
-    return frame_structure_.getStructure();
+    return frame_types_.getKeys();
 }
 
-// void Problem::appendToStructure(const StateStructure& _structure)
-// {
-//     for (auto key : _structure)
-//         if (frame_structure_.find(key) == std::string::npos) // now key not found in problem structure -> append!
-//             frame_structure_ += key;
-// }
+TypeComposite Problem::getFrameTypes(StateKeys _keys) const
+{
+    if (_keys == "")
+        return frame_types_;
+    
+    TypeComposite types;
+    for (auto key : _keys)
+        types.emplace(key, frame_types_.at(key));
+
+    return types;
+}
 
-void Problem::appendToStructure(const TypeComposite& _types)
+void Problem::appendToFrameTypes(const TypeComposite& _types)
 {
     for (auto pair : _types)
-        if (not frame_structure_.has(pair.first)) // now key not found in problem structure -> append!
-            frame_structure_.emplace(pair.first, pair.second);
+        if (not frame_types_.has(pair.first)) // now key not found in problem structure -> append!
+            frame_types_.emplace(pair.first, pair.second);
 }
 
 void Problem::setTreeManager(TreeManagerBasePtr _gm)
@@ -741,9 +702,6 @@ void Problem::setTreeManager(TreeManagerBasePtr _gm)
 
 void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr)
 {
-    // Grow TypeComposite storing information about the types of the StateBlocks corresponding to each key
-    // TODO
-
     // Check if is state getter
     if (not _motion_provider_ptr->isStateGetter())
     {
@@ -763,7 +721,21 @@ void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr)
 
     // add to map ordered by priority
     motion_provider_map_.emplace(_motion_provider_ptr->getOrder(), _motion_provider_ptr);
-    appendToStructure(_motion_provider_ptr->getStateStructure());
+
+    // check consistent types
+    for (auto new_key : _motion_provider_ptr->getStateTypes().getKeys())
+    if (frame_types_.has(new_key) and frame_types_[new_key] != _motion_provider_ptr->getStateTypes().at(new_key))
+    {
+        throw std::runtime_error("Problem::addMotionProvider: inconsistent type for key " + 
+                                 std::string(1,new_key) + 
+                                 " previously registered as " + 
+                                 frame_types_[new_key] + 
+                                 " and now as " + 
+                                 _motion_provider_ptr->getStateTypes().at(new_key));
+    }
+
+    // Grow TypeComposite storing information about the types of the StateBlocks corresponding to each key
+    appendToFrameTypes(_motion_provider_ptr->getStateTypes());
 }
 
 void Problem::removeMotionProvider(MotionProviderPtr proc)
@@ -773,33 +745,34 @@ void Problem::removeMotionProvider(MotionProviderPtr proc)
     motion_provider_map_.erase(proc->getOrder());
 
     // rebuild frame structure with remaining motion processors
-    frame_structure_.clear();
+    frame_types_.clear();
     for (auto pm : motion_provider_map_)
-        appendToStructure(pm.second->getStateStructure());
+        appendToFrameTypes(pm.second->getStateTypes());
 }
 
-VectorComposite Problem::stateZero ( const StateStructure& _structure ) const
+VectorComposite Problem::stateZero ( TypeComposite _types ) const
 {
-    StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+    if (_types.empty())
+        _types = frame_types_;
 
     VectorComposite state;
-    for (auto key : structure)
+    for (auto&& type_pair : _types)
     {
-        VectorXd vec;
-        if (key == 'O')
-        {
-            if (dim_ == 2) vec = VectorXd::Zero(1);
-            else if (dim_ == 3) vec = Quaterniond::Identity().coeffs();
-        }
-        else
-        {
-            vec = VectorXd::Zero(dim_);
-        }
-        state.emplace(key, vec);
+        state.emplace(type_pair.first, FactoryStateBlockIdentityVector::create(type_pair.second));
     }
     return state;
 }
 
+VectorComposite Problem::stateZero ( const StateKeys& _keys ) const
+{
+    if (not frame_types_.has(_keys))
+        throw std::runtime_error("Problem::stateZero any unknown key... asked for " + _keys + " but only have " + frame_types_.getKeys());
+
+    StateKeys keys = (_keys == "" ? getFrameKeys() : _keys);
+
+    return stateZero(getFrameTypes(_keys));
+}
+
 bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr) const
 {
     // This should implement a black list of processors that have forbidden keyframe creation
@@ -1097,8 +1070,6 @@ MapBasePtr Problem::getMap()
 
 void Problem::setMap(MapBasePtr _map_ptr)
 {
-    assert(map_ptr_ == nullptr && "map has already been set");
-
     map_ptr_ = _map_ptr;
 }
 
diff --git a/src/processor/motion_provider.cpp b/src/processor/motion_provider.cpp
index 4cd15583000eb43bc5802aad9bd59e8e6b803fde..bd232f8d8b23af080711e953998e9042e54d7f20 100644
--- a/src/processor/motion_provider.cpp
+++ b/src/processor/motion_provider.cpp
@@ -26,19 +26,18 @@ using namespace wolf;
 
 void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr)
 {
-    setOdometry(_prb_ptr->stateZero(state_structure_));
-    if (not isStateGetter())
+    WOLF_DEBUG_COND(not isStateGetter(), "MotionProvider::addToProblem: MotionProvider processor with state_provider=false. Not adding this processor");
+    if (isStateGetter())
     {
-        WOLF_WARN("MotionProvider::addToProblem: MotionProvider processor with state_provider=false. Not adding this processor");
-        return;
+        _prb_ptr->addMotionProvider(_motion_ptr);
     }
-    _prb_ptr->addMotionProvider(_motion_ptr);
+    setOdometry(_prb_ptr->stateZero(state_types_));
 }
 
 void MotionProvider::setOdometry(const VectorComposite& _odom)
 {
-    assert(_odom.has(state_structure_) && "MotionProvider::setOdometry(): any key missing in _odom.");
+    assert(_odom.has(getStateKeys()) && "MotionProvider::setOdometry(): any key missing in _odom.");
 
-    for (auto key : state_structure_)
-        odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_structure_
+    for (auto key : getStateKeys())
+        odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_types_
 }
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index fb2f26fea436e06e2c68f87f958d8f9209b8f586..e3dfc26a3c9856d76a96ea26efc7a04e996e7493 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -141,12 +141,13 @@ void ProcessorBase::link(SensorBasePtr _sen_ptr)
 
 void ProcessorBase::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;
 
+    if (dim_ != 0 and dim_ != _problem->getDim())
+        throw std::runtime_error("Processor works with " + std::to_string(dim_) + 
+                                 "D but problem is " + std::to_string(_problem->getDim()) + "D");
+
     NodeBase::setProblem(_problem);
 
     // adding motion provider to the motion providers vector
diff --git a/src/processor/processor_fixed_wing_model.cpp b/src/processor/processor_fixed_wing_model.cpp
index 25ec91bc928b5c057db3bbf8b81eebb840149e1d..527f8b2c2e556ef065f4347ff1fcf412177bc59a 100644
--- a/src/processor/processor_fixed_wing_model.cpp
+++ b/src/processor/processor_fixed_wing_model.cpp
@@ -41,7 +41,7 @@ ProcessorFixedWingModel::~ProcessorFixedWingModel()
 
 void ProcessorFixedWingModel::configure(SensorBasePtr _sensor)
 {
-    assert(_sensor->getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V");
+    assert(_sensor->getProblem()->getFrameKeys().find('V') != std::string::npos && "Processor only works with problems with V");
 }
 
 void ProcessorFixedWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr)
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 2936dfc16e9be074686db29f97e060302affc1ca..d69eb4b4ead92ee67c549570e11d541c8bc09cab 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -19,14 +19,6 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-/**
- * \file processor_motion.cpp
- *
- *  Created on: 15/03/2016
- *      \author: jsola
- */
-
-
 
 #include "core/processor/processor_motion.h"
 #include "core/state_block/factory_state_block.h"
@@ -35,7 +27,7 @@ namespace wolf
 {
 
 ProcessorMotion::ProcessorMotion(const std::string& _type,
-                                 std::string _state_structure,
+                                 TypeComposite _state_types,
                                  int _dim,
                                  SizeEigen _state_size,
                                  SizeEigen _delta_size,
@@ -44,7 +36,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
                                  SizeEigen _calib_size,
                                  ParamsProcessorMotionPtr _params_motion) :
         ProcessorBase(_type, _dim, _params_motion),
-        MotionProvider(_state_structure, _params_motion),
+        MotionProvider(_state_types, _params_motion),
         params_motion_(_params_motion),
         processing_step_(RUNNING_WITHOUT_KF),
         bootstrapping_(false),
@@ -198,13 +190,13 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         case FIRST_TIME_WITHOUT_KF :
         {
             // There is no KF: create own origin
-            setOrigin(getProblem()->stateZero(getStateStructure()), _incoming_ptr->getTimeStamp());
+            setOrigin(getProblem()->stateZero(getStateKeys()), _incoming_ptr->getTimeStamp());
             break;
         }
         case FIRST_TIME_WITH_KF_BEFORE_INCOMING :
         {
             // cannot join to the KF: create own origin
-            setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
+            setOrigin(getProblem()->getState(getStateKeys()), _incoming_ptr->getTimeStamp());
             break;
         }
         case FIRST_TIME_WITH_KF_ON_INCOMING :
@@ -216,7 +208,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         case FIRST_TIME_WITH_KF_AFTER_INCOMING :
         {
             // cannot join to the KF: create own origin
-            setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
+            setOrigin(getProblem()->getState(getStateKeys()), _incoming_ptr->getTimeStamp());
             break;
         }
         case RUNNING_WITHOUT_KF :
@@ -482,7 +474,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
 
         // create a new frame
         auto frame_new      = std::make_shared<FrameBase>(getTimeStamp(),
-                                                          getStateStructure(),
+                                                          getStateTypes(),
                                                           getProblem()->getState());
         // create a new capture
         auto capture_new    = emplaceCapture(frame_new,
@@ -516,9 +508,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
 
 }
 
-VectorComposite ProcessorMotion::getState(const StateStructure& _structure) const
+VectorComposite ProcessorMotion::getState(const StateKeys& _keys) const
 {
-    const StateStructure& structure = (_structure == "" ? state_structure_ : _structure);
+    const StateKeys& keys = (_keys == "" ? getStateKeys() : _keys);
 
 
     if (origin_ptr_ == nullptr or
@@ -538,7 +530,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
     // this may happen when in the very first frame where the capture has no motion info --> empty buffer
     if (last_ptr_->getBuffer().empty())
     {
-        return last_ptr_->getFrame()->getState(structure);
+        return last_ptr_->getFrame()->getState(keys);
     }
 
     /* Doing this:
@@ -555,7 +547,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
      */
 
     // Get state of origin
-    auto x_origin = getOrigin()->getFrame()->getState(state_structure_);
+    auto x_origin = getOrigin()->getFrame()->getState(getStateKeys());
 
     // Get most recent motion
     const auto& motion = last_ptr_->getBuffer().back();
@@ -591,7 +583,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
         statePlusDelta(x_origin, delta_preint, last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(), state);
     }
 
-    if (_structure == "")
+    if (_keys == "")
         return state;
 
     else
@@ -600,7 +592,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
         auto pair_key_vec_it = state.begin();
         while (pair_key_vec_it != state.end())
         {
-            if (_structure.find(pair_key_vec_it->first) == std::string::npos)
+            if (_keys.find(pair_key_vec_it->first) == std::string::npos)
                 pair_key_vec_it = state.erase(pair_key_vec_it);
 
             else
@@ -614,11 +606,11 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
 
 
 // _x needs to have the size of the processor state
-VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStructure& _structure) const
+VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys& _keys) const
 {
     assert(_ts.ok());
 
-    const StateStructure& structure = (_structure == "" ? state_structure_ : _structure);
+    const StateKeys& keys = (_keys == "" ? getStateKeys() : _keys);
 
     // We need to search for the capture containing a motion buffer with the queried time stamp
     auto capture_motion = findCaptureContainingTimeStamp(_ts);
@@ -636,7 +628,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
         // this may happen when in the very first frame where the capture has no motion info --> empty buffer
         if (capture_motion->getBuffer().empty())
         {
-            return capture_motion->getFrame()->getState(structure);
+            return capture_motion->getFrame()->getState(keys);
         }
 
         /* Doing this:
@@ -654,7 +646,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
 
         // Get state of origin
         auto cap_orig   = capture_motion->getOriginCapture();
-        auto x_origin = cap_orig->getFrame()->getState(state_structure_);
+        auto x_origin = cap_orig->getFrame()->getState(getStateKeys());
 
         // Get motion at time stamp
         const auto& motion = capture_motion->getBuffer().getMotion(_ts);
@@ -689,7 +681,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
             statePlusDelta(x_origin, delta_preint, _ts - cap_orig->getTimeStamp(), state);
         }
 
-        if (_structure == "")
+        if (_keys == "")
             return state;
 
         else
@@ -698,7 +690,7 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
             auto pair_key_vec_it = state.begin();
             while (pair_key_vec_it != state.end())
             {
-                if (_structure.find(pair_key_vec_it->first) == std::string::npos)
+                if (_keys.find(pair_key_vec_it->first) == std::string::npos)
                     pair_key_vec_it = state.erase(pair_key_vec_it);
 
                 else
@@ -715,7 +707,7 @@ FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const
 {
     FrameBasePtr key_frame_ptr = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
                                                                _ts_origin,
-                                                               getStateStructure(),
+                                                               getStateTypes(),
                                                                _x_origin);
     setOrigin(key_frame_ptr);
 
@@ -744,7 +736,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
     // ---------- LAST ----------
     // Make non-key-frame for last Capture
     last_frame_ptr_  = std::make_shared<FrameBase>(origin_ts,
-                                                   getStateStructure(),
+                                                   getStateTypes(),
                                                    _origin_frame->getState());
                                         
     // emplace (emtpy) last Capture
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index be37af7ec5938fecdeb54c24b71c368305ed0461..c6c0e34c4ae47a507a235d990810f52745a3db04 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -29,7 +29,10 @@ namespace wolf
 {
 
 ProcessorOdom2d::ProcessorOdom2d(ParamsProcessorOdom2dPtr _params) :
-                                 ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params),
+                                 ProcessorMotion("ProcessorOdom2d", 
+                                                 {{'P',"StatePoint2d"},{'O',"StateAngle"}}, 
+                                                 2, 3, 3, 3, 2, 0, 
+                                                 _params),
                                  params_odom_2d_(_params)
 {
     //
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index f6eb7c2d30a340d5235938ec5c1fbe000f96344a..26a2a8d19b8c1156443638263bc92c68c9220ff4 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -27,7 +27,10 @@ namespace wolf
 {
 
 ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params) :
-                        ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params),
+                        ProcessorMotion("ProcessorOdom3d",
+                                        {{'P',"StatePoint3d"},{'O',"StateQuaternion"}},
+                                        3, 7, 7, 6, 6, 0,
+                                        _params),
                         params_odom_3d_ (_params)
 {
     //
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 623066614b764486e4cf568e892d8fed95c90cbf..0892276de37c33ad0390f2a01b38e14400375f29 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -36,7 +36,7 @@ namespace wolf
 {
 
 ProcessorTracker::ProcessorTracker(const std::string& _type,
-                                   const StateStructure& _structure,
+                                   const StateKeys& _state_keys,
                                    int _dim,
                                    ParamsProcessorTrackerPtr _params_tracker) :
         ProcessorBase(_type, _dim, _params_tracker),
@@ -46,7 +46,7 @@ ProcessorTracker::ProcessorTracker(const std::string& _type,
         last_ptr_(nullptr),
         incoming_ptr_(nullptr),
         last_frame_ptr_(nullptr),
-        state_structure_(_structure)
+        state_keys_(_state_keys)
 {
     //
 }
@@ -104,10 +104,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_KEYFRAME" );
 
             // make a new KF at incoming
-            FrameBasePtr keyframe = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
-                                                                  incoming_ptr_->getTimeStamp(),
-                                                                  getProblem()->getFrameStructure(),
-                                                                  getProblem()->getState());
+            FrameBasePtr keyframe = getProblem()->emplaceFrame(incoming_ptr_->getTimeStamp());
 
             // link incoming to the new KF
             incoming_ptr_->link(keyframe);
@@ -151,10 +148,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_KEYFRAME" );
 
             // Make a NON KEY Frame to hold incoming capture
-            FrameBasePtr keyframe = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
-                                                                getProblem()->getFrameStructure(),
-                                                                getProblem()->getState());
-            incoming_ptr_->link(keyframe);
+            FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+                                                             getProblem()->getFrameTypes(),
+                                                             getProblem()->getState());
+            incoming_ptr_->link(frame);
 
             // Process info
             // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
@@ -170,7 +167,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             // Update pointers
             origin_ptr_ = last_ptr_;
             last_ptr_   = incoming_ptr_;
-            last_frame_ptr_ = keyframe;
+            last_frame_ptr_ = frame;
             incoming_ptr_ = nullptr;
 
             break;
@@ -192,8 +189,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
 
             // Create new NON KEY frame for incoming
             FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
-                                                           getProblem()->getFrameStructure(),
-                                                           getProblem()->getState());
+                                                             getProblem()->getFrameTypes(),
+                                                             getProblem()->getState());
             incoming_ptr_->link(frame);
 
             // Detect new Features, initialize Landmarks, ...
@@ -237,7 +234,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
 
                 // make NON KEY frame; append incoming to new frame
                 FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
-                                                                 getProblem()->getFrameStructure(),
+                                                                 getProblem()->getFrameTypes(),
                                                                  getProblem()->getState(incoming_ptr_->getTimeStamp()));
                 incoming_ptr_   ->link(frame);
 
@@ -256,7 +253,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
 
                 // Replace last frame for a new NON KEY frame in incoming
                 FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
-                                                                 getProblem()->getFrameStructure(),
+                                                                 getProblem()->getFrameTypes(),
                                                                  getProblem()->getState(incoming_ptr_->getTimeStamp()));
                 incoming_ptr_->link(frame);
                 last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index d55a2666cc00b3c8cdc53854ab43f7903f0b9d6e..3638807c5cfc769988aa54eac16e7f7048c4d668 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -32,7 +32,7 @@ namespace wolf
 {
 
 ProcessorTrackerFeature::ProcessorTrackerFeature(const std::string& _type,
-                                                 const StateStructure& _structure,
+                                                 const StateKeys& _structure,
                                                  int _dim,
                                                  ParamsProcessorTrackerFeaturePtr _params_tracker_feature) :
             ProcessorTracker(_type, _structure, _dim, _params_tracker_feature),
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index bab09ac8279b7c9ec770e985b953b29ed9cb82af..4bda28a91680b0f8b171c49876f5882da608b376 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -35,7 +35,7 @@ namespace wolf
 {
 
 ProcessorTrackerLandmark::ProcessorTrackerLandmark(const std::string& _type,
-                                                   const StateStructure& _structure,
+                                                   const StateKeys& _structure,
                                                    ParamsProcessorTrackerLandmarkPtr _params_tracker_landmark) :
     ProcessorTracker(_type,
                      _structure,
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 725b9f9036856ac463eafd59bb926b36ef98a23b..10503f2632b25e93a4cb2137417c32d79807f8a5 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -52,12 +52,12 @@ SensorBase::SensorBase(const std::string& _type,
         auto prior = state_pair.second;
 
         // type
-        if (key == 'P' and prior.getType() != "P" and prior.getType() != "StatePoint2d" and prior.getType() != "StatePoint3d")
-            throw std::runtime_error("Prior type for key P has to be 'P', 'StatePoint2d' or 'StatePoint3d'");
-        if (key == 'V' and prior.getType() != "V" and prior.getType() != "StateVector2d" and prior.getType() != "StateVector3d")
-            throw std::runtime_error("Prior type for key V has to be 'V' 'StateVector2d' or 'StateVector3d'");
-        if (key == 'O' and prior.getType() != "O" and prior.getType() != "StateAngle" and prior.getType() != "StateQuaternion")
-            throw std::runtime_error("Prior type for key O in 3D has to be 'O', 'StateAngle' or 'StateQuaternion'");
+        if (key == 'P' and prior.getType() != "StatePoint2d" and prior.getType() != "StatePoint3d")
+            throw std::runtime_error("Prior type for key P has to be 'StatePoint2d' or 'StatePoint3d'");
+        if (key == 'V' and prior.getType() != "StateVector2d" and prior.getType() != "StateVector3d")
+            throw std::runtime_error("Prior type for key V has to be 'StateVector2d' or 'StateVector3d'");
+        if (key == 'O' and prior.getType() != "StateAngle" and prior.getType() != "StateQuaternion")
+            throw std::runtime_error("Prior type for key O in 3D has to be 'StateAngle' or 'StateQuaternion'");
         
         // Create state block
         auto sb = FactoryStateBlock::create(prior.getType(), prior.getState(), prior.isFixed());
@@ -73,10 +73,6 @@ SensorBase::SensorBase(const std::string& _type,
         if (prior.isDynamic())
             setDriftStd(key, prior.getDriftStd());
     }
-
-    WOLF_INFO(_priors);
-    auto base_prior = _priors.cast<SpecComposite>();
-    WOLF_INFO(base_prior);
 }
 
 SensorBase::~SensorBase()
@@ -107,7 +103,7 @@ void SensorBase::unfixExtrinsics()
 
 void SensorBase::fixIntrinsics()
 {
-    for (auto key : getStructure())
+    for (auto key : getKeys())
     {
         if (key != 'P' and key != 'O') // exclude extrinsics
         {
@@ -120,7 +116,7 @@ void SensorBase::fixIntrinsics()
 
 void SensorBase::unfixIntrinsics()
 {
-    for (auto key : getStructure())
+    for (auto key : getKeys())
     {
         if (key != 'P' and key != 'O') // exclude extrinsics
         {
@@ -549,7 +545,7 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
 {
     if (_metric && _state_blocks)
     {
-        for (auto key : getStructure())
+        for (auto key : getKeys())
         {
             auto sb = getStateBlockDynamic(key);
             if (sb)
@@ -560,12 +556,12 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
     else if (_metric)
     {
         _stream << _tabs << "  " << (isFixed() ? "Fix" : "Est") << ",\t x = ( " << std::setprecision(3)
-                << getStateVector(getStructure()).transpose() << " )" << std::endl;
+                << getStateVector(getKeys()).transpose() << " )" << std::endl;
     }
     else if (_state_blocks)
     {
         _stream << _tabs << "  " << "sb:";
-        for (auto key : getStructure())
+        for (auto key : getKeys())
         {
             auto sb = getStateBlockDynamic(key);
             if (sb)
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 717fc720e38d23f66f3f0d19ffc170a0cad4b149..84668cfff6d8ba7f9996255bef11d82d3bc0ad8c 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -57,7 +57,7 @@ HasStateBlocks::HasStateBlocks(const SpecComposite& _specs)
 
 HasStateBlocks::HasStateBlocks(const TypeComposite& _types, const VectorComposite& _vectors)
 {
-    if (not _types.has(_vectors.getStructure()) or not _vectors.has(_types.getStructure()))
+    if (not _types.has(_vectors.getKeys()) or not _vectors.has(_types.getKeys()))
     {
         throw std::runtime_error("HasStateBlocks::HasStateBlocks: provided type and vector composites don't have the same structure");
     }
@@ -121,7 +121,7 @@ void HasStateBlocks::registerNewStateBlocks(ProblemPtr _problem)
 
 void HasStateBlocks::removeStateBlocks(ProblemPtr _problem)
 {
-    for (const char key : getStructure()) // note: key is a char
+    for (const char key : getKeys()) // note: key is a char
     {
         auto sbp = getStateBlock(key);
         if (sbp != nullptr)
@@ -156,7 +156,7 @@ void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream&
 {
     if (_metric && _state_blocks)
     {
-        for (auto key : getStructure())
+        for (auto key : getKeys())
         {
             auto sb = getStateBlock(key);
             if (sb)
@@ -169,13 +169,13 @@ void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream&
     else if (_metric)
     {
         _stream << _tabs << "  " << (isFixed() ? "Fix" : "Est")
-                << ",\t x = ( " << std::setprecision(3) << getStateVector(getStructure()).transpose() << " )"
+                << ",\t x = ( " << std::setprecision(3) << getStateVector(getKeys()).transpose() << " )"
                 << std::endl;
     }
     else if (_state_blocks)
     {
         _stream << _tabs << "  " << "sb:";
-        for (auto key : getStructure())
+        for (auto key : getKeys())
         {
             auto sb = getStateBlock(key);
             if (sb)
diff --git a/src/state_block/spec_composite.cpp b/src/state_block/spec_composite.cpp
index f6a43c2c81d4fea91168f20c769b7033da6dd360..00234e6c75f5cf26eec05e033402a81637d6a639 100644
--- a/src/state_block/spec_composite.cpp
+++ b/src/state_block/spec_composite.cpp
@@ -34,7 +34,7 @@ SpecState::SpecState(const std::string&     _type,
                      const Eigen::VectorXd& _noise_std)
     : type_(_type), state_(_state), mode_(_mode), noise_std_(_noise_std)
 {
-    check();
+    SpecState::check();
 }
 
 SpecState::SpecState(const YAML::Node& _n)
@@ -47,7 +47,7 @@ SpecState::SpecState(const YAML::Node& _n)
     else 
       noise_std_ = Eigen::VectorXd(0);
 
-    check();
+    SpecState::check();
 }
 
 void SpecState::check() const
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index c061ea96beff00f3ac0a7c102c06f7e57dd4e349..4eb72cd5415d27e78ad677eec9210aedb194758f 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -77,21 +77,4 @@ namespace wolf{
 WOLF_REGISTER_STATEBLOCK(StateQuaternion);
 WOLF_REGISTER_STATEBLOCK(StateAngle);
 WOLF_REGISTER_STATEBLOCK(StateHomogeneous3d);
-WOLF_REGISTER_STATEBLOCK_WITH_KEY(H, StateHomogeneous3d);
-
-StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
-{
-    if (_state.size() == 1)
-        return StateAngle::create(_state, _fixed);
-    if (_state.size() == 4)
-        return StateQuaternion::create(_state, _fixed);
-
-    throw std::runtime_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D.");
-
-    return nullptr;
-}
-
-//WOLF_REGISTER_STATEBLOCK_WITH_KEY(O, wolf::create_orientation);
-namespace{ const bool __attribute__((used)) create_orientationRegisteredWithO =                   \
-     FactoryStateBlock::registerCreator("O", wolf::create_orientation); }
 }
\ No newline at end of file
diff --git a/src/state_block/state_block_derived.cpp b/src/state_block/state_block_derived.cpp
index e1eca5d666cc70fbd1ed3074dbc981bddbcdd872..0b12644d84ab0320ee6ef6d7949e2449c9991c97 100644
--- a/src/state_block/state_block_derived.cpp
+++ b/src/state_block/state_block_derived.cpp
@@ -25,69 +25,6 @@
 
 namespace wolf
 {
-StateBlockPtr StatePoint2d::create(const Eigen::VectorXd& _state, bool _fixed)
-{
-    if (_state.size() == 2) return std::make_shared<StatePoint2d>(_state, _fixed);
-
-    throw std::runtime_error("Wrong vector size for Point2d.");
-}
-
-StateBlockPtr StatePoint3d::create(const Eigen::VectorXd& _state, bool _fixed)
-{
-    if (_state.size() == 3) return std::make_shared<StatePoint3d>(_state, _fixed);
-
-    throw std::runtime_error("Wrong vector size for Point3d.");
-}
-
-StateBlockPtr StateVector2d::create(const Eigen::VectorXd& _state, bool _fixed)
-{
-    if (_state.size() == 2) return std::make_shared<StateVector2d>(_state, _fixed);
-
-    throw std::runtime_error("Wrong vector size for Vector2d.");
-}
-
-StateBlockPtr StateVector3d::create(const Eigen::VectorXd& _state, bool _fixed)
-{
-    if (_state.size() == 3) return std::make_shared<StateVector3d>(_state, _fixed);
-
-    throw std::runtime_error("Wrong vector size for Vector3d.");
-}
-
-StateBlockPtr create_point(const Eigen::VectorXd& _state, bool _fixed)
-{
-    if (_state.size() == 2)
-        return std::make_shared<StatePoint2d>(_state, _fixed);
-    else if (_state.size() == 3)
-        return std::make_shared<StatePoint3d>(_state, _fixed);
-
-    throw std::runtime_error("Wrong vector size for Point.");
-}
-
-StateBlockPtr create_vector(const Eigen::VectorXd& _state, bool _fixed)
-{
-    if (_state.size() == 2)
-        return std::make_shared<StateVector2d>(_state, _fixed);
-    else if (_state.size() == 3)
-        return std::make_shared<StateVector3d>(_state, _fixed);
-
-    throw std::runtime_error("Wrong vector size for Vector.");
-}
-
-template <size_t size>
-StateBlockPtr StateParams<size>::create(const Eigen::VectorXd& _state, bool _fixed)
-{
-    if (_state.size() == size) return std::make_shared<StateParams<size>>(_state, _fixed);
-
-    throw std::runtime_error("Wrong vector size for Params.");
-}
-
-namespace
-{
-const bool __attribute__((used)) create_pointRegisteredWithP =
-    FactoryStateBlock::registerCreator("P", wolf::create_point);
-const bool __attribute__((used)) create_vectorRegisteredWithP =
-    FactoryStateBlock::registerCreator("V", wolf::create_vector);
-}  // namespace
 
 WOLF_REGISTER_STATEBLOCK(StatePoint2d);
 WOLF_REGISTER_STATEBLOCK(StateVector2d);
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index a84a74f090fb125f975d29bcf0560e25c622216b..4328fe42a1719744bef044586ad88f47a5573948 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -317,7 +317,7 @@ MatrixComposite operator * (double scalar_left, const MatrixComposite& M)
     return S;
 }
 
-MatrixXd MatrixComposite::matrix(const StateStructure &_struct_rows, const StateStructure &_struct_cols) const
+MatrixXd MatrixComposite::matrix(const StateKeys &_struct_rows, const StateKeys &_struct_cols) const
 {
 
     std::unordered_map < char, unsigned int> indices_rows;
@@ -367,8 +367,8 @@ unsigned int MatrixComposite::cols() const
     return cols;
 }
 
-void MatrixComposite::sizeAndIndices(const StateStructure &_struct_rows,
-                                     const StateStructure &_struct_cols,
+void MatrixComposite::sizeAndIndices(const StateKeys &_struct_rows,
+                                     const StateKeys &_struct_cols,
                                      std::unordered_map<char, unsigned int> &_indices_rows,
                                      std::unordered_map<char, unsigned int> &_indices_cols,
                                      unsigned int &_rows,
@@ -388,16 +388,16 @@ void MatrixComposite::sizeAndIndices(const StateStructure &_struct_rows,
     }
 }
 
-MatrixComposite::MatrixComposite (const StateStructure& _row_structure, const StateStructure& _col_structure)
+MatrixComposite::MatrixComposite (const StateKeys& _row_structure, const StateKeys& _col_structure)
 {
     for (const auto& rkey : _row_structure)
         for (const auto& ckey : _col_structure)
             this->emplace(rkey, ckey, MatrixXd(0,0));
 }
 
-MatrixComposite::MatrixComposite (const StateStructure& _row_structure,
+MatrixComposite::MatrixComposite (const StateKeys& _row_structure,
                                   const std::list<int>& _row_sizes,
-                                  const StateStructure& _col_structure,
+                                  const StateKeys& _col_structure,
                                   const std::list<int>& _col_sizes)
 {
     assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
@@ -418,9 +418,9 @@ MatrixComposite::MatrixComposite (const StateStructure& _row_structure,
 }
 
 MatrixComposite::MatrixComposite (const MatrixXd& _m,
-                                  const StateStructure& _row_structure,
+                                  const StateKeys& _row_structure,
                                   const std::list<int>& _row_sizes,
-                                  const StateStructure& _col_structure,
+                                  const StateKeys& _col_structure,
                                   const std::list<int>& _col_sizes)
 {
     assert (_row_structure.size() == _row_sizes.size() && "Row structure and sizes have different number of elements!");
@@ -454,8 +454,8 @@ MatrixComposite::MatrixComposite (const MatrixXd& _m,
     assert(_m.rows() == rindex && "Provided matrix has too many rows");
 }
 
-MatrixComposite MatrixComposite::zero (const StateStructure& _row_structure, const std::list<int>& _row_sizes,
-                                       const StateStructure& _col_structure, const std::list<int>& _col_sizes)
+MatrixComposite MatrixComposite::zero (const StateKeys& _row_structure, const std::list<int>& _row_sizes,
+                                       const StateKeys& _col_structure, const std::list<int>& _col_sizes)
 {
     MatrixComposite m;
 
@@ -477,7 +477,7 @@ MatrixComposite MatrixComposite::zero (const StateStructure& _row_structure, con
     return m;
 }
 
-MatrixComposite MatrixComposite::identity (const StateStructure& _structure, const std::list<int>& _sizes)
+MatrixComposite MatrixComposite::identity (const StateKeys& _structure, const std::list<int>& _sizes)
 {
     MatrixComposite m;
 
@@ -655,7 +655,7 @@ void StateBlockComposite::set(const char& _key, const VectorXd &_vec)
     it->second->setState(_vec);
 }
 
-void StateBlockComposite::setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors)
+void StateBlockComposite::setVectors(const StateKeys& _structure, const std::list<VectorXd> &_vectors)
 {
     assert(_structure.size() == _vectors.size() && "Sizes of structure and vector list do not match");
 
@@ -801,7 +801,7 @@ SizeEigen StateBlockComposite::stateSize() const
     return size;
 }
 
-SizeEigen StateBlockComposite::stateSize(const StateStructure &_structure) const
+SizeEigen StateBlockComposite::stateSize(const StateKeys &_structure) const
 {
     SizeEigen size = 0;
     for (const auto& key : _structure)
@@ -811,7 +811,7 @@ SizeEigen StateBlockComposite::stateSize(const StateStructure &_structure) const
     return size;
 }
 
-VectorXd StateBlockComposite::stateVector(const StateStructure &_structure) const
+VectorXd StateBlockComposite::stateVector(const StateKeys &_structure) const
 {
     VectorXd x(stateSize(_structure));
     SizeEigen index = 0;
@@ -826,7 +826,7 @@ VectorXd StateBlockComposite::stateVector(const StateStructure &_structure) cons
     return x;
 }
 
-void StateBlockComposite::stateVector(const StateStructure &_structure, VectorXd &_vector) const
+void StateBlockComposite::stateVector(const StateKeys &_structure, VectorXd &_vector) const
 {
     _vector.resize(stateSize(_structure));
     SizeEigen index = 0;
diff --git a/src/state_block/vector_composite.cpp b/src/state_block/vector_composite.cpp
index f01efc485e1991bb7f874695e1a7c3e6d8bfb55e..1de0778e2155347244597c728ba0be5a2c0d7ae3 100644
--- a/src/state_block/vector_composite.cpp
+++ b/src/state_block/vector_composite.cpp
@@ -24,49 +24,41 @@
 
 namespace wolf{
 
-VectorComposite::VectorComposite(const StateStructure& _structure, const std::list<int>& _sizes)
+VectorComposite::VectorComposite() :
+    Composite<Eigen::VectorXd>()
 {
-    auto size_it = _sizes.begin();
-    for ( const auto& key : _structure)
-    {
-        const auto& size    = *size_it;
-
-        this->emplace(key, VectorXd(size).setZero());
-
-        size_it ++;
-    }
 }
 
-VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
+VectorComposite::VectorComposite(const StateKeys& _keys, const VectorXd& _v, const std::list<int>& _sizes)
 {
+    assert(_keys.size() == _sizes.size() && "Keys and _sizes list size mismatch");
+
     int index = 0;
     auto size_it = _sizes.begin();
-    for ( const auto& key : _structure)
+    for ( const auto& key : _keys)
     {
         const auto& size    = *size_it;
 
+        if (_v.size() < index + size)
+            throw std::runtime_error("VectorComposite: provided vector _v is too short");
+
         (*this)[key]        = _v.segment(index,size);
 
         index += size;
         size_it ++;
     }
-}
 
-VectorComposite::VectorComposite (const StateStructure& _s)
-{
-    for (const auto& key : _s)
-    {
-        this->emplace(key,VectorXd(0));
-    }
+    if (_v.size() != index)
+        throw std::runtime_error("VectorComposite: provided vector _v is too long");
 }
 
-VectorComposite::VectorComposite (const StateStructure& _structure, const std::list<VectorXd>& _vectors)
+VectorComposite::VectorComposite (const StateKeys& _keys, const std::list<VectorXd>& _vectors)
 {
-    assert(_structure.size() == _vectors.size() && "Structure and vector list size mismatch");
+    assert(_keys.size() == _vectors.size() && "Keys and vector list size mismatch");
 
     auto vector_it = _vectors.begin();
 
-    for (const auto& key : _structure)
+    for (const auto& key : _keys)
     {
         this->emplace(key, *vector_it);
         vector_it++;
@@ -74,12 +66,12 @@ VectorComposite::VectorComposite (const StateStructure& _structure, const std::l
 }
 
 
-Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
+Eigen::VectorXd VectorComposite::vector(const StateKeys &_keys) const
 {
     // traverse once with unordered_map access
     std::vector<const VectorXd*> vp;
     unsigned int size = 0;
-    for (const auto& key : _structure)
+    for (const auto& key : _keys)
     {
         vp.push_back(&(this->at(key)));
         size +=  vp.back()->size();
@@ -99,55 +91,18 @@ Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
 
 std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x)
 {
-    for (const auto &pair_key_vec : _x)
+    for (auto&& pair : _x)
     {
-        const auto &key = pair_key_vec.first;
-        const auto &vec = pair_key_vec.second;
-        _os << key << ": (" << vec.transpose() << ");  ";
+        _os << pair.first << ": (" << pair.second.transpose() << ");  ";
     }
     return _os;
 }
 
-// wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y)
-// {
-//     wolf::VectorComposite xpy;
-//     for (const auto& pair_i_xi : _x)
-//     {
-//         const auto& i = pair_i_xi.first;
-
-//         xpy.emplace(i, _x.at(i) + _y.at(i));
-//     }
-//     return xpy;
-// }
-
-// VectorComposite operator -(const VectorComposite &_x, const VectorComposite &_y)
-// {
-//     VectorComposite xpy;
-//     for (const auto& pair_i_xi : _x)
-//     {
-//         const auto& i = pair_i_xi.first;
-
-//         xpy.emplace(i, _x.at(i) - _y.at(i));
-//     }
-//     return xpy;
-// }
-
-// VectorComposite operator -(const wolf::VectorComposite &_x)
-// {
-//     wolf::VectorComposite m;
-//     for (const auto& pair_i_xi : _x)
-//     {
-//         const auto& i = pair_i_xi.first;
-//         m.emplace(i, - _x.at(i));
-//     }
-//     return m;
-// }
-
-void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
+void VectorComposite::set (const VectorXd& _v, const StateKeys& _keys, const std::list<int>& _sizes)
 {
     int index = 0;
     auto size_it = _sizes.begin();
-    for ( const auto& key : _structure)
+    for ( const auto& key : _keys)
     {
         const auto& size    = *size_it;
 
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 0e96bf4bb0d8442a9614e690d9d586883bb97ab8..f37dc90125c15da89c96d339347b9ae5f9732b37 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -32,19 +32,18 @@ wolf_add_gtest(gtest_example gtest_example.cpp)           #
 
 # BufferFrame
 wolf_add_gtest(gtest_buffer_frame gtest_buffer_frame.cpp)
-target_link_libraries(gtest_buffer_frame PUBLIC dummy)
 
 # CaptureBase class test
 wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
 
-# Converter from String to various types used by the parameters server
-wolf_add_gtest(gtest_converter gtest_converter.cpp)
+# # Converter from String to various types used by the parameters server
+# wolf_add_gtest(gtest_converter gtest_converter.cpp)
 
 # FactorBase class test
 wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
 
 # FactorAutodiff class test
-# wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
+wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
 
 # Factory test
 wolf_add_gtest(gtest_factory gtest_factory.cpp)
@@ -52,11 +51,10 @@ target_link_libraries(gtest_factory PUBLIC dummy)
 
 # FactoryStateBlock factory test
 wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
-target_link_libraries(gtest_factory_state_block PUBLIC dummy)
 
 # Node Emplace test
-# wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
-# target_link_libraries(gtest_emplace PUBLIC dummy)
+wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
+target_link_libraries(gtest_emplace PUBLIC dummy)
 
 # FeatureBase classes test
 wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
@@ -71,7 +69,7 @@ wolf_add_gtest(gtest_graph_search gtest_graph_search.cpp)
 wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
 
 # IsMotion classes test
-# wolf_add_gtest(gtest_motion_provider gtest_motion_provider.cpp)
+wolf_add_gtest(gtest_motion_provider gtest_motion_provider.cpp)
 
 # LocalParametrizationXxx classes test
 wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
@@ -82,11 +80,11 @@ wolf_add_gtest(gtest_logging gtest_logging.cpp)
 # MotionBuffer class test
 wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
 
-# Parameters server
-# wolf_add_gtest(gtest_param_server gtest_param_server.cpp)
+# # Parameters server
+# # wolf_add_gtest(gtest_param_server gtest_param_server.cpp)
 
-# YAML parser
-# wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
+# # YAML parser
+# # wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
 
 # Problem class test
 wolf_add_gtest(gtest_problem gtest_problem.cpp)
@@ -108,35 +106,35 @@ wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
 # SE2 test
 wolf_add_gtest(gtest_SE2 gtest_SE2.cpp)
 
-# SensorBase test
-wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
+# # SensorBase test
+# wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
 
-# shared_from_this test
-wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp)
+# # shared_from_this test
+# wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp)
 
-# SolverManager test
-wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
+# # SolverManager test
+# wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp)
 
-# SolverManagerMultithread test
-wolf_add_gtest(gtest_solver_manager_multithread gtest_solver_manager_multithread.cpp)
+# # SolverManagerMultithread test
+# wolf_add_gtest(gtest_solver_manager_multithread gtest_solver_manager_multithread.cpp)
 
-# StateBlock class test
-wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
+# # StateBlock class test
+# wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
 
-# StateBlock class test
-wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp)
+# # StateBlock class test
+# wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp)
 
-# TimeStamp class test
-wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
+# # TimeStamp class test
+# wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
 
-# TrackMatrix class test
-wolf_add_gtest(gtest_track_matrix gtest_track_matrix.cpp)
+# # TrackMatrix class test
+# wolf_add_gtest(gtest_track_matrix gtest_track_matrix.cpp)
 
-# TrajectoryBase class test
-wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp)
+# # TrajectoryBase class test
+# wolf_add_gtest(gtest_trajectory gtest_trajectory.cpp)
 
-# TreeManager class test
-wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp)
+# # TreeManager class test
+# wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp)
 
 # ------- Now Derived classes ----------
 
diff --git a/test/dummy/factory_dummy_object.h b/test/dummy/factory_dummy_object.h
index 145ced54972a3b16cc042f519995a284df1d5e42..9b3705ab10dd98be39be717d15f5fd72f9bc2aa4 100644
--- a/test/dummy/factory_dummy_object.h
+++ b/test/dummy/factory_dummy_object.h
@@ -28,7 +28,7 @@ namespace wolf
 {
 
 class DummyObject;
-typedef Factory<DummyObject,
+typedef Factory<std::shared_ptr<DummyObject>,
                 const std::string&> FactoryDummyObject;
 template<>
 inline std::string FactoryDummyObject::getClass() const
diff --git a/test/dummy/processor_motion_provider_dummy.h b/test/dummy/processor_motion_provider_dummy.h
index a7e64038e15c0a22130ba1f6fbddb22c08d02373..88f1465748908c2077ed6f52590c3b9c97eafc28 100644
--- a/test/dummy/processor_motion_provider_dummy.h
+++ b/test/dummy/processor_motion_provider_dummy.h
@@ -31,7 +31,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProviderDummy);
 
 struct ParamsMotionProviderDummy : public ParamsProcessorBase, public ParamsMotionProvider
 {
-        std::string state_structure = "PO";
+        TypeComposite state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"}};
 
         ParamsMotionProviderDummy() = default;
         ParamsMotionProviderDummy(const YAML::Node& _n):
@@ -49,7 +49,7 @@ class MotionProviderDummy : public ProcessorBase, public MotionProvider
     public:
         MotionProviderDummy(ParamsMotionProviderDummyPtr _params) :
             ProcessorBase("MotionProviderDummy", 2, _params),
-            MotionProvider(_params->state_structure, _params)
+            MotionProvider(_params->state_types, _params)
         {}
         ~MotionProviderDummy(){};
 
@@ -66,12 +66,12 @@ class MotionProviderDummy : public ProcessorBase, public MotionProvider
         bool voteForKeyFrame() const override { return false; };
         TimeStamp getTimeStamp() const override {return TimeStamp(0);};
 
-        VectorComposite getState(const StateStructure& _structure = "") const override
+        VectorComposite getState(const StateKeys& _structure = "") const override
         {
             return getOdometry();
         };
 
-        VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override
+        VectorComposite getState(const TimeStamp& _ts, const StateKeys& _structure = "") const override
         {
             return getOdometry();
         };
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index 5d9df91f1f67018ef774c3f3ee1eea2833f34a0f..dd1d7b0c1958c254085119a763bc2fc1bf400640 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -19,19 +19,10 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-/**
- * \file gtest_SE3.cpp
- *
- *  Created on: Feb 2, 2019
- *      \author: jsola
- */
-
 
 #include "core/math/SE3.h"
 #include "core/utils/utils_gtest.h"
 
-
-
 using namespace Eigen;
 using namespace wolf;
 using namespace SE3;
@@ -142,7 +133,7 @@ TEST(SE3, inverseComposite)
     Vector3d pi_true = -(q.conjugate() * p);
     Quaterniond qi_true = q.conjugate();
 
-    VectorComposite pose_vc_out("PO", {3, 4});
+    VectorComposite pose_vc_out("PO", Vector7d::Zero(), {3, 4});
     inverse(pose_vc, pose_vc_out);
     ASSERT_MATRIX_APPROX(pose_vc_out.at('P'), pi_true, 1e-8);
     ASSERT_MATRIX_APPROX(pose_vc_out.at('O'), qi_true.coeffs(), 1e-8);
@@ -215,7 +206,7 @@ TEST(SE3, composeVectorComposite)
 
     compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
 
-    VectorComposite x1, x2, xc("PO", {3,4});
+    VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3,4});
 
     x1.emplace('P', p1);
     x1.emplace('O', q1.coeffs());
@@ -246,7 +237,7 @@ TEST(SE3, composeVectorComposite_Jacobians)
 
     compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
 
-    VectorComposite x1, x2, xc("PO", {3,4});
+    VectorComposite x1, x2, xc("PO", Vector7d::Zero(), {3,4});
     MatrixComposite J_xc_x1, J_xc_x2;
 
     x1.emplace('P', p1);
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index c9f34bbf031ec8b09e5d50ea3ab0fcc7f6c370bb..2a2a4a55443fc7cc40fbb85aaf94a40b75104eb6 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -30,6 +30,7 @@
 
 #include "core/state_block/state_block_derived.h"
 #include "core/state_block/state_angle.h"
+#include "core/state_block/state_quaternion.h"
 #include "core/problem/problem.h"
 #include "core/sensor/sensor_base.h"
 #include "core/sensor/sensor_odom.h"
@@ -58,12 +59,10 @@ TEST(Emplace, Sensor)
 {
     ProblemPtr P = Problem::create("POV", 3);
 
-    auto sen = SensorBase::emplace<SensorDummy>(P->getHardware(), // SensorBase is abstract
-                                                "dummy_name", 
-                                                2, 
-                                                std::make_shared<ParamsSensorDummy>(), 
-                                                SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())},
-                                                                {'O',SpecStateSensor("O",Vector1d::Zero())}}));
+    auto sen = SensorBase::emplace<SensorDummy<3>>(P->getHardware(), // SensorBase is abstract
+                                                   std::make_shared<ParamsSensorDummy>(), 
+                                                   SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())},
+                                                                        {'O',SpecStateSensor("StateQuaternion",Vector4d::Random().normalized())}}));
                                                         
     ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getHardware()->getProblem());
 }
@@ -72,7 +71,10 @@ TEST(Emplace, Frame)
     ProblemPtr P = Problem::create("POV", 3);
 
     ASSERT_NE(P->getTrajectory(), nullptr);
-    FrameBase::emplace<FrameBase>(P->getTrajectory(),  TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
+    FrameBase::emplace<FrameBase>(P->getTrajectory(),  
+                                  TimeStamp(0),
+                                  std::make_shared<StatePoint3d>(Vector3d::Zero(),true), 
+                                  std::make_shared<StateQuaternion>(Vector4d::Random().normalized(),true));
     ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
 }
 
@@ -80,24 +82,29 @@ TEST(Emplace, Processor)
 {
     ProblemPtr P = Problem::create("PO", 2);
 
-    auto sen = SensorBase::emplace<SensorDummy>(P->getHardware(), // SensorBase is abstract
-                                                "dummy_name", 
-                                                2, 
-                                                std::make_shared<ParamsSensorDummy>(), 
-                                                SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())},
-                                                        {'O',SpecStateSensor("O",Vector1d::Zero())}}));
+    auto sen = SensorBase::emplace<SensorDummy<2>>(P->getHardware(), // SensorBase is abstract
+                                                   std::make_shared<ParamsSensorDummy>(), 
+                                                   SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())},
+                                                                        {'O',SpecStateSensor("StateAngle",Vector1d::Zero())}}));
+    WOLF_INFO("0");
     auto prc = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen, std::make_shared<ParamsProcessorOdom2d>());
+    WOLF_INFO("1");
     ASSERT_EQ(P, P->getHardware()->getSensorList().front()->getProcessorList().front()->getSensor()->getProblem());
+    WOLF_INFO("2");
     ASSERT_EQ(sen, sen->getProcessorList().front()->getSensor());
     ASSERT_EQ(prc, sen->getProcessorList().front());
 
-    SensorBasePtr sen2 = SensorBase::emplace<SensorDummy>(P->getHardware(), // SensorBase is abstract
-                                                          "dummy_name", 
-                                                          2, 
-                                                          std::make_shared<ParamsSensorDummy>(), 
-                                                          SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())},
-                                                                  {'O',SpecStateSensor("O",Vector1d::Zero())}}));
+    WOLF_INFO("1");
+
+    SensorBasePtr sen2 = SensorBase::emplace<SensorDummy<2>>(P->getHardware(), // SensorBase is abstract
+                                                             std::make_shared<ParamsSensorDummy>(), 
+                                                             SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())},
+                                                                                  {'O',SpecStateSensor("StateAngle",Vector1d::Zero())}}));
+
+    WOLF_INFO("2");
     ProcessorOdom2dPtr prc2 = ProcessorOdom2d::emplace<ProcessorOdom2d>(sen2, std::make_shared<ParamsProcessorOdom2d>());
+
+    WOLF_INFO("3");
     ASSERT_EQ(sen2, sen2->getProcessorList().front()->getSensor());
     ASSERT_EQ(prc2, sen2->getProcessorList().front());
 }
@@ -107,7 +114,10 @@ TEST(Emplace, Capture)
     ProblemPtr P = Problem::create("POV", 3);
 
     ASSERT_NE(P->getTrajectory(), nullptr);
-    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(0,true));
+    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),
+                                             TimeStamp(0),
+                                             std::make_shared<StatePoint3d>(Vector3d::Zero(),true), 
+                                             std::make_shared<StateQuaternion>(Vector4d::Random().normalized(),true));
     ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
 
     auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -121,7 +131,10 @@ TEST(Emplace, Feature)
     ProblemPtr P = Problem::create("POV", 3);
 
     ASSERT_NE(P->getTrajectory(), nullptr);
-    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
+    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),
+                                             TimeStamp(0),
+                                             std::make_shared<StatePoint3d>(Vector3d::Zero(),true), 
+                                             std::make_shared<StateQuaternion>(Vector4d::Random().normalized(),true));
     ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
 
     auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -139,7 +152,11 @@ TEST(Emplace, Factor)
     ProblemPtr P = Problem::create("POV", 3);
 
     ASSERT_NE(P->getTrajectory(), nullptr);
-    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
+
+    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),
+                                             TimeStamp(0),
+                                             std::make_shared<StatePoint3d>(Vector3d::Zero(),true), 
+                                             std::make_shared<StateQuaternion>(Vector4d::Random().normalized(),true));
     ASSERT_EQ(P, P->getTrajectory()->getFirstFrame()->getTrajectory()->getProblem());
 
     auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
@@ -157,15 +174,13 @@ TEST(Emplace, Factor)
 
 TEST(Emplace, EmplaceDerived)
 {
-    ProblemPtr P = Problem::create("POV", 3);
+    ProblemPtr P = Problem::create("PO", 2);
 
     auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
-    auto sen = SensorBase::emplace<SensorOdom>(P->getHardware(),
-                                               "dummy_name", 
-                                               2, 
-                                               std::make_shared<ParamsSensorOdom>(), 
-                                               SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())},
-                                                       {'O',SpecStateSensor("O",Vector1d::Zero())}}));
+    auto sen = SensorBase::emplace<SensorOdom2d>(P->getHardware(),
+                                                 std::make_shared<ParamsSensorOdom>(), 
+                                                 SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())},
+                                                                      {'O',SpecStateSensor("StateAngle",Vector1d::Zero())}}));
     auto cov = Eigen::MatrixXd::Identity(2,2);
     auto cpt = CaptureBase::emplace<CaptureOdom2d>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
     auto m = Eigen::Matrix<double,9,6>();
@@ -186,7 +201,10 @@ TEST(Emplace, ReturnDerived)
     ProblemPtr P = Problem::create("POV", 3);
 
     ASSERT_NE(P->getTrajectory(), nullptr);
-    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  TimeStamp(0), std::make_shared<StatePoint2d>(Vector2d::Zero(),true), std::make_shared<StateAngle>(2,true));
+    auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),
+                                             TimeStamp(0),
+                                             std::make_shared<StatePoint3d>(Vector3d::Zero(),true), 
+                                             std::make_shared<StateQuaternion>(Vector4d::Random().normalized(),true));
     auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
     auto cov = Eigen::MatrixXd::Identity(2,2);
     auto ftr = FeatureBase::emplace<FeatureOdom2d>(cpt, Eigen::VectorXd(2), cov);
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 718cf14ba4731b4b754de43812f0d785cfc72efa..84cddc0e0d66078976fd71a119375018d98ee77c 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -381,7 +381,7 @@ TEST(FactorAutodiff, EmplaceOdom2d)
     FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2));
 
     // SENSOR
-    auto sensor_ptr = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+    auto sensor_ptr = FactorySensorYaml::create("SensorOdom2d", "SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
 
     // CAPTURE
     auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
@@ -407,11 +407,11 @@ TEST(FactorAutodiff, ResidualOdom2d)
     f1_pose << 2,1,M_PI;
     f2_pose << 3,5,3*M_PI/2;
 
-    FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose);
-    FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
+    FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), "PO", f1_pose);
+    FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), "PO", f2_pose);
 
     // SENSOR
-    auto sensor_ptr = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+    auto sensor_ptr = FactorySensorYaml::create("SensorOdom2d", "SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
 
     // CAPTURE
     auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
@@ -449,11 +449,11 @@ TEST(FactorAutodiff, JacobianOdom2d)
     f1_pose << 2,1,M_PI;
     f2_pose << 3,5,3*M_PI/2;
 
-    FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose);
-    FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
+    FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), "PO", f1_pose);
+    FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), "PO", f2_pose);
 
     // SENSOR
-    auto sensor_ptr = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+    auto sensor_ptr = FactorySensorYaml::create("SensorOdom2d", "SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
 
     // CAPTURE
     auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
@@ -526,11 +526,11 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
     f1_pose << 2,1,M_PI;
     f2_pose << 3,5,3*M_PI/2;
 
-    FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), f1_pose);
-    FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), f2_pose);
+    FrameBasePtr fr1_ptr = problem->emplaceFrame(TimeStamp(1), "PO", f1_pose);
+    FrameBasePtr fr2_ptr = problem->emplaceFrame(TimeStamp(2), "PO", f2_pose);
 
     // SENSOR
-    auto sensor_ptr = FactorySensorYaml::create("SensorOdom", "sensor_1", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+    auto sensor_ptr = FactorySensorYaml::create("SensorOdom2d", "SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
 
     // CAPTURE
     auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index ea1f4be3c9258238ba927e95196e9d0e2781cfb9..1f1adb3de9d9f25dcf39b68b78e6c6de6522aa78 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -35,31 +35,39 @@
 
 
 using namespace wolf;
+using namespace Eigen;
 
-
+////////////////////////////////////////////////////////
+//              FACTORY STATE BLOCK                   //
+////////////////////////////////////////////////////////
 TEST(FactoryStateBlock, creator_non_registered_key)
 {
     // non registered -> throw
-    ASSERT_THROW(auto sba = FactoryStateBlock::create("A", Eigen::Vector1d(6), false), std::runtime_error);
+    ASSERT_THROW(auto sba = FactoryStateBlock::create("A", Vector1d(6), false), std::runtime_error);
 }
 
 TEST(FactoryStateBlock, creator_StateBlock)
 {
-    ASSERT_THROW(auto sbp = FactoryStateBlock::create("StateBlock", Eigen::Vector3d(1,2,3), false), std::runtime_error);
+    ASSERT_THROW(auto sbp = FactoryStateBlock::create("StateBlock", Vector3d(1,2,3), false), std::runtime_error);
 }
 
 TEST(FactoryStateBlock, creator_Quaternion)
 {
-    auto sbq = FactoryStateBlock::create("StateQuaternion", Eigen::Vector4d(1,2,3,4).normalized(), false);
+    auto sbq = FactoryStateBlock::create("StateQuaternion", Vector4d(1,2,3,4).normalized(), false);
 
     ASSERT_EQ(sbq->getSize()     , 4);
     ASSERT_EQ(sbq->getLocalSize(), 3);
     ASSERT_TRUE(sbq->hasLocalParametrization());
 }
 
+TEST(FactoryStateBlock, creator_Quaternion_not_normalized)
+{
+    ASSERT_THROW(auto sbq = FactoryStateBlock::create("StateQuaternion", Vector4d(1,2,3,4), false), std::runtime_error);
+}
+
 TEST(FactoryStateBlock, creator_Angle)
 {
-    auto sba = FactoryStateBlock::create("StateAngle", Eigen::Vector1d(1), false);
+    auto sba = FactoryStateBlock::create("StateAngle", Vector1d(1), false);
 
     ASSERT_EQ(sba->getSize()     , 1);
     ASSERT_EQ(sba->getLocalSize(), 1);
@@ -68,54 +76,22 @@ TEST(FactoryStateBlock, creator_Angle)
 
 TEST(FactoryStateBlock, creator_Homogeneous3d)
 {
-    auto sbh = FactoryStateBlock::create("StateHomogeneous3d", Eigen::Vector4d(1,2,3,4), false);
+    auto sbh = FactoryStateBlock::create("StateHomogeneous3d", Vector4d(1,2,3,4), false);
 
     ASSERT_EQ(sbh->getSize()     , 4);
     ASSERT_EQ(sbh->getLocalSize(), 3);
     ASSERT_TRUE(sbh->hasLocalParametrization());
 }
 
-TEST(FactoryStateBlock, creator_H)
-{
-    auto sbh = FactoryStateBlock::create("H", Eigen::Vector4d(1,2,3,4), false);
-
-    ASSERT_EQ(sbh->getSize()     , 4);
-    ASSERT_EQ(sbh->getLocalSize(), 3);
-    ASSERT_TRUE(sbh->hasLocalParametrization());
-}
-
-TEST(FactoryStateBlock, creator_O_is_quaternion)
-{
-    auto sbq = FactoryStateBlock::create("O", Eigen::Vector4d(1,2,3,4).normalized(), false);
-
-    ASSERT_EQ(sbq->getSize()     , 4);
-    ASSERT_EQ(sbq->getLocalSize(), 3);
-    ASSERT_TRUE(sbq->hasLocalParametrization());
-}
-
-TEST(FactoryStateBlock, creator_O_is_angle)
-{
-    auto sba = FactoryStateBlock::create("O", Eigen::Vector1d(1), false);
-
-    ASSERT_EQ(sba->getSize()     , 1);
-    ASSERT_EQ(sba->getLocalSize(), 1);
-    ASSERT_TRUE(sba->hasLocalParametrization());
-}
-
-TEST(FactoryStateBlock, creator_O_is_wrong_size)
-{
-    ASSERT_THROW(auto sba = FactoryStateBlock::create("O", Eigen::Vector2d(1,2), false) , std::runtime_error);
-}
-
 TEST(FactoryStateBlock, creator_Point)
 {
-    auto sba = FactoryStateBlock::create("StatePoint2d", Eigen::Vector2d(1,2), false);
+    auto sba = FactoryStateBlock::create("StatePoint2d", Vector2d(1,2), false);
 
     ASSERT_EQ(sba->getSize()     , 2);
     ASSERT_EQ(sba->getLocalSize(), 2);
     ASSERT_FALSE(sba->hasLocalParametrization());
 
-    sba = FactoryStateBlock::create("StatePoint3d", Eigen::Vector3d(1,2,3), false);
+    sba = FactoryStateBlock::create("StatePoint3d", Vector3d(1,2,3), false);
 
     ASSERT_EQ(sba->getSize()     , 3);
     ASSERT_EQ(sba->getLocalSize(), 3);
@@ -125,72 +101,152 @@ TEST(FactoryStateBlock, creator_Point)
     ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error);
 }
 
-TEST(FactoryStateBlock, creator_P)
+TEST(FactoryStateBlock, creator_Vector)
 {
-    auto sba = FactoryStateBlock::create("P", Eigen::Vector2d(1,2), false);
+    auto sba = FactoryStateBlock::create("StateVector2d", Vector2d(1,2), false);
 
     ASSERT_EQ(sba->getSize()     , 2);
     ASSERT_EQ(sba->getLocalSize(), 2);
     ASSERT_FALSE(sba->hasLocalParametrization());
 
-    sba = FactoryStateBlock::create("P", Eigen::Vector3d(1,2,3), false);
+    sba = FactoryStateBlock::create("StateVector3d", Vector3d(1,2,3), false);
 
     ASSERT_EQ(sba->getSize()     , 3);
     ASSERT_EQ(sba->getLocalSize(), 3);
     ASSERT_FALSE(sba->hasLocalParametrization());
 
     // fails
-    ASSERT_THROW(sba = FactoryStateBlock::create("P", Vector1d(1), false) , std::runtime_error);
+    ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error);
 }
 
-TEST(FactoryStateBlock, creator_Vector)
+TEST(FactoryStateBlock, creator_Params)
 {
-    auto sba = FactoryStateBlock::create("StateVector2d", Eigen::Vector2d(1,2), false);
+    auto sb1  = FactoryStateBlock::create("StateParams1", Vector1d::Ones(), false);
+    auto sb2  = FactoryStateBlock::create("StateParams2", Vector2d::Ones(), false);
+    auto sb3  = FactoryStateBlock::create("StateParams3", Vector3d::Ones(), false);
+    auto sb4  = FactoryStateBlock::create("StateParams4", Vector4d::Ones(), false);
+    auto sb5  = FactoryStateBlock::create("StateParams5", Vector5d::Ones(), false);
+    auto sb6  = FactoryStateBlock::create("StateParams6", Vector6d::Ones(), false);
+    auto sb7  = FactoryStateBlock::create("StateParams7", Vector7d::Ones(), false);
+    auto sb8  = FactoryStateBlock::create("StateParams8", Vector8d::Ones(), false);
+    auto sb9  = FactoryStateBlock::create("StateParams9", Vector9d::Ones(), false);
+    auto sb10 = FactoryStateBlock::create("StateParams10", Vector10d::Ones(), false);
 
-    ASSERT_EQ(sba->getSize()     , 2);
-    ASSERT_EQ(sba->getLocalSize(), 2);
-    ASSERT_FALSE(sba->hasLocalParametrization());
-
-    sba = FactoryStateBlock::create("StateVector3d", Eigen::Vector3d(1,2,3), false);
+    ASSERT_EQ(sb1->getSize(), 1);
+    ASSERT_EQ(sb2->getSize(), 2);
+    ASSERT_EQ(sb3->getSize(), 3);
+    ASSERT_EQ(sb4->getSize(), 4);
+    ASSERT_EQ(sb5->getSize(), 5);
+    ASSERT_EQ(sb6->getSize(), 6);
+    ASSERT_EQ(sb7->getSize(), 7);
+    ASSERT_EQ(sb8->getSize(), 8);
+    ASSERT_EQ(sb9->getSize(), 9);
+    ASSERT_EQ(sb10->getSize(), 10);
 
-    ASSERT_EQ(sba->getSize()     , 3);
-    ASSERT_EQ(sba->getLocalSize(), 3);
-    ASSERT_FALSE(sba->hasLocalParametrization());
+    ASSERT_EQ(sb1->getLocalSize(), 1);
+    ASSERT_FALSE(sb1->hasLocalParametrization());
 
     // fails
-    ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error);
+    ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::runtime_error);
 }
 
-TEST(FactoryStateBlock, creator_V)
+////////////////////////////////////////////////////////
+//        FACTORY STATE BLOCK IDENTITY                //
+////////////////////////////////////////////////////////
+TEST(FactoryStateBlockIdentity, creator_idendity_non_registered_key)
 {
-    auto sba = FactoryStateBlock::create("V", Eigen::Vector2d(1,2), false);
+    // non registered -> throw
+    ASSERT_THROW(auto sba = FactoryStateBlockIdentity::create("A"), std::runtime_error);
+}
 
-    ASSERT_EQ(sba->getSize()     , 2);
-    ASSERT_EQ(sba->getLocalSize(), 2);
-    ASSERT_FALSE(sba->hasLocalParametrization());
+TEST(FactoryStateBlockIdentity, creator_StateBlock)
+{
+    ASSERT_THROW(auto sbp = FactoryStateBlockIdentity::create("StateBlock"), std::runtime_error);
+}
 
-    sba = FactoryStateBlock::create("V", Eigen::Vector3d(1,2,3), false);
+TEST(FactoryStateBlockIdentity, creator_Quaternion)
+{
+    auto sbq = FactoryStateBlockIdentity::create("StateQuaternion");
 
-    ASSERT_EQ(sba->getSize()     , 3);
-    ASSERT_EQ(sba->getLocalSize(), 3);
-    ASSERT_FALSE(sba->hasLocalParametrization());
+    ASSERT_EQ(sbq->getSize()     , 4);
+    ASSERT_EQ(sbq->getLocalSize(), 3);
+    ASSERT_TRUE(sbq->hasLocalParametrization());
+    ASSERT_TRUE(sbq->isValid());
+    ASSERT_QUATERNION_VECTOR_APPROX(Quaterniond::Identity().coeffs(), sbq->getState(), Constants::EPS_SMALL);
+}
 
-    // fails
-    ASSERT_THROW(sba = FactoryStateBlock::create("V", Vector1d(1), false) , std::runtime_error);
+TEST(FactoryStateBlockIdentity, creator_Angle)
+{
+    auto sba = FactoryStateBlockIdentity::create("StateAngle");
+
+    ASSERT_EQ(sba->getSize()     , 1);
+    ASSERT_EQ(sba->getLocalSize(), 1);
+    ASSERT_TRUE(sba->hasLocalParametrization());
+    ASSERT_TRUE(sba->isValid());
+    ASSERT_MATRIX_APPROX(Vector1d::Zero(), sba->getState(), Constants::EPS_SMALL);
 }
 
-TEST(FactoryStateBlock, creator_Params)
+TEST(FactoryStateBlockIdentity, creator_Homogeneous3d)
+{
+    auto sbh = FactoryStateBlockIdentity::create("StateHomogeneous3d");
+
+    ASSERT_EQ(sbh->getSize()     , 4);
+    ASSERT_EQ(sbh->getLocalSize(), 3);
+    ASSERT_TRUE(sbh->hasLocalParametrization());
+    ASSERT_TRUE(sbh->isValid());
+    ASSERT_MATRIX_APPROX(Quaterniond::Identity().coeffs(), sbh->getState(), Constants::EPS_SMALL);
+}
+
+TEST(FactoryStateBlockIdentity, creator_Point)
+{
+    auto sbp2d = FactoryStateBlockIdentity::create("StatePoint2d");
+
+    ASSERT_EQ(sbp2d->getSize()     , 2);
+    ASSERT_EQ(sbp2d->getLocalSize(), 2);
+    ASSERT_FALSE(sbp2d->hasLocalParametrization());
+    ASSERT_TRUE(sbp2d->isValid());
+    ASSERT_MATRIX_APPROX(Vector2d::Zero(), sbp2d->getState(), Constants::EPS_SMALL);
+
+    auto sbp3d  = FactoryStateBlockIdentity::create("StatePoint3d");
+
+    ASSERT_EQ(sbp3d->getSize()     , 3);
+    ASSERT_EQ(sbp3d->getLocalSize(), 3);
+    ASSERT_FALSE(sbp3d->hasLocalParametrization());
+    ASSERT_TRUE(sbp3d->isValid());
+    ASSERT_MATRIX_APPROX(Vector3d::Zero(), sbp3d->getState(), Constants::EPS_SMALL);
+}
+
+TEST(FactoryStateBlockIdentity, creator_Vector)
+{
+    auto sbv2d = FactoryStateBlockIdentity::create("StateVector2d");
+
+    ASSERT_EQ(sbv2d->getSize()     , 2);
+    ASSERT_EQ(sbv2d->getLocalSize(), 2);
+    ASSERT_FALSE(sbv2d->hasLocalParametrization());
+    ASSERT_TRUE(sbv2d->isValid());
+    ASSERT_MATRIX_APPROX(Vector2d::Zero(), sbv2d->getState(), Constants::EPS_SMALL);
+
+    auto sbv3d = FactoryStateBlockIdentity::create("StateVector3d");
+
+    ASSERT_EQ(sbv3d->getSize()     , 3);
+    ASSERT_EQ(sbv3d->getLocalSize(), 3);
+    ASSERT_FALSE(sbv3d->hasLocalParametrization());
+    ASSERT_TRUE(sbv3d->isValid());
+    ASSERT_MATRIX_APPROX(Vector3d::Zero(), sbv3d->getState(), Constants::EPS_SMALL);
+}
+
+TEST(FactoryStateBlockIdentity, creator_Params)
 {
-    auto sb1  = FactoryStateBlock::create("StateParams1", Eigen::Vector1d::Ones(), false);
-    auto sb2  = FactoryStateBlock::create("StateParams2", Eigen::Vector2d::Ones(), false);
-    auto sb3  = FactoryStateBlock::create("StateParams3", Eigen::Vector3d::Ones(), false);
-    auto sb4  = FactoryStateBlock::create("StateParams4", Eigen::Vector4d::Ones(), false);
-    auto sb5  = FactoryStateBlock::create("StateParams5", Eigen::Vector5d::Ones(), false);
-    auto sb6  = FactoryStateBlock::create("StateParams6", Eigen::Vector6d::Ones(), false);
-    auto sb7  = FactoryStateBlock::create("StateParams7", Eigen::Vector7d::Ones(), false);
-    auto sb8  = FactoryStateBlock::create("StateParams8", Eigen::Vector8d::Ones(), false);
-    auto sb9  = FactoryStateBlock::create("StateParams9", Eigen::Vector9d::Ones(), false);
-    auto sb10 = FactoryStateBlock::create("StateParams10", Eigen::Vector10d::Ones(), false);
+    auto sb1  = FactoryStateBlockIdentity::create("StateParams1");
+    auto sb2  = FactoryStateBlockIdentity::create("StateParams2");
+    auto sb3  = FactoryStateBlockIdentity::create("StateParams3");
+    auto sb4  = FactoryStateBlockIdentity::create("StateParams4");
+    auto sb5  = FactoryStateBlockIdentity::create("StateParams5");
+    auto sb6  = FactoryStateBlockIdentity::create("StateParams6");
+    auto sb7  = FactoryStateBlockIdentity::create("StateParams7");
+    auto sb8  = FactoryStateBlockIdentity::create("StateParams8");
+    auto sb9  = FactoryStateBlockIdentity::create("StateParams9");
+    auto sb10 = FactoryStateBlockIdentity::create("StateParams10");
 
     ASSERT_EQ(sb1->getSize(), 1);
     ASSERT_EQ(sb2->getSize(), 2);
@@ -204,10 +260,141 @@ TEST(FactoryStateBlock, creator_Params)
     ASSERT_EQ(sb10->getSize(), 10);
 
     ASSERT_EQ(sb1->getLocalSize(), 1);
+    ASSERT_EQ(sb2->getLocalSize(), 2);
+    ASSERT_EQ(sb3->getLocalSize(), 3);
+    ASSERT_EQ(sb4->getLocalSize(), 4);
+    ASSERT_EQ(sb5->getLocalSize(), 5);
+    ASSERT_EQ(sb6->getLocalSize(), 6);
+    ASSERT_EQ(sb7->getLocalSize(), 7);
+    ASSERT_EQ(sb8->getLocalSize(), 8);
+    ASSERT_EQ(sb9->getLocalSize(), 9);
+    ASSERT_EQ(sb10->getLocalSize(), 10);
+
     ASSERT_FALSE(sb1->hasLocalParametrization());
+    ASSERT_FALSE(sb2->hasLocalParametrization());
+    ASSERT_FALSE(sb3->hasLocalParametrization());
+    ASSERT_FALSE(sb4->hasLocalParametrization());
+    ASSERT_FALSE(sb5->hasLocalParametrization());
+    ASSERT_FALSE(sb6->hasLocalParametrization());
+    ASSERT_FALSE(sb7->hasLocalParametrization());
+    ASSERT_FALSE(sb8->hasLocalParametrization());
+    ASSERT_FALSE(sb9->hasLocalParametrization());
+    ASSERT_FALSE(sb10->hasLocalParametrization());
+    
+    ASSERT_TRUE(sb1->isValid());
+    ASSERT_TRUE(sb2->isValid());
+    ASSERT_TRUE(sb3->isValid());
+    ASSERT_TRUE(sb4->isValid());
+    ASSERT_TRUE(sb5->isValid());
+    ASSERT_TRUE(sb6->isValid());
+    ASSERT_TRUE(sb7->isValid());
+    ASSERT_TRUE(sb8->isValid());
+    ASSERT_TRUE(sb9->isValid());
+    ASSERT_TRUE(sb10->isValid());
+    
+    ASSERT_MATRIX_APPROX(Vector1d::Zero(), sb1->getState(), Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector2d::Zero(), sb2->getState(), Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector3d::Zero(), sb3->getState(), Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector4d::Zero(), sb4->getState(), Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector5d::Zero(), sb5->getState(), Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector6d::Zero(), sb6->getState(), Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector7d::Zero(), sb7->getState(), Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector8d::Zero(), sb8->getState(), Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector9d::Zero(), sb9->getState(), Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector10d::Zero(), sb10->getState(), Constants::EPS_SMALL);
+}
 
-    // fails
-    ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::runtime_error);
+////////////////////////////////////////////////////////
+//       FACTORY STATE BLOCK IDENTITY VECTOR          //
+////////////////////////////////////////////////////////
+TEST(FactoryStateBlockIdentityVector, creator_idendity_non_registered_key)
+{
+    // non registered -> throw
+    ASSERT_THROW(auto vec = FactoryStateBlockIdentityVector::create("A"), std::runtime_error);
+}
+
+TEST(FactoryStateBlockIdentityVector, creator_StateBlock)
+{
+    ASSERT_THROW(auto vec_p = FactoryStateBlockIdentityVector::create("StateBlock"), std::runtime_error);
+}
+
+TEST(FactoryStateBlockIdentityVector, creator_Quaternion)
+{
+    auto vec_q = FactoryStateBlockIdentityVector::create("StateQuaternion");
+
+    ASSERT_QUATERNION_VECTOR_APPROX(Quaterniond::Identity().coeffs(), vec_q, Constants::EPS_SMALL);
+}
+
+TEST(FactoryStateBlockIdentityVector, creator_Angle)
+{
+    auto vec = FactoryStateBlockIdentityVector::create("StateAngle");
+
+    ASSERT_MATRIX_APPROX(Vector1d::Zero(), vec, Constants::EPS_SMALL);
+}
+
+TEST(FactoryStateBlockIdentityVector, creator_Homogeneous3d)
+{
+    auto vec_h = FactoryStateBlockIdentityVector::create("StateHomogeneous3d");
+
+    ASSERT_MATRIX_APPROX(Quaterniond::Identity().coeffs(), vec_h, Constants::EPS_SMALL);
+}
+
+TEST(FactoryStateBlockIdentityVector, creator_Point)
+{
+    auto vec = FactoryStateBlockIdentityVector::create("StatePoint2d");
+
+    ASSERT_MATRIX_APPROX(Vector2d::Zero(), vec, Constants::EPS_SMALL);
+
+    vec = FactoryStateBlockIdentityVector::create("StatePoint3d");
+
+    ASSERT_MATRIX_APPROX(Vector3d::Zero(), vec, Constants::EPS_SMALL);
+}
+
+TEST(FactoryStateBlockIdentityVector, creator_Vector)
+{
+    auto vec = FactoryStateBlockIdentityVector::create("StateVector2d");
+    
+    ASSERT_MATRIX_APPROX(Vector2d::Zero(), vec, Constants::EPS_SMALL);
+
+    vec = FactoryStateBlockIdentityVector::create("StateVector3d");
+
+    ASSERT_MATRIX_APPROX(Vector3d::Zero(), vec, Constants::EPS_SMALL);
+}
+
+TEST(FactoryStateBlockIdentityVectorVector, creator_Params)
+{
+    auto vec_1  = FactoryStateBlockIdentityVector::create("StateParams1");
+    auto vec_2  = FactoryStateBlockIdentityVector::create("StateParams2");
+    auto vec_3  = FactoryStateBlockIdentityVector::create("StateParams3");
+    auto vec_4  = FactoryStateBlockIdentityVector::create("StateParams4");
+    auto vec_5  = FactoryStateBlockIdentityVector::create("StateParams5");
+    auto vec_6  = FactoryStateBlockIdentityVector::create("StateParams6");
+    auto vec_7  = FactoryStateBlockIdentityVector::create("StateParams7");
+    auto vec_8  = FactoryStateBlockIdentityVector::create("StateParams8");
+    auto vec_9  = FactoryStateBlockIdentityVector::create("StateParams9");
+    auto vec_10 = FactoryStateBlockIdentityVector::create("StateParams10");
+
+    ASSERT_EQ(vec_1.size(), 1);
+    ASSERT_EQ(vec_2.size(), 2);
+    ASSERT_EQ(vec_3.size(), 3);
+    ASSERT_EQ(vec_4.size(), 4);
+    ASSERT_EQ(vec_5.size(), 5);
+    ASSERT_EQ(vec_6.size(), 6);
+    ASSERT_EQ(vec_7.size(), 7);
+    ASSERT_EQ(vec_8.size(), 8);
+    ASSERT_EQ(vec_9.size(), 9);
+    ASSERT_EQ(vec_10.size(), 10);
+    
+    ASSERT_MATRIX_APPROX(Vector1d::Zero(), vec_1, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector2d::Zero(), vec_2, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector3d::Zero(), vec_3, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector4d::Zero(), vec_4, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector5d::Zero(), vec_5, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector6d::Zero(), vec_6, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector7d::Zero(), vec_7, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector8d::Zero(), vec_8, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector9d::Zero(), vec_9, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(Vector10d::Zero(), vec_10, Constants::EPS_SMALL);
 }
 
 int main(int argc, char **argv)
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index ac3e52514df922b77473e912d7e10ef297904afd..9305205d9218ee1f0df289f2e68e833c1ca5dbb3 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -419,7 +419,7 @@ TEST(FrameBase, GetSetState)
 TEST(FrameBase, Constructor_composite)
 {
     FrameBase F = FrameBase(0.0,
-                            "POV",
+                            {{'P',"StatePoint3d"},{'O',"StateQuaternion"},{'V',"StateVector3d"}},
                             VectorComposite("POV", {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)}));
 
     ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
@@ -427,18 +427,6 @@ TEST(FrameBase, Constructor_composite)
     ASSERT_EQ           (F.getStateBlock('V')->getSize(), 3);
 }
 
-TEST(FrameBase, Constructor_vectors)
-{
-    FrameBase F = FrameBase(0.0,
-                            "POV",
-                            {Vector3d(1,2,3), Vector4d(1,2,3,4).normalized(), Vector3d(1,2,3)});
-
-    ASSERT_MATRIX_APPROX(F.getP()->getState(), Vector3d(1,2,3), 1e-15);
-    ASSERT_TRUE         (F.getO()->hasLocalParametrization());
-    ASSERT_EQ           (F.getStateBlock('V')->getSize(), 3);
-}
-
-
 
 int main(int argc, char **argv)
 {
diff --git a/test/gtest_graph_search.cpp b/test/gtest_graph_search.cpp
index 43d4b57242a3bb7d1e4532b893d34fcb47fcfcb2..cae74f13d4f7134d855f799ce60e5b206a7fc0de 100644
--- a/test/gtest_graph_search.cpp
+++ b/test/gtest_graph_search.cpp
@@ -51,7 +51,7 @@ class GraphSearchTest : public testing::Test
 
         FrameBasePtr emplaceFrame(const TimeStamp& ts)
         {
-            return problem->emplaceFrame(ts, Vector3d::Zero());
+            return problem->emplaceFrame(ts, "PO", Vector3d::Zero());
         }
 
         FactorBasePtr createFactor(FrameBasePtr frm1, FrameBasePtr frm2)
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index 2b02be288217cca57703ab98b5b9c7a40f32d505..940d27670ead0f61139e387a1f5bc519b383a803 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.cpp
@@ -19,13 +19,6 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-/**
- * \file gtest_has_state_blocks.cpp
- *
- *  Created on: Mar 24, 2020
- *      \author: jsola
- */
-
 
 #include "core/utils/utils_gtest.h"
 
@@ -41,9 +34,7 @@ using namespace wolf;
 
 class HasStateBlocksTest : public testing::Test
 {
-
     public:
-
         ProblemPtr      problem;
         SensorBasePtr   S0, S1;
         FrameBasePtr    F0, F1;
@@ -52,7 +43,6 @@ class HasStateBlocksTest : public testing::Test
         StateBlockPtr   sbp0, sbv0, sbp1, sbv1;
         StateQuaternionPtr   sbo0, sbo1;
 
-
         void SetUp() override
         {
             problem = Problem::create("POV", 3);
@@ -69,10 +59,8 @@ class HasStateBlocksTest : public testing::Test
 
         }
         void TearDown() override{}
-
 };
 
-
 TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
 {
     Notification n;
@@ -98,7 +86,6 @@ TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
 
     ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
     ASSERT_EQ(n, ADD);
-
 }
 
 TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
@@ -123,12 +110,10 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
 
     ASSERT_TRUE(problem->getStateBlockNotification(sbv1, n));
     ASSERT_EQ(n, ADD);
-
 }
 
 TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
 {
-
     SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
 
 
@@ -166,7 +151,6 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
     // Add again the same SB. This should crash
 
     ASSERT_DEATH( F0->addStateBlock('V', sbv0, nullptr) , "" );
-
 }
 
 TEST_F(HasStateBlocksTest, hasStateBlock)
@@ -189,7 +173,7 @@ TEST_F(HasStateBlocksTest, getState_structure)
 {
     F0->addStateBlock('V', sbv0, nullptr); // now KF0 is POV
 
-    WOLF_DEBUG("Retrieving state from F0 with structure ", F0->getStructure());
+    WOLF_DEBUG("Retrieving state from F0 with structure ", F0->getKeys());
 
     auto state0 = F0->getState();
     WOLF_DEBUG("getState()     = ", state0);
@@ -219,10 +203,8 @@ TEST_F(HasStateBlocksTest, getState_structure)
     ASSERT_TRUE(state0.count('O'));
     ASSERT_FALSE(state0.count('V'));
     ASSERT_FALSE(state0.count('W'));
-
 }
 
-
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_motion_provider.cpp b/test/gtest_motion_provider.cpp
index b63823360fb0ed81441108927e5feb3d2947baf1..b8434ef925848e9203b9df3e76d14b8fd6d2ac86 100644
--- a/test/gtest_motion_provider.cpp
+++ b/test/gtest_motion_provider.cpp
@@ -54,41 +54,38 @@ class MotionProviderTest : public testing::Test
             problem = Problem::create("POV", 2);
 
             // Install odom sensor
-            sen = problem->installSensor("SensorOdom",
-                                         "odometer",
-                                         wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+            sen = problem->installSensor("SensorOdom2d",
+                                         wolf_root + "/test/yaml/sensor_odom_2d.yaml",
+                                         {wolf_root});
 
             // Install 3 odom processors
             ParamsMotionProviderDummyPtr prc1_params = std::make_shared<ParamsMotionProviderDummy>();
             prc1_params->time_tolerance = dt/2;
-            prc1_params->state_structure = "PO";
+            prc1_params->state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"}};
             prc1_params->state_provider = false;
-            prc1 = problem->installProcessor("MotionProviderDummy",
-                                             "not getter processor",
-                                             sen,
-                                             prc1_params);
+            prc1_params->name = "not getter processor";
+            
+            prc1 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc1_params);
             im1 = std::dynamic_pointer_cast<MotionProvider>(prc1);
 
             ParamsMotionProviderDummyPtr prc2_params = std::make_shared<ParamsMotionProviderDummy>();
             prc2_params->time_tolerance = dt/2;
-            prc2_params->state_structure = "PO";
+            prc2_params->state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"}};
             prc2_params->state_provider = true;
-            prc2_params->state_order = 1;
-            prc2 = problem->installProcessor("MotionProviderDummy",
-                                             "getter processor",
-                                             sen,
-                                             prc2_params);
+            prc2_params->state_provider_order = 1;
+            prc1_params->name = "getter processor";
+
+            prc2 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc2_params);
             im2 = std::dynamic_pointer_cast<MotionProvider>(prc2);
 
             ParamsMotionProviderDummyPtr prc3_params = std::make_shared<ParamsMotionProviderDummy>();
             prc3_params->time_tolerance = dt/2;
-            prc3_params->state_structure = "POV";
+            prc3_params->state_types = {{'P',"StatePoint2d"},{'O',"StateAngle"},{'V',"StateVector2d"}};
             prc3_params->state_provider = true;
-            prc3_params->state_order = 1;
-            prc3 = problem->installProcessor("MotionProviderDummy",
-                                             "getter processor low priority",
-                                             sen,
-                                             prc3_params);
+            prc3_params->state_provider_order = 1;
+            prc1_params->name = "getter processor lower priority";
+
+            prc3 = ProcessorBase::emplace<MotionProviderDummy>(sen, prc3_params);
             im3 = std::dynamic_pointer_cast<MotionProvider>(prc3);
         }
 };
@@ -119,9 +116,15 @@ TEST_F(MotionProviderTest, install)
     ASSERT_TRUE(im3->isStateGetter());
     ASSERT_EQ(im2->getOrder(), 1);
     ASSERT_EQ(im3->getOrder(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised.
-    ASSERT_EQ(im1->getStateStructure(), "PO");
-    ASSERT_EQ(im2->getStateStructure(), "PO");
-    ASSERT_EQ(im3->getStateStructure(), "POV");
+    ASSERT_TRUE(im1->getStateTypes().has('P'));
+    ASSERT_TRUE(im1->getStateTypes().has('O'));
+    ASSERT_FALSE(im1->getStateTypes().has('V'));
+    ASSERT_TRUE(im2->getStateTypes().has('P'));
+    ASSERT_TRUE(im2->getStateTypes().has('O'));
+    ASSERT_FALSE(im2->getStateTypes().has('V'));
+    ASSERT_TRUE(im3->getStateTypes().has('P'));
+    ASSERT_TRUE(im3->getStateTypes().has('O'));
+    ASSERT_TRUE(im3->getStateTypes().has('V'));
 
     // Only 2 and 3 in problem::motion_provider_map_
     ASSERT_EQ(problem->getMotionProviderMap().size(), 2);
@@ -131,12 +134,8 @@ TEST_F(MotionProviderTest, install)
 
 TEST_F(MotionProviderTest, odometry)
 {
-    VectorComposite odom_p("P"); // missing P key
-    odom_p['P'] = Vector2d::Zero();
-    VectorComposite odom_pov("POV"); // key V not needed by prc1 and prc2
-    odom_pov['P'] = Vector2d::Zero();
-    odom_pov['O'] = Vector1d::Zero();
-    odom_pov['V'] = Vector2d::Zero();
+    VectorComposite odom_p("P",{Vector2d::Zero()});
+    VectorComposite odom_pov("POV",{Vector2d::Zero(),Vector1d::Zero(),Vector2d::Zero()});
 
     // Error: required PO keys to be added
     ASSERT_DEATH({im1->setOdometry(odom_p);},"");
@@ -145,9 +144,7 @@ TEST_F(MotionProviderTest, odometry)
     im3->setOdometry(odom_pov);
 
     // im1 ->set odom = 0, 0, 0
-    VectorComposite odom1("PO");
-    odom1['P'] = Vector2d::Zero();
-    odom1['O'] = Vector1d::Zero();
+    VectorComposite odom1("PO",{Vector2d::Zero(),Vector1d::Zero()});
     im1->setOdometry(odom1);
     auto odom1_get = im1->getOdometry();
     EXPECT_TRUE(odom1_get.count('P') == 1);
@@ -156,9 +153,7 @@ TEST_F(MotionProviderTest, odometry)
     EXPECT_MATRIX_APPROX(odom1_get.at('O'), odom1.at('O'), 1e-9);
 
     // im1 ->set odom = 1, 1, 1
-    VectorComposite odom2("PO");
-    odom2['P'] = Vector2d::Ones();
-    odom2['O'] = Vector1d::Ones();
+    VectorComposite odom2("PO",{Vector2d::Ones(),Vector1d::Ones()});
     im2->setOdometry(odom2);
     auto odom2_get = im2->getOdometry();
     EXPECT_TRUE(odom2_get.count('P') == 1);
@@ -167,10 +162,7 @@ TEST_F(MotionProviderTest, odometry)
     EXPECT_MATRIX_APPROX(odom2_get.at('O'), odom2.at('O'), 1e-9);
 
     // im1 ->set odom = 2, 2, 2, 2, 2
-    VectorComposite odom3("POV");
-    odom3['P'] = 2 * Vector2d::Ones();
-    odom3['O'] = 2 * Vector1d::Ones();
-    odom3['V'] = 2 * Vector2d::Ones();
+    VectorComposite odom3("POV",{2 * Vector2d::Ones(), 2 * Vector1d::Ones(), 2 * Vector2d::Ones()});
     im3->setOdometry(odom3);
     auto odom3_get = im3->getOdometry();
     EXPECT_TRUE(odom3_get.count('P') == 1);
diff --git a/test/gtest_prior_sensor.cpp b/test/gtest_prior_sensor.cpp
index 53bda0b3e1379939454150e52a6e162e4b29e29e..c6a6256990957c0cd7081144c378855f78f9c914 100644
--- a/test/gtest_prior_sensor.cpp
+++ b/test/gtest_prior_sensor.cpp
@@ -97,101 +97,45 @@ TEST(SpecStateSensor, P)
   std::vector<PriorAsStruct> setups_ok, setups_death;
 
   // Initial guess - not dynamic
-  setups_ok   .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,false}));
-  setups_ok   .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,false}));
-  setups_death.push_back(PriorAsStruct({"P","initial_guess",vector4,vector0,false})); // wrong state size
+  setups_ok   .push_back(PriorAsStruct({"StatePoint2d","initial_guess",vector2,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"StatePoint3d","initial_guess",vector3,vector0,false}));
+  setups_death.push_back(PriorAsStruct({"StatePoint3d","initial_guess",vector4,vector0,false})); // wrong state size
 
   // Initial guess - dynamic
-  setups_ok   .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,true}));
-  setups_ok   .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,true,vector0}));
-  setups_ok   .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,true,vector2}));
-  setups_ok   .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,true,vector3}));
-  setups_death.push_back(PriorAsStruct({"P","initial_guess",vector4,vector0,true,vector4})); // wrong state size
-  setups_death.push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,true,vector4})); // inconsistent size
+  setups_ok   .push_back(PriorAsStruct({"StatePoint2d","initial_guess",vector2,vector0,true}));
+  setups_ok   .push_back(PriorAsStruct({"StatePoint3d","initial_guess",vector3,vector0,true,vector0}));
+  setups_ok   .push_back(PriorAsStruct({"StatePoint2d","initial_guess",vector2,vector0,true,vector2}));
+  setups_ok   .push_back(PriorAsStruct({"StatePoint3d","initial_guess",vector3,vector0,true,vector3}));
+  setups_death.push_back(PriorAsStruct({"StatePoint3d","initial_guess",vector4,vector0,true,vector4})); // wrong state size
+  setups_death.push_back(PriorAsStruct({"StatePoint3d","initial_guess",vector3,vector0,true,vector4})); // inconsistent size
   
   // Fix - not dynamic
-  setups_ok   .push_back(PriorAsStruct({"P","fix",vector2,vector0,false}));
-  setups_ok   .push_back(PriorAsStruct({"P","fix",vector3,vector0,false}));
-  setups_death.push_back(PriorAsStruct({"P","fix",vector4,vector0,false})); // wrong size
+  setups_ok   .push_back(PriorAsStruct({"StatePoint2d","fix",vector2,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"StatePoint3d","fix",vector3,vector0,false}));
+  setups_death.push_back(PriorAsStruct({"StatePoint2d","fix",vector4,vector0,false})); // wrong size
 
   // Fix - dynamic
-  setups_ok   .push_back(PriorAsStruct({"P","fix",vector2,vector0,true,vector0}));
-  setups_ok   .push_back(PriorAsStruct({"P","fix",vector3,vector0,true}));
-  setups_ok   .push_back(PriorAsStruct({"P","fix",vector2,vector0,true,vector2}));
-  setups_ok   .push_back(PriorAsStruct({"P","fix",vector3,vector0,true,vector3}));
-  setups_death.push_back(PriorAsStruct({"P","fix",vector4,vector0,true,vector4})); // wrong state size
-  setups_death.push_back(PriorAsStruct({"P","fix",vector3,vector0,true,vector4})); // inconsistent size
+  setups_ok   .push_back(PriorAsStruct({"StatePoint2d","fix",vector2,vector0,true,vector0}));
+  setups_ok   .push_back(PriorAsStruct({"StatePoint3d","fix",vector3,vector0,true}));
+  setups_ok   .push_back(PriorAsStruct({"StatePoint2d","fix",vector2,vector0,true,vector2}));
+  setups_ok   .push_back(PriorAsStruct({"StatePoint3d","fix",vector3,vector0,true,vector3}));
+  setups_death.push_back(PriorAsStruct({"StatePoint3d","fix",vector4,vector0,true,vector4})); // wrong state size
+  setups_death.push_back(PriorAsStruct({"StatePoint3d","fix",vector3,vector0,true,vector4})); // inconsistent size
   
   // Factor - not dynamic
-  setups_ok   .push_back(PriorAsStruct({"P","factor",vector2,vector2,false}));
-  setups_ok   .push_back(PriorAsStruct({"P","factor",vector3,vector3,false}));
-  setups_death.push_back(PriorAsStruct({"P","factor",vector4,vector4,false})); // wrong state size
-  setups_death.push_back(PriorAsStruct({"P","factor",vector2,vector3,false})); // inconsistent size
+  setups_ok   .push_back(PriorAsStruct({"StatePoint2d","factor",vector2,vector2,false}));
+  setups_ok   .push_back(PriorAsStruct({"StatePoint3d","factor",vector3,vector3,false}));
+  setups_death.push_back(PriorAsStruct({"StatePoint3d","factor",vector4,vector4,false})); // wrong state size
+  setups_death.push_back(PriorAsStruct({"StatePoint2d","factor",vector2,vector3,false})); // inconsistent size
 
   // Factor - dynamic
-  setups_ok   .push_back(PriorAsStruct({"P","factor",vector2,vector2,true}));
-  setups_ok   .push_back(PriorAsStruct({"P","factor",vector3,vector3,true}));
-  setups_ok   .push_back(PriorAsStruct({"P","factor",vector2,vector2,true,vector2}));
-  setups_ok   .push_back(PriorAsStruct({"P","factor",vector3,vector3,true,vector3}));
-  setups_death.push_back(PriorAsStruct({"P","factor",vector4,vector4,true,vector4})); // wrong state size
-  setups_death.push_back(PriorAsStruct({"P","factor",vector3,vector3,true,vector4})); // inconsistent size
-  setups_death.push_back(PriorAsStruct({"P","factor",vector3,vector4,true,vector3})); // inconsistent size
-
-  // TEST SETUPS
-  testPriorSensorSensorMap(setups_ok, true);
-  testPriorSensorSensorMap(setups_death, false);
-}
-
-TEST(SpecStateSensor, O)
-{
-  std::vector<PriorAsStruct> setups_ok, setups_death;
-
-  // Initial guess - not dynamic
-  setups_ok   .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,false}));
-  setups_ok   .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,false}));
-  setups_death.push_back(PriorAsStruct({"O","initial_guess",vector4,vector0,false})); // not normalized
-  setups_death.push_back(PriorAsStruct({"O","initial_guess",vector3,vector0,false})); // wrong size
-
-  // Initial guess - dynamic
-  setups_ok   .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,true}));
-  setups_ok   .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,true,vector0}));
-  setups_ok   .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,true,vector1}));
-  setups_ok   .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,true,vector3}));
-  setups_death.push_back(PriorAsStruct({"O","initial_guess",vector4,vector0,true,vector3})); // not normalized
-  setups_death.push_back(PriorAsStruct({"O","initial_guess",vector3,vector0,true,vector3})); // wrong size
-  setups_death.push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,true,vector4})); // inconsistent size
-  
-  // Fix - not dynamic
-  setups_ok   .push_back(PriorAsStruct({"O","fix",vector1,vector0,false}));
-  setups_ok   .push_back(PriorAsStruct({"O","fix",vectorq,vector0,false}));
-  setups_death.push_back(PriorAsStruct({"O","fix",vector3,vector0,false})); // wrong size
-  setups_death.push_back(PriorAsStruct({"O","fix",vector4,vector0,false})); // not normalized
-
-  // Fix - dynamic
-  setups_ok   .push_back(PriorAsStruct({"O","fix",vector1,vector0,true,vector0}));
-  setups_ok   .push_back(PriorAsStruct({"O","fix",vectorq,vector0,true}));
-  setups_ok   .push_back(PriorAsStruct({"O","fix",vector1,vector0,true,vector1}));
-  setups_ok   .push_back(PriorAsStruct({"O","fix",vectorq,vector0,true,vector3}));
-  setups_death.push_back(PriorAsStruct({"O","fix",vector4,vector0,true,vector3})); // not normalized
-  setups_death.push_back(PriorAsStruct({"O","fix",vector3,vector0,true,vector3})); // wrong size
-  setups_death.push_back(PriorAsStruct({"O","fix",vectorq,vector0,true,vector4})); // inconsistent size
-
-  // Factor - not dynamic
-  setups_ok   .push_back(PriorAsStruct({"O","factor",vector1,vector1,false}));
-  setups_ok   .push_back(PriorAsStruct({"O","factor",vectorq,vector3,false}));
-  setups_death.push_back(PriorAsStruct({"O","factor",vector4,vector3,false})); // not normalized
-  setups_death.push_back(PriorAsStruct({"O","factor",vector3,vector3,false})); // wrong size
-  setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector4,false})); // inconsistent size
-
-  // Factor - dynamic
-  setups_ok   .push_back(PriorAsStruct({"O","factor",vector1,vector1,true}));
-  setups_ok   .push_back(PriorAsStruct({"O","factor",vectorq,vector3,true}));
-  setups_ok   .push_back(PriorAsStruct({"O","factor",vector1,vector1,true,vector1}));
-  setups_ok   .push_back(PriorAsStruct({"O","factor",vectorq,vector3,true,vector3}));
-  setups_death.push_back(PriorAsStruct({"O","factor",vector4,vector3,true,vector3})); // not normalized
-  setups_death.push_back(PriorAsStruct({"O","factor",vector3,vector3,true,vector3})); // wrong size
-  setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector3,true,vector4})); // inconsistent size
-  setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector4,true,vector3})); // inconsistent size
+  setups_ok   .push_back(PriorAsStruct({"StatePoint2d","factor",vector2,vector2,true}));
+  setups_ok   .push_back(PriorAsStruct({"StatePoint3d","factor",vector3,vector3,true}));
+  setups_ok   .push_back(PriorAsStruct({"StatePoint2d","factor",vector2,vector2,true,vector2}));
+  setups_ok   .push_back(PriorAsStruct({"StatePoint3d","factor",vector3,vector3,true,vector3}));
+  setups_death.push_back(PriorAsStruct({"StatePoint3d","factor",vector4,vector4,true,vector4})); // wrong state size
+  setups_death.push_back(PriorAsStruct({"StatePoint3d","factor",vector3,vector3,true,vector4})); // inconsistent size
+  setups_death.push_back(PriorAsStruct({"StatePoint3d","factor",vector3,vector4,true,vector3})); // inconsistent size
 
   // TEST SETUPS
   testPriorSensorSensorMap(setups_ok, true);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 1b65d7c045617e2124668eb69f6717a82bedfb6c..2fbf543f033f9a2efee6959e57d1d3347fca89fa 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -19,12 +19,6 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-/*
- * gtest_problem.cpp
- *
- *  Created on: Nov 23, 2016
- *      Author: jsola
- */
 
 #include "core/utils/utils_gtest.h"
 
@@ -68,7 +62,8 @@ TEST(Problem, create)
     ASSERT_EQ(P, P->getMap()->getProblem());
 
     // check frame structure through the state size
-    ASSERT_EQ(P->getFrameStructure(), "POV");
+    ASSERT_EQ(P->getFrameKeys().size(), 3);
+    ASSERT_TRUE(P->getFrameTypes().has("POV"));
 }
 
 TEST(Problem, Sensors)
@@ -80,8 +75,8 @@ TEST(Problem, Sensors)
     params->name = "dummy_name";
     auto S = SensorBase::emplace<SensorDummy3d>(P->getHardware(),
                                                 params,
-                                                SpecSensorComposite({{'P',SpecStateSensor("P",Vector3d::Zero())},
-                                                        {'O',SpecStateSensor("O",Vector4d::Random().normalized())}}));
+                                                SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())},
+                                                                     {'O',SpecStateSensor("StateQuaternion",Vector4d::Random().normalized())}}));
     // check pointers
     ASSERT_EQ(P, S->getProblem());
     ASSERT_EQ(P->getHardware(), S->getHardware());
@@ -100,8 +95,8 @@ TEST(Problem, Processor)
     params->name = "dummy_name";
     auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), 
                                                 params, 
-                                                SpecSensorComposite({{'P',SpecStateSensor("P",Vector3d::Zero())},
-                                                        {'O',SpecStateSensor("O",Vector4d::Random().normalized())}}));
+                                                SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d",Vector3d::Zero())},
+                                                                     {'O',SpecStateSensor("StateQuaternion",Vector4d::Random().normalized())}}));
 
     // add motion processor
     auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ParamsProcessorOdom3d>());
@@ -116,16 +111,24 @@ TEST(Problem, Installers)
     ProblemPtr P = Problem::create("PO", 3);
     Eigen::Vector7d xs; xs.setRandom(); xs.tail(4).normalize();
 
-    SensorBasePtr S = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root});
+    SensorBasePtr S = P->installSensor("SensorOdom3d", 
+                                       wolf_root + "/test/yaml/sensor_odom_3d.yaml", 
+                                       {wolf_root});
 
     // install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test)
-    auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", S->getName(), wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_root});
+    auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", 
+                                  S->getName(), 
+                                  wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", 
+                                  {wolf_root});
 
     // check motion processor IS NOT set
     ASSERT_TRUE(P->getMotionProviderMap().empty());
 
     // install processor motion
-    ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", S->getName(), wolf_root + "/test/yaml/processor_odom_3d.yaml", {wolf_root});
+    ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", 
+                                              S->getName(), 
+                                              wolf_root + "/test/yaml/processor_odom_3d.yaml", 
+                                              {wolf_root});
 
     // check motion processor is set
     ASSERT_FALSE(P->getMotionProviderMap().empty());
@@ -139,8 +142,8 @@ TEST(Problem, SetOrigin_PO_2d)
     ProblemPtr P = Problem::create("PO", 2);
     TimeStamp t0(0);
     SpecComposite prior;
-    prior.emplace('P',SpecState("P",Vector2d(1,2),"factor",Vector2d(sqrt(0.1),sqrt(0.1))));
-    prior.emplace('O',SpecState("O",Vector1d(3),"factor",Vector1d(sqrt(0.1))));
+    prior.emplace('P',SpecState("StatePoint2d",Vector2d(1,2),"factor",Vector2d(sqrt(0.1),sqrt(0.1))));
+    prior.emplace('O',SpecState("StateAngle",Vector1d(3),"factor",Vector1d(sqrt(0.1))));
     P->setPrior(prior, t0);
 
     // P->setPriorFactor(x0, s0, t0);
@@ -198,8 +201,8 @@ TEST(Problem, SetOrigin_PO_3d)
     ProblemPtr P = Problem::create("PO", 3);
     TimeStamp       t0(0);
     SpecComposite prior;
-    prior.emplace('P',SpecState("P",Vector3d(1,2,3),"factor",Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1))));
-    prior.emplace('O',SpecState("O",Vector4d(4,5,6,7).normalized(),"factor",Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1))));
+    prior.emplace('P',SpecState("StatePoint3d",Vector3d(1,2,3),"factor",Vector3d::Constant(sqrt(0.1))));
+    prior.emplace('O',SpecState("StateQuaternion",Vector4d(4,5,6,7).normalized(),"factor",Vector3d::Constant(sqrt(0.1))));
     P->setPrior(prior, t0);
     // Eigen::VectorXd vec7(7); vec7 << 1,2,3,4,5,6,7;
     // vec7.tail(4).normalize();
@@ -257,35 +260,55 @@ TEST(Problem, SetOrigin_PO_3d)
     //    P->print(4,1,1,1);
 }
 
-TEST(Problem, emplaceFrame_factory)
+TEST(Problem, emplaceFrame_factory_2D)
 {
-    ProblemPtr P = Problem::create("PO", 2);
+    Vector3d xpo;
+    Vector5d xpov;
+    
+    ProblemPtr P = Problem::create("POV", 2);
+    FrameBasePtr f1 = P->emplaceFrame(0, "PO" , xpo  );
+    FrameBasePtr f2 = P->emplaceFrame(1, "POV", xpov );
+
+    // check that all frames are effectively in the trajectory
+    ASSERT_EQ(P->getTrajectory()->size(), (SizeStd) 2);
+
+    ASSERT_EQ(f1->getState().size(), 2 );
+    ASSERT_EQ(f2->getState().size(), 3 );
+    ASSERT_EQ(f1->getStateVector("PO").size(), 3 );
+    ASSERT_EQ(f2->getStateVector("POV").size(), 5 );
+
 
-    Vector3d xpo2;
-    Vector7d xpo3; xpo3 << 1,2,3,.5,.5,.5,.5;
-    VectorXd xpov3(10); xpov3 << 1,2,3,.5,.5,.5,.5,1,2,3;
+    // check that all frames are linked to Problem
+    ASSERT_EQ(f1->getProblem(), P);
+    ASSERT_EQ(f2->getProblem(), P);
+
+    ASSERT_EQ(P->getFrameKeys().size(), 3);
+    ASSERT_TRUE(P->getFrameTypes().has("POV"));
+}
 
-    FrameBasePtr f0 = P->emplaceFrame(0, "PO" , 2,  xpo2  );
-    FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3,  xpo3  );
-    FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3,  xpov3 );
+TEST(Problem, emplaceFrame_factory_3D)
+{
+    Vector7d xpo; xpo << 1,2,3,.5,.5,.5,.5;
+    VectorXd xpov(10); xpov << 1,2,3,.5,.5,.5,.5,1,2,3;
 
+    ProblemPtr P = Problem::create("POV", 3);
+    FrameBasePtr f1 = P->emplaceFrame(0, "PO" , xpo  );
+    FrameBasePtr f2 = P->emplaceFrame(1, "POV", xpov );
 
     // check that all frames are effectively in the trajectory
-    ASSERT_EQ(P->getTrajectory()->size(), (SizeStd) 3);
+    ASSERT_EQ(P->getTrajectory()->size(), (SizeStd) 2);
 
-    ASSERT_EQ(f0->getState().size(), 2 );
     ASSERT_EQ(f1->getState().size(), 2 );
     ASSERT_EQ(f2->getState().size(), 3 );
-    ASSERT_EQ(f0->getStateVector("PO").size(), 3 );
     ASSERT_EQ(f1->getStateVector("PO").size(), 7 );
     ASSERT_EQ(f2->getStateVector("POV").size(), 10 );
 
     // check that all frames are linked to Problem
-    ASSERT_EQ(f0->getProblem(), P);
     ASSERT_EQ(f1->getProblem(), P);
     ASSERT_EQ(f2->getProblem(), P);
 
-    ASSERT_EQ(P->getFrameStructure(), "POV");
+    ASSERT_EQ(P->getFrameKeys().size(), 3);
+    ASSERT_TRUE(P->getFrameTypes().has("POV"));
 }
 
 TEST(Problem, StateBlocks)
@@ -297,14 +320,22 @@ TEST(Problem, StateBlocks)
     Eigen::Vector3d xs2d;
 
     // 2 state blocks, fixed
-    SensorBasePtr Sm = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root});
+    SensorBasePtr Sm = P->installSensor("SensorOdom3d", 
+                                        wolf_root + "/test/yaml/sensor_odom_3d.yaml", 
+                                        {wolf_root});
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
 
-    auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", Sm->getName(), wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_root});
-    auto pm = P->installProcessor("ProcessorOdom3d", Sm->getName(), wolf_root + "/test/yaml/processor_odom_3d.yaml", {wolf_root});
+    auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", 
+                                  Sm->getName(), 
+                                  wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", 
+                                  {wolf_root});
+    auto pm = P->installProcessor("ProcessorOdom3d",
+                                  Sm->getName(), 
+                                  wolf_root + "/test/yaml/processor_odom_3d.yaml", 
+                                  {wolf_root});
 
     // 2 state blocks, estimated
-    auto KF = P->emplaceFrame(0, "PO", 3, xs3d );
+    auto KF = P->emplaceFrame(0, "PO", xs3d );
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2));
 
     // Notifications
@@ -319,7 +350,7 @@ TEST(Problem, StateBlocks)
     Sm->unfixExtrinsics();
     ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2)); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
 
-        P->print(4,1,1,1);
+    P->print(4,1,1,1);
 
     // consume notifications
     SolverManagerDummyPtr SM = std::make_shared<SolverManagerDummy>(P);
@@ -350,8 +381,12 @@ TEST(Problem, Covariances)
 
     Eigen::Vector3d xs2d;
 
-    SensorBasePtr Sm = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root});
-    SensorBasePtr St = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d_other.yaml", {wolf_root});
+    SensorBasePtr Sm = P->installSensor("SensorOdom3d", 
+                                        wolf_root + "/test/yaml/sensor_odom_3d.yaml", 
+                                        {wolf_root});
+    SensorBasePtr St = P->installSensor("SensorOdom3d", 
+                                        wolf_root + "/test/yaml/sensor_odom_3d_other.yaml", 
+                                        {wolf_root});
 
     ParamsProcessorTrackerFeaturePtr params = std::make_shared<ParamsProcessorTrackerFeature>();
     params->time_tolerance            = 0.1;
@@ -359,12 +394,18 @@ TEST(Problem, Covariances)
     params->min_features_for_keyframe = 10;
 
 
-    auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", Sm->getName(), wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_root});
-    auto pm = P->installProcessor("ProcessorOdom3d", St->getName(), wolf_root + "/test/yaml/processor_odom_3d.yaml", {wolf_root});
+    auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", 
+                                  Sm->getName(), 
+                                  wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", 
+                                  {wolf_root});
+    auto pm = P->installProcessor("ProcessorOdom3d", 
+                                  St->getName(), 
+                                  wolf_root + "/test/yaml/processor_odom_3d.yaml", 
+                                  {wolf_root});
 
     // 4 state blocks, estimated
     St->unfixExtrinsics();
-    FrameBasePtr F = P->emplaceFrame(0, "PO", 3, xs3d );
+    FrameBasePtr F = P->emplaceFrame(0, "PO", xs3d );
 
     // set covariance (they are not computed without a solver)
     P->addCovarianceBlock(F->getP(), Eigen::Matrix3d::Identity());
@@ -391,16 +432,16 @@ TEST(Problem, perturb)
     param->ticks_per_wheel_revolution = 100;
     auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(),
                                                        param,
-                                                       SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())},
-                                                               {'O',SpecStateSensor("O",Vector1d::Zero())},
-                                                               {'I',SpecStateSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}));
+                                                       SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())},
+                                                                            {'O',SpecStateSensor("StateAngle",Vector1d::Zero())},
+                                                                            {'I',SpecStateSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}));
 
     Vector3d pose; pose << 0,0,0;
 
     int i = 0;
     for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
     {
-        auto F = problem->emplaceFrame(t, pose );
+        auto F = problem->emplaceFrame(t, "PO", pose );
         if (i==0) F->fix();
 
         for (int j = 0; j< 2 ; j++)
@@ -477,15 +518,15 @@ TEST(Problem, check)
     param->ticks_per_wheel_revolution = 100;
     auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(),
                                                        param,
-                                                       SpecSensorComposite({{'P',SpecStateSensor("P",Vector2d::Zero())},
-                                                               {'O',SpecStateSensor("O",Vector1d::Zero())},
-                                                               {'I',SpecStateSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}));
+                                                       SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d",Vector2d::Zero())},
+                                                                            {'O',SpecStateSensor("StateAngle",Vector1d::Zero())},
+                                                                            {'I',SpecStateSensor("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}));
     Vector3d pose; pose << 0,0,0;
 
     int i = 0;
     for (TimeStamp t = 0.0; t < 2.0; t += 1.0)
     {
-        auto F = problem->emplaceFrame(t, pose);
+        auto F = problem->emplaceFrame(t, "PO", pose);
         if (i==0) F->fix();
 
         for (int j = 0; j< 2 ; j++)
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 1c484638d4617173a7906a571cf865c2656f9d5c..859cd66471849d82fb9fcef97e1af2cbe90034a7 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -123,8 +123,8 @@ TEST(ProcessorBase, KeyFrameCallback)
 
     // initialize
     TimeStamp   t(0.0);
-    SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"factor",Vector2d(sqrt(0.1),sqrt(0.1)))},
-                        {'O',SpecState("O",Vector1d(0),"factor",Vector1d(sqrt(0.1)))}};
+    SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(sqrt(0.1),sqrt(0.1)))},
+                        {'O',SpecState("StateAngle",Vector1d(0),"factor",Vector1d(sqrt(0.1)))}};
     problem->setPrior(prior, t);
 
     CaptureOdom2dPtr capt_odo = make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5,0));
@@ -156,7 +156,9 @@ TEST(ProcessorBase, KeyFrameCallback)
         std::cout << "6\n";
 
         // Only odom creating KFs
-        ASSERT_TRUE( problem->getLastFrame()->getStructure().compare("PO")==0 );
+        ASSERT_EQ( problem->getLastFrame()->getKeys().size(),2);
+        ASSERT_TRUE(  problem->getLastFrame()->getKeys().find('P') != std::string::npos);
+        ASSERT_TRUE(  problem->getLastFrame()->getKeys().find('O') != std::string::npos);
     }
 }
 
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 443e4f1edd4e02ea85e849476c34cc7e8ada6423..be7edb760e57dc52efd41a7ba54663add1c6c576 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -178,8 +178,8 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
     // Prior
     TimeStamp t(0.0);
     SpecComposite prior;
-    prior.emplace('P',SpecState("P",Vector2d(0,0),"factor",Vector2d(1,1)));
-    prior.emplace('O',SpecState("O",Vector1d(0),"factor",Vector1d(1)));
+    prior.emplace('P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(1,1)));
+    prior.emplace('O',SpecState("StateAngle",Vector1d(0),"factor",Vector1d(1)));
     auto KF_0 = problem->setPrior(prior, t);
     processor->setOrigin(KF_0);
 
@@ -203,8 +203,8 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
 {
     // Prior
     TimeStamp t(0.0);
-    SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"fix")},
-                        {'O',SpecState("O",Vector1d(0),"fix")}};
+    SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"fix")},
+                        {'O',SpecState("StateAngle",Vector1d(0),"fix")}};
     auto KF_0 = problem->setPrior(prior, t);
     processor->setOrigin(KF_0);
 
@@ -251,8 +251,8 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
 {
     // Prior
     TimeStamp t(0.0);
-    SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"factor",Vector2d(1,1))},
-                        {'O',SpecState("O",Vector1d(0),"factor",Vector1d(1))}};
+    SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(1,1))},
+                        {'O',SpecState("StateAngle",Vector1d(0),"factor",Vector1d(1))}};
     auto KF_0 = problem->setPrior(prior, t);
     processor->setOrigin(KF_0);
 
@@ -277,8 +277,8 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
     // Prior
     TimeStamp t(0.0);
     SpecComposite prior;
-    prior.emplace('P',SpecState("P",Vector2d(0,0),"fix"));
-    prior.emplace('O',SpecState("O",Vector1d(0),"fix"));
+    prior.emplace('P',SpecState("StatePoint2d",Vector2d(0,0),"fix"));
+    prior.emplace('O',SpecState("StateAngle",Vector1d(0),"fix"));
     auto KF_0 = problem->setPrior(prior, t);
     processor->setOrigin(KF_0);
 
@@ -418,8 +418,8 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
 {
     // Prior
     TimeStamp t(0.0);
-    SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"factor",Vector2d(1,1))},
-                        {'O',SpecState("O",Vector1d(0),"factor",Vector1d(1))}};
+    SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"factor",Vector2d(1,1))},
+                        {'O',SpecState("StateAngle",Vector1d(0),"factor",Vector1d(1))}};
     auto KF_0 = problem->setPrior(prior, t);
     processor->setOrigin(KF_0);
 
@@ -460,8 +460,8 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
 {
     // Prior
     TimeStamp t(0.0);
-    SpecComposite prior{{'P',SpecState("P",Vector2d(0,0),"fix")},
-                        {'O',SpecState("O",Vector1d(0),"fix")}};
+    SpecComposite prior{{'P',SpecState("StatePoint2d",Vector2d(0,0),"fix")},
+                        {'O',SpecState("StateAngle",Vector1d(0),"fix")}};
     auto KF_0 = problem->setPrior(prior, t);
     processor->setOrigin(KF_0);
 
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 12b8d2a1a637260a8c7bb47f34fdc682a8a6e1eb..a679c61169dda05d9a53ff096a41f9ad0e009bc1 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -55,10 +55,10 @@ TEST(TrajectoryBase, ClosestKeyFrame)
 
     FrameBasePtr F1 = P->emplaceFrame(          1, Eigen::Vector3d::Zero() );
     FrameBasePtr F2 = P->emplaceFrame(          2, Eigen::Vector3d::Zero() );
-    FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameStructure(),
+    FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameKeys(),
                                                   //                                                              P->getDim(),
                                                   std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
-    FrameBasePtr F4 = std::make_shared<FrameBase>(4, P->getFrameStructure(),
+    FrameBasePtr F4 = std::make_shared<FrameBase>(4, P->getFrameKeys(),
                                                   //                                                              P->getDim(),
                                                   std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}) );
 
@@ -114,7 +114,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
     std::cout << __LINE__ << std::endl;
 
     // add F3
-    FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameStructure(),
+    FrameBasePtr F3 = std::make_shared<FrameBase>(3, P->getFrameKeys(),
                                                   //                                                              P->getDim(),
                                                   std::list<VectorXd>({Eigen::Vector2d::Zero(),Eigen::Vector1d::Zero()}));
     if (debug) P->print(2,0,0,0);
diff --git a/test/yaml/params_problem_autosetup.yaml b/test/yaml/params_problem_autosetup.yaml
index 7c445e2c32fe2de43f48b4f6e36636fa38f26329..0cf74a11958598bee8058f7d4d3117ade6e98a61 100644
--- a/test/yaml/params_problem_autosetup.yaml
+++ b/test/yaml/params_problem_autosetup.yaml
@@ -51,5 +51,5 @@ processors:
     
     unmeasured_perturbation_std: 0.001
     
-    state_getter: true
-    state_priority: 1
+    state_provider: true
+    state_provider_order: 1
diff --git a/test/yaml/params_problem_autosetup_no_map.yaml b/test/yaml/params_problem_autosetup_no_map.yaml
index c0713d0f39d04da8920000e38dfa434950b3e1d9..54d1d63c2725f4af948bc3e72d81782150b34414 100644
--- a/test/yaml/params_problem_autosetup_no_map.yaml
+++ b/test/yaml/params_problem_autosetup_no_map.yaml
@@ -48,5 +48,5 @@ processors:
     
     unmeasured_perturbation_std: 0.00111
     
-    state_getter: true
-    state_priority: 1
+    state_provider: true
+    state_provider_order: 1
diff --git a/test/yaml/params_problem_odom_3d.yaml b/test/yaml/params_problem_odom_3d.yaml
index e038298e42bb24516caff219fd8b3a80ea976ac3..e49632f45e08386f993aed3ec454b56ccad18e86 100644
--- a/test/yaml/params_problem_odom_3d.yaml
+++ b/test/yaml/params_problem_odom_3d.yaml
@@ -41,5 +41,5 @@ processors:
     
     unmeasured_perturbation_std: 0.00111
     
-    state_getter: true
+    state_provider: true
     state_priority: 1
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index f7a110bf8f5ec83efeb186fd4b934c5a5817047b..3e59567cb367b45df3146b97e216df14eef84772 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/test/yaml/params_tree_manager1.yaml
@@ -53,6 +53,6 @@ processors:
     
     unmeasured_perturbation_std: 0.00111
     
-    state_getter: true
+    state_provider: true
     state_priority: 1
     
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
index edb6b17b80eda799719190d7d1b2efd6cf4d82af..fb7b93ef57e41f82ba257142873d0334bb1c8430 100644
--- a/test/yaml/params_tree_manager2.yaml
+++ b/test/yaml/params_tree_manager2.yaml
@@ -53,6 +53,6 @@ processors:
     
     unmeasured_perturbation_std: 0.00111
     
-    state_getter: true
+    state_provider: true
     state_priority: 1
     
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index 22aea41299db11cc1a6ccc326067827d14e525ec..67d349e02ad526a9c71f015337d5685950006882 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -48,6 +48,6 @@ processors:
     
     unmeasured_perturbation_std: 0.00111
     
-    state_getter: true
+    state_provider: true
     state_priority: 1
     
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index 09288cfd12247f66f2cc2b3800380d87a9cbd4e0..01a6731f1577f8a46019313b54b4c62cf07ea96a 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -48,6 +48,6 @@ processors:
     
     unmeasured_perturbation_std: 0.00111
     
-    state_getter: true
+    state_provider: true
     state_priority: 1
     
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
index 0d725631260166ea8aebcc3b89d206638b6532ac..5ff6eff5eee764e306f3077014fb7e6902e5d221 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -56,6 +56,6 @@ processors:
     
     unmeasured_perturbation_std: 0.00111
     
-    state_getter: true
+    state_provider: true
     state_priority: 1
     
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
index 47e54b3ab04136e768c7479b18d3b09adfa592f8..288f3a9c23cb93f34b3103023a46323adc3c9f7e 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
@@ -50,5 +50,5 @@ processors:
     
     unmeasured_perturbation_std: 0.00111
     
-    state_getter: true
+    state_provider: true
     state_priority: 1
diff --git a/test/yaml/processor_odom_3d.yaml b/test/yaml/processor_odom_3d.yaml
index 93ec73af0a811d65cf893c7dc9e023333db8ccc7..34ab78f54be873ab3b0b2f28918aed64ff50c00a 100644
--- a/test/yaml/processor_odom_3d.yaml
+++ b/test/yaml/processor_odom_3d.yaml
@@ -13,5 +13,5 @@ unmeasured_perturbation_std: 0.001
 
 apply_loss_function: false
 
-state_getter: true
-state_priority: 1
\ No newline at end of file
+state_provider: true
+state_provider_order: 1
\ No newline at end of file