diff --git a/CMakeLists.txt b/CMakeLists.txt
index 832377f6711f059221d85374397950e7f51b3135..16f94577c6d285823875434038935bbcb9f1694a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -101,14 +101,9 @@ IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
   message(FATAL_ERROR "Bug: Specified CONFIG_DIR: "
     "${WOLF_CONFIG_DIR} exists, but is not a directory.")
 ENDIF()
+
 # Configure config.h
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-message(STATUS "WOLF CONFIG DIRECTORY ${WOLF_CONFIG_DIR}")
-message(STATUS "WOLF CONFIG FILE      ${WOLF_CONFIG_DIR}/config.h")
-include_directories("${PROJECT_BINARY_DIR}/conf")
-
-# ============ INCLUDES ============ 
-INCLUDE_DIRECTORIES("include") # In this same project
 
 # ============ HEADERS ============ 
 SET(HDRS_CAPTURE
@@ -215,6 +210,7 @@ SET(HDRS_STATE_BLOCK
   include/${PROJECT_NAME}/state_block/prior.h
   include/${PROJECT_NAME}/state_block/state_angle.h
   include/${PROJECT_NAME}/state_block/state_block.h
+  include/${PROJECT_NAME}/state_block/state_block_derived.h
   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
@@ -446,9 +442,9 @@ install(
   ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
 )
 
-# Specifies include directories to use when compiling the plugin target
-# This way, include_directories does not need to be called in plugins depending on this one
-target_include_directories(${PLUGIN_NAME} INTERFACE
+target_include_directories(${PLUGIN_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<BUILD_INTERFACE:${PROJECT_BINARY_DIR}/conf>
   $<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
 )
 
@@ -494,7 +490,7 @@ INSTALL(FILES ${HDRS_UTILS}
 INSTALL(FILES ${HDRS_YAML}
   DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/yaml)
 
-INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
+INSTALL(FILES ${WOLF_CONFIG_DIR}/config.h
   DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
 
 export(PACKAGE ${PLUGIN_NAME})
diff --git a/doc/doxygen.conf b/doc/doxygen.conf
index f0009526c6f9fcb8a61ae84916690a62190e32c6..29dd647045c69da17bfd41a11bd4d047216b2583 100644
--- a/doc/doxygen.conf
+++ b/doc/doxygen.conf
@@ -757,7 +757,8 @@ WARN_LOGFILE           =
 
 INPUT                  = ../doc/main.dox \
                          ../src \
-                         ../include/core
+                         ../include/core \
+                         conf/core/internal
 
 # This tag can be used to specify the character encoding of the source files
 # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index 0a42e1275ef5e96f505837774d0c75842c6e81cd..c73d042f6599a2e386a19b2d10fed02bf8dd3aa8 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -305,6 +305,7 @@ class Factory
     public:
         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);
         std::string getClass() const;
         static void printAddress();
@@ -356,6 +357,12 @@ inline bool Factory<TypeBase, TypeInput...>::unregisterCreator(const std::string
     return get().callbacks_.erase(_type) == 1;
 }
 
+template<class TypeBase, typename... TypeInput>
+inline bool Factory<TypeBase, TypeInput...>::isCreatorRegistered(const std::string& _type)
+{
+    return get().callbacks_.count(_type) == 1;
+}
+
 template<class TypeBase, typename... TypeInput>
 inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input)
 {
diff --git a/include/core/common/params_base.h b/include/core/common/params_base.h
index 14b8dcb18cce99dc793e89f66281b04e7c07eb2e..51825a75307752c7ee88447dd14008e435c5a9a1 100644
--- a/include/core/common/params_base.h
+++ b/include/core/common/params_base.h
@@ -23,7 +23,7 @@
 #define PARAMS_BASE_H_
 
 #include "core/utils/params_server.h"
-#include "yaml-schema-cpp/yaml-schema-cpp.hpp"
+#include "yaml-schema-cpp/yaml_server.hpp"
 
 namespace wolf {
   struct ParamsBase
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index 9eb2e5fd9b1c644742a7d32d055591231f10c7aa..7917935549619fbc58cc917e834d5ae735d90d41 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.h
@@ -108,7 +108,7 @@ inline void inverse(const MatrixBase<D1>& p, const QuaternionBase<D2>& q,
     MatrixSizeCheck<3, 1>::check(p);
     MatrixSizeCheck<3, 1>::check(ip);
 
-    ip = - q.conjugate() * p ;
+    ip = -(q.conjugate() * p) ;
     iq =   q.conjugate() ;
 }
 
@@ -135,6 +135,21 @@ inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
     return id;
 }
 
+inline void inverse(const VectorComposite& v, VectorComposite& c)
+{
+    Map<const Quaternion<double> > qv( & v.at('O')(0) );
+    Map<Quaternion<double> > qc( & c['O'](0) );
+    inverse(v.at('P'), qv, c['P'], qc);
+}
+
+inline VectorComposite inverse(const VectorComposite& v)
+{
+    VectorComposite c("PO", {3,4});
+    inverse(v, c);
+    return c;
+}
+
+
 template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
 inline void compose(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1,
                     const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2,
@@ -231,6 +246,13 @@ inline void compose(const VectorComposite& x1, const VectorComposite& x2, Vector
     compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']);
 }
 
+inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
+{
+    VectorComposite c("PO", {3,4});
+    compose(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), c['P'], c['O']);
+    return c;
+}
+
 inline void compose(const VectorComposite& x1,
                     const VectorComposite& x2,
                     VectorComposite& c,
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 147a04430dd803e62321b143c194cfa6ac7f0e50..7c551678e21f932c75f084cd770b0c35165e4e2d 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -87,7 +87,11 @@ class Problem : public std::enable_shared_from_this<Problem>
         StateStructure frame_structure_;
         PriorOptionsPtr prior_options_;
 
-    private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
+        std::atomic_bool transformed_;
+        VectorComposite  transformation_;
+        mutable std::mutex mut_transform_;
+
+      private:  // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
         Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
         void setup();
 
@@ -364,6 +368,10 @@ class Problem : public std::enable_shared_from_this<Problem>
         // All branches -------------------------------------------
         // perturb states
         void perturb(double amplitude = 0.01);
+        void transform(const VectorComposite& _transformation);
+        bool isTransformed() const;
+        void resetTransformed();
+        VectorComposite getTransformation() const;
 
         // Covariances
         void clearCovariance();
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 3e529411d32fff7d9a2adc907b4c1e5eaffef88f..62019c087088db640e09f86d846807ab6f84d8cb 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -98,8 +98,6 @@ static ProcessorBasePtr create(const YAML::Node& _input_node)
                                                                                         \
     auto processor  = std::make_shared<ProcessorClass>(params);                         \
                                                                                         \
-    processor       ->setName(_unique_name);                                            \
-                                                                                        \
     return processor;                                                                   \
 }                                                                                       \
 
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index be64fda51927defcaedfed31c7683a1be5b6f77f..e9b1597f96effd18b2ff273f00d7cc369eb98cfc 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -177,12 +177,13 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
             RUNNING_WITH_KF_AFTER_ORIGIN
         } ProcessingStep ;
 
-    protected:
+      protected:
         ParamsProcessorMotionPtr params_motion_;
-        ProcessingStep processing_step_;        ///< State machine controlling the processing step
+        ProcessingStep           processing_step_; ///< State machine controlling the processing step
+        bool                     bootstrapping_;   ///< processor is bootstrapping
 
-    // This is the main public interface
-    public:
+        // This is the main public interface
+      public:
         ProcessorMotion(const std::string& _type,
                         std::string _state_structure,
                         int _dim,
@@ -299,6 +300,11 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
          */
         virtual void preProcess(){ };
 
+        /**
+         * @brief Bootstrap the processor with some initialization steps
+         */
+        virtual void bootstrap(){};
+
         /** Post-process
          *
          * This is called by process() after finishing the processing algorithm.
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 2e778b40f0a2268e6ed9920a701fd832542bd793..605400d49e6a44b4acc85d264e1404c574c581c1 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -55,44 +55,62 @@ namespace wolf {
  *   SensorClass(const std::string& _unique_name, 
  *               SizeEigen _dim,
  *               ParamsSensorClassPtr _params, 
- *               const ParamsServer& _server)
- *
- *   SensorClass(const std::string& _unique_name, 
- *               SizeEigen _dim,
- *               ParamsSensorClassPtr _params, 
  *               const Priors& _priors)
+ * 
+ * Also, there should be the schema file 'SensorClass.schema' containing the specifications 
+ * of the user input yaml file.
  *
  */
-#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass)                              \
-static SensorBasePtr create(const std::string& _unique_name,                            \
-                            SizeEigen _dim,                                             \
-                            const ParamsServer& _server)                                \
-{                                                                                       \
-    auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server);           \
-                                                                                        \
-    return std::make_shared<SensorClass>(_unique_name, _dim, params, _server);          \
-}                                                                                       \
-static SensorBasePtr create(const std::string& _unique_name,                            \
-                            SizeEigen _dim,                                             \
-                            const std::string& _yaml_filepath)                          \
-{                                                                                       \
-    auto parser = ParserYaml(_yaml_filepath, true);                                     \
-                                                                                        \
-    auto server = ParamsServer(parser.getParams(), "sensor/" + _unique_name);           \
-                                                                                        \
-    auto params = std::make_shared<ParamsSensorClass>(_unique_name, server);            \
-                                                                                        \
-    return std::make_shared<SensorClass>(_unique_name, _dim, params, server);           \
-}                                                                                       \
-static SensorBasePtr create(const std::string& _unique_name,                            \
-                            SizeEigen _dim,                                             \
-                            ParamsSensorBasePtr _params,                                \
-                            const Priors& _priors)                                      \
-{                                                                                       \
-    auto params_derived = std::static_pointer_cast<ParamsSensorClass>(_params);         \
-                                                                                        \
-    return std::make_shared<SensorClass>(_unique_name, _dim, params_derived, _priors);  \
-}                                                                                       \
+#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass)                       \
+static SensorBasePtr create(SizeEigen _dim,                                      \
+                            const YAML::Node& _input_node,                       \
+                            const std::vector<std::string>& folders_schema)      \
+{                                                                                \
+    std::stringstream log;                                                       \
+    if (not checkNode(_input_node, SensorClass, folders_schema, log))            \
+    {                                                                            \
+        WOLF_ERROR(log.str());                                                   \
+        return nullptr;                                                          \
+    }                                                                            \
+                                                                                 \
+    auto params = std::make_shared<ParamsSensorClass>(_input_node);              \
+                                                                                 \
+    auto priors = Priors(_input_node["states"]);                                 \
+                                                                                 \
+    return std::make_shared<SensorClass>(_dim, params, priors);                  \
+}                                                                                \
+static SensorBasePtr create(SizeEigen _dim,                                      \
+                            const std::string& _yaml_filepath,                   \
+                            const std::vector<std::string>& folders_schema)      \
+{                                                                                \
+    auto schema = yaml-schema-cpp::YamlServer(folders_schema, _yaml_filepath);   \
+                                                                                 \
+    std::stringstream log;                                                       \
+    if (not schema.validate(SensorClass, log))                                   \
+    {                                                                            \
+        WOLF_ERROR(log.str());                                                   \
+        return nullptr;                                                          \
+    }                                                                            \
+    auto params = std::make_shared<ParamsSensorClass>(schema.getNode());         \
+                                                                                 \
+    auto priors = Priors(schema.getNode()["states"]);                            \
+                                                                                 \
+    return std::make_shared<SensorClass>(_dim, params, priors);                  \
+}                                                                                \
+static SensorBasePtr create(SizeEigen _dim,                                      \
+                            ParamsSensorBasePtr _params,                         \
+                            const Priors& _priors)                               \
+{                                                                                \
+    auto params_derived = std::dynamic_pointer_cast<ParamsSensorClass>(_params); \
+    if (not params_derived)                                                      \
+    {                                                                            \
+        WOLF_ERROR("In " #SensorClass " creator:",                               \
+                   " _params is not of type " #ParamsSensorClass "!");           \
+        return nullptr;                                                          \
+    }                                                                            \
+                                                                                 \
+    return std::make_shared<SensorClass>(_dim, params_derived, _priors);         \
+}                                                                                \
 
 /** \brief base struct for intrinsic sensor parameters
  *
@@ -101,10 +119,10 @@ static SensorBasePtr create(const std::string& _unique_name,
 WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorBase);
 struct ParamsSensorBase: public ParamsBase
 {
-    std::string prefix = "sensor/";
+    std::string name;
 
     ParamsSensorBase() = default;
-    ParamsSensorBase(std::string _unique_name, const wolf::ParamsServer& _server)
+    ParamsSensorBase(const YAML::Node& _input_node)
     {
     }
     ~ParamsSensorBase() override = default;
@@ -115,25 +133,6 @@ struct ParamsSensorBase: public ParamsBase
     }
 };
 
-struct SpecState
-{
-    std::string type;
-    SizeEigen size;
-    std::string doc;
-
-    SpecState(const std::string& _type, SizeEigen _size, const std::string _doc) : 
-        type(_type), size(_size), doc(_doc)
-    {
-        assert(not(_type == "StateQuaternion" and _size != 4));
-        assert(not(_type == "StateAngle" and _size != 1));
-        assert(not(_type == "O" and _size != 1 and _size != 4));
-        assert(not(_type == "P" and _size != 1 and _size != 4));
-        assert(not(_type == "V" and _size != 1 and _size != 4));
-    };
-};
-
-typedef std::unordered_map<char,SpecState> SpecStates;
-
 WOLF_PTR_TYPEDEFS(SensorBase);
 class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<SensorBase>
 {
@@ -160,7 +159,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         std::map<char, Eigen::MatrixXd> drift_covs_; // covariance of the drift of dynamic state blocks [unit²/s]
 
         void setProblem(ProblemPtr _problem) override final;
-        virtual void loadPriors(const Priors& _priors, SizeEigen _dim, const SpecStates& _state_specs);
+        virtual void loadPriors(const Priors& _priors, SizeEigen _dim);
 
     public:
 
@@ -172,34 +171,13 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
          * \param _dim Problem dimension
          * \param _params params struct
          * \param _priors priors of the sensor state blocks
-         * \param _state_specs_apart_from_PO Unordered map containing the keys and the specs for each of the state blocks (apart from extrinsics: P, O)
-         *
-         **/
-        SensorBase(const std::string& _type,
-                   const std::string& _unique_name,
-                   const SizeEigen& _dim,
-                   ParamsSensorBasePtr _params,
-                   const Priors& _priors,
-                   SpecStates _state_specs_apart_from_PO={});
-
-        /** \brief Constructor with ParamServer and keys
-         *
-         * Constructor with parameter vector
-         * TODO: update
-         * \param _tp Type of the sensor  (types defined at wolf.h)
-         * \param _unique_name Name of the sensor
-         * \param _dim Problem dimension
-         * \param _params params struct
-         * \param _server parameter server
-         * \param _state_specs_apart_from_PO Unordered map containing the keys and the specs for each of the state blocks (apart from extrinsics: P, O)
          *
          **/
         SensorBase(const std::string& _type,
                    const std::string& _unique_name,
                    const SizeEigen& _dim,
                    ParamsSensorBasePtr _params,
-                   const ParamsServer& _server,
-                   SpecStates _state_specs_apart_from_PO={});
+                   const Priors& _priors);
 
         ~SensorBase() override;
 
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index d8ea81ae0bbe3ec3fa4547961ab715eb810662f7..e25e7d908a18a7b0989e4b9c91bf87cb18229266 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -40,6 +40,8 @@ namespace wolf
 
 class HasStateBlocks
 {
+    friend Problem;
+
     public:
         HasStateBlocks();
         HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
@@ -95,11 +97,13 @@ class HasStateBlocks
         unsigned int getLocalSize(const StateStructure& _structure="") const;
 
         // Perturb state
-        void perturb(double amplitude = 0.01);
+        void perturb(double amplitude = 0.01);        
 
-    protected:
-        void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
+      protected:
+        // transform state
+        void transform(const VectorComposite& _transformation);
 
+        void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
 
     private:
         StateStructure structure_;
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index 1a307ce72310bcee0bf7d5b21b833aec8caf9340..fe0fb46473f64743b17ee96d47143a2b00492ac7 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -38,15 +38,17 @@ namespace wolf
 class StateAngle : public StateBlock
 {
     public:
-        StateAngle(double _angle = 0.0, bool _fixed = false);
+        StateAngle(double _angle = 0.0, bool _fixed = false, bool _transformable = true);
 
         ~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) :
-        StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>())
+inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) :
+        StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
 {
     state_(0) = pi2pi(_angle);
 }
@@ -64,6 +66,11 @@ inline StateBlockPtr StateAngle::create (const Eigen::VectorXd& _state, bool _fi
     return std::make_shared<StateAngle>(_state(0), _fixed);
 }
 
+inline void StateAngle::transform(const VectorComposite& _transformation)
+{
+    if (isTransformable()) setState(_transformation.at('O') + getState()); // 2D rotation is a sum of angles
+}
+
 } /* namespace wolf */
 
 #endif /* STATE_ANGLE_H_ */
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 8f4b0468b8d9fdc519e11137a8dc491330ec3ac7..2572cc9b64a729f36496ac403fb18700846dc43c 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -31,6 +31,7 @@ class LocalParametrizationBase;
 
 //Wolf includes
 #include "core/common/wolf.h"
+#include "core/state_block/state_composite.h"
 
 //std includes
 #include <iostream>
@@ -65,6 +66,8 @@ public:
         std::atomic_bool fix_updated_;          ///< Flag to indicate whether the status has been updated or not
         std::atomic_bool local_param_updated_;  ///< Flag to indicate whether the local_parameterization has been updated or not
 
+        bool transformable_;                    ///< Flag to indicate if the state block can be transformed to another reference frame
+
     public:
         /** \brief Constructor from size
          *
@@ -72,7 +75,7 @@ public:
          * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
          * \param _local_param_ptr pointer to the local parametrization for the block
          */
-        StateBlock(const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr);
+        StateBlock(const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
 
         /** \brief Constructor from vector
          * 
@@ -80,7 +83,7 @@ public:
          * \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
          * \param _local_param_ptr pointer to the local parametrization for the block
          **/
-        StateBlock(const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr);
+        StateBlock(const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
 
         ///< Explicitly not copyable/movable
         StateBlock(const StateBlock& o) = delete;
@@ -177,6 +180,11 @@ public:
          */
         void perturb(double amplitude = 0.1);
 
+        bool isTransformable() const {
+            return transformable_;
+        };  
+
+        virtual void transform(const VectorComposite& _transformation) { };
 
         void plus(const Eigen::VectorXd& _dv);
 
@@ -186,6 +194,7 @@ public:
 
 };
 
+
 } // namespace wolf
 
 // IMPLEMENTATION
@@ -195,31 +204,30 @@ public:
 
 namespace wolf {
 
-inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
-//        notifications_{Notification::ADD},
+inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
         fixed_(_fixed),
         state_size_(_state.size()),
         state_(_state),
         local_param_ptr_(_local_param_ptr),
         state_updated_(false),
         fix_updated_(false),
-        local_param_updated_(false)
+        local_param_updated_(false),
+        transformable_(_transformable)
 {
 //    std::cout << "constructed           +sb" << std::endl;
 }
 
-inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
-//        notifications_{Notification::ADD},
+inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
         fixed_(_fixed),
         state_size_(_size),
         state_(Eigen::VectorXd::Zero(_size)),
         local_param_ptr_(_local_param_ptr),
         state_updated_(false),
         fix_updated_(false),
-        local_param_updated_(false)
+        local_param_updated_(false),
+        transformable_(_transformable)
 {
-    //
-//    std::cout << "constructed           +sb" << std::endl;
+    //    std::cout << "constructed           +sb" << std::endl;
 }
 
 inline StateBlock::~StateBlock()
diff --git a/include/core/state_block/state_block_derived.h b/include/core/state_block/state_block_derived.h
new file mode 100644
index 0000000000000000000000000000000000000000..541b79a19453f34cedf13343de1caee2d2fca74b
--- /dev/null
+++ b/include/core/state_block/state_block_derived.h
@@ -0,0 +1,154 @@
+//--------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--------
+#ifndef STATE_BLOCK_DERIVED_H_
+#define STATE_BLOCK_DERIVED_H_
+
+#include "core/state_block/state_block.h"
+
+namespace wolf
+{
+
+/**
+ * @brief State block for general parameters
+ * 
+ * This state block cannot be transformed with a geometric frame transformation.
+ * 
+ * This state block cannot have a local parametrization
+ * 
+ * @tparam size the size of the state block's vector
+ */
+template <size_t size>
+class StateParams : public StateBlock
+{
+  public:
+    StateParams(const Eigen::VectorXd& _state, bool _fixed = false) : StateBlock(_state, _fixed, nullptr, false) {}
+    void transform(const VectorComposite& _transformation) override
+    {
+        // non transformable! --> do nothing
+    }
+};
+
+typedef StateParams<1>  StateParams1;
+typedef StateParams<2>  StateParams2;
+typedef StateParams<3>  StateParams3;
+typedef StateParams<4>  StateParams4;
+typedef StateParams<5>  StateParams5;
+typedef StateParams<6>  StateParams6;
+typedef StateParams<7>  StateParams7;
+typedef StateParams<8>  StateParams8;
+typedef StateParams<9>  StateParams9;
+typedef StateParams<10> StateParams10;
+
+/**
+ * @brief State block for general 2D points and positions
+ * 
+ * This state block can be transformed with a geometric frame transformation.
+ * This is controlled at construction time by parameter _transformable,
+ * or later via setTransformable(bool).
+ * 
+ * This state block cannot have a local parametrization
+ */
+class StatePoint2d : public StateBlock
+{
+  public:
+    StatePoint2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
+        : StateBlock(_state, _fixed, nullptr, _transformable)
+    {
+    }
+    void transform(const VectorComposite& _transformation) override
+    {
+        if (transformable_) setState(_transformation.at('P') + Rotation2Dd(_transformation.at('O')(0)) * getState());
+    }
+};
+
+/**
+ * @brief State block for general 2D vectors
+ * 
+ * This state block can be transformed with a geometric frame transformation.
+ * This is controlled at construction time by parameter _transformable,
+ * or later via setTransformable(bool).
+ * Being a vector, the geometric frame transformation will rotate the vector.
+ * 
+ * This state block cannot have a local parametrization
+ */
+class StateVector2d : public StateBlock
+{
+  public:
+    StateVector2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
+        : StateBlock(_state, _fixed, nullptr, _transformable)
+    {
+    }
+    void transform(const VectorComposite& _transformation) override
+    {
+        if (transformable_) setState(Rotation2Dd(_transformation.at('O')(0)) * getState());
+    }
+};
+
+/**
+ * @brief State block for general 3D points and positions
+ * 
+ * This state block can be transformed with a geometric frame transformation.
+ * This is controlled at construction time by parameter _transformable,
+ * or later via setTransformable(bool).
+ * 
+ * This state block cannot have a local parametrization
+ */
+class StatePoint3d : public StateBlock
+{
+  public:
+    StatePoint3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
+        : StateBlock(_state, _fixed, nullptr, _transformable)
+    {
+    }
+    void transform(const VectorComposite& _transformation) override
+    {
+        if (transformable_)
+            setState(_transformation.at('P') + Quaterniond(_transformation.at('O').data()) * getState());
+    }
+};
+
+/**
+ * @brief State block for general 3D vectors
+ * 
+ * This state block can be transformed with a geometric frame transformation.
+ * This is controlled at construction time by parameter _transformable,
+ * or later via setTransformable(bool).
+ * Being a vector, the geometric frame transformation will rotate the vector.
+ * 
+ * This state block cannot have a local parametrization
+ */
+class StateVector3d : public StateBlock
+{
+  public:
+    StateVector3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
+        : StateBlock(_state, _fixed, nullptr, _transformable)
+    {
+    }
+    void transform(const VectorComposite& _transformation) override
+    {
+        if (transformable_) setState(Quaterniond(_transformation.at('O').data()) * getState());
+    }
+};
+
+}  // namespace wolf
+
+#endif  // STATE_BLOCK_DERIVED_H_
\ No newline at end of file
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index e3569b5b4ed30159e0593de789b3b7a6c3644387..dcfa71155597334de8ff4a3daedbb760314f1856 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -246,7 +246,7 @@ class MatrixComposite : public std::unordered_map < char, std::unordered_map < c
          *   S["V"]["V"] S["V"]["W"]
          *   S["W"]["V"] S["W"]["W"]
          */
-        MatrixComposite propagate(const MatrixComposite & _Cov); // This performs Jac * this * Jac.tr
+        MatrixComposite propagate(const MatrixComposite & _Cov); // This performs this * _Cov * this.tr
 
         /**
          * \brief Matrix-vector product
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index 6ee8a85c28725805ff5a28fb2149950f7cb8e132..8af6059aa6badcfc91665307e92df8fd9379694b 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -37,25 +37,27 @@ namespace wolf {
 class StateHomogeneous3d : public StateBlock
 {
     public:
-        StateHomogeneous3d(bool _fixed = false);
-        StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false);
+        StateHomogeneous3d(bool _fixed = false, bool _transformable = true);
+        StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false, bool _transformable = true);
         ~StateHomogeneous3d() override;
 
         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) :
-        StateBlock(_state, _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.");
     local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
 }
 
-inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed) :
-        StateBlock(4, _fixed)
+inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed, bool _transformable) :
+        StateBlock(4, _fixed, nullptr, _transformable)
 {
     local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
     state_ << 0, 0, 0, 1; // Set origin
@@ -77,6 +79,12 @@ inline Eigen::VectorXd StateHomogeneous3d::identity() const
     return Eigen::Quaterniond::Identity().coeffs();
 }
 
+inline void StateHomogeneous3d::transform(const VectorComposite& _transformation)
+{
+    if (transformable_)
+        setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs()); // TODO is this correct?????? probably not!!!
+}
+
 inline StateBlockPtr StateHomogeneous3d::create (const Eigen::VectorXd& _state, bool _fixed)
 {
     MatrixSizeCheck<4,1>::check(_state);
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index d5278e1a6f4e3580c22056db7cc2c9ef073c21fb..ae7ed2f6381e9ce1149bff585df744d71e74b59e 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -37,19 +37,21 @@ namespace wolf {
 class StateQuaternion : public StateBlock
 {
     public:
-        StateQuaternion(bool _fixed = false);
-        StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false);
-        StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false);
+        StateQuaternion(bool _fixed = false, bool _transformable = true);
+        StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true);
+        StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false, bool _transformable = true);
         ~StateQuaternion() override;
 
         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) :
-        StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
+inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable) :
+        StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
 {
     // TODO: remove these lines after issue #381 is addressed
     if(not isValid(1e-5))
@@ -58,8 +60,8 @@ inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, b
     state_.normalize();
 }
 
-inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed) :
-        StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
+inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed, bool _transformable) :
+        StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
 {
     if(state_.size() != 4)
         throw std::runtime_error("The quaternion state vector must be of size 4!");
@@ -71,8 +73,8 @@ inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fix
     state_.normalize();
 }
 
-inline StateQuaternion::StateQuaternion(bool _fixed) :
-        StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
+inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable) :
+        StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
 {
     state_ = Eigen::Quaterniond::Identity().coeffs();
     //
@@ -94,6 +96,12 @@ inline Eigen::VectorXd StateQuaternion::identity() const
     return Eigen::Quaterniond::Identity().coeffs();
 }
 
+inline void StateQuaternion::transform(const VectorComposite& _transformation)
+{
+    if (transformable_)
+        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);
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 921fe7896652ba7b3f3eec865136a6c6dda05237..266c0ef1925bf90725d317eb9eb17a4ccb1b3299 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -27,7 +27,7 @@
 #include "core/state_block/state_angle.h"
 #include "core/state_block/state_quaternion.h"
 #include "core/common/factory.h"
-// #include "core/yaml/yaml_conversion.h"
+#include "yaml-schema-cpp/yaml_conversion.hpp"
 
 namespace wolf {
 
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 59299e8bd8b7665d0cbd2b266135fb0d755bf73c..70e80e1ce766a5fee328f1a9ffeab3dd5ddc9d91 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -44,7 +44,8 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr
         map_ptr_(_map),
         motion_provider_map_(),
         frame_structure_(_frame_structure),
-        prior_options_(std::make_shared<PriorOptions>())
+        prior_options_(std::make_shared<PriorOptions>()),
+        transformed_(false)
 {
     dim_ = _dim;
     if (_frame_structure == "PO" and _dim == 2)
@@ -134,20 +135,35 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
         loaders.push_back(l);
     }
 
-    // Install sensors and processors
+    // Install sensors and processors -- load plugins only if sensors and processor are not registered
     auto sensors        = _server.getParam<std::vector<std::map<std::string, std::string>>>("sensors");
-    for(auto sen : sensors)
-        problem->installSensor(sen["type"],
-                               sen["name"],
-                               _server);
-
-    auto processors     = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors");
-    for(auto prc : processors)
-        problem->installProcessor(prc["type"],
-                                  prc["name"],
-                                  prc["sensor_name"],
-                                  _server);
-
+    for (auto sen : sensors)
+    {
+        if (not FactorySensor::isCreatorRegistered(sen["type"]))
+        {
+            auto        plugin_name = sen["plugin"];
+            std::string plugin      = plugins_path + "libwolf" + plugin_name + lib_extension;
+            WOLF_TRACE("Loading plugin " + plugin_name + " via " + plugin);
+            auto l = std::make_shared<LoaderRaw>(plugin);
+            l->load();
+            loaders.push_back(l);
+        }
+        problem->installSensor(sen["type"], sen["name"], _server);
+    }
+    auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors");
+    for (auto prc : processors)
+    {
+        if (not FactoryProcessor::isCreatorRegistered(prc["type"]))
+        {
+            auto        plugin_name = prc["plugin"];
+            std::string plugin      = plugins_path + "libwolf" + plugin_name + lib_extension;
+            WOLF_TRACE("Loading plugin " + plugin_name + " via " + plugin);
+            auto l = std::make_shared<LoaderRaw>(plugin);
+            l->load();
+            loaders.push_back(l);
+        }
+        problem->installProcessor(prc["type"], prc["name"], prc["sensor_name"], _server);
+    }
 
     // Map (optional)
     std::string map_type, map_plugin;
@@ -1262,4 +1278,42 @@ void Problem::perturb(double amplitude)
         L->perturb(amplitude);
 }
 
+void Problem::transform(const VectorComposite& _transformation)
+{
+    std::lock_guard<std::mutex> lock_transform (mut_transform_); // Protect especially from SolverManager::update()
+
+    transformation_ = _transformation;
+    transformed_.store(true);
+
+    // Sensors
+    for (auto S : getHardware()->getSensorList())
+        S->transform(_transformation);
+
+    // Frames and Captures
+    for (auto F : getTrajectory()->getFrameMap())
+    {
+        F.second->transform(_transformation);
+        for (auto& C : F.second->getCaptureList())
+            C->transform(_transformation);
+    }
+
+    // Landmarks
+    for (auto L : getMap()->getLandmarkList())
+        L->transform(_transformation);
+}
+
+bool Problem::isTransformed() const
+{
+    return transformed_.load();
+}
+void Problem::resetTransformed()
+{
+    transformed_.store(false);
+}
+VectorComposite Problem::getTransformation() const
+{
+    std::lock_guard<std::mutex> lock(mut_transform_);
+    return transformation_;
+}
+
 } // namespace wolf
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 31b2b770afc2e1c7ac80adc9d793cb24648960b7..a934fe6b741722c6931b3cfb698e36da7c1c69ae 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -47,6 +47,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
         MotionProvider(_state_structure, _params_motion),
         params_motion_(_params_motion),
         processing_step_(RUNNING_WITHOUT_KF),
+        bootstrapping_(false),
         x_size_(_state_size),
         data_size_(_data_size),
         delta_size_(_delta_size),
@@ -202,19 +203,19 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         }
         case FIRST_TIME_WITH_KF_BEFORE_INCOMING :
         {
-            // cannot joint to the KF: create own origin
+            // cannot join to the KF: create own origin
             setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
             break;
         }
         case FIRST_TIME_WITH_KF_ON_INCOMING :
         {
-            // can joint to the KF
+            // can join to the KF
             setOrigin(keyframe_from_callback);
             break;
         }
         case FIRST_TIME_WITH_KF_AFTER_INCOMING :
         {
-            // cannot joint to the KF: create own origin
+            // cannot join to the KF: create own origin
             setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
             break;
         }
@@ -231,8 +232,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
     // Done at this place because setPrior() needs 
     integrateOneStep();
 
+    // perform bootstrap steps (usually only IMU requires this)
+    if (bootstrapping_) bootstrap();
 
-    switch(processing_step_)
+    switch (processing_step_)
     {
         case RUNNING_WITH_KF_BEFORE_ORIGIN :
         {
@@ -1066,7 +1069,7 @@ void ProcessorMotion::printHeader(int _depth, bool _constr_by, bool _metric, boo
 {
     _stream << _tabs << "PrcM" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
     if (getOrigin())
-        _stream << _tabs << "  " << "o: Cap" << getOrigin()->id() << " - " << "  Frm"
+        _stream << _tabs << "  " << "o: Cap" << getOrigin()->id() << " - " << " Frm"
                 << getOrigin()->getFrame()->id() << std::endl;
     if (getLast())
         _stream << _tabs << "  " << "l: Cap" << getLast()->id() << " - " << " Frm"
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index ab2e921d8ca025fbbc205eb3f162ac16e3b8e2a8..8f61345d8622d3417eef1bc2eac870477aa6290b 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -33,17 +33,11 @@ namespace wolf {
 
 unsigned int SensorBase::sensor_id_count_ = 0;
 
-SpecState spec_p_2d = SpecState("StateBlock",     2,"Sensor extrinsics in 2D: position (x, y) of the sensor in robot coordinates [m].");
-SpecState spec_p_3d = SpecState("StateBlock",     3,"Sensor extrinsics in 3D: position (x, y, z) of the sensor in robot coordinates [m].");
-SpecState spec_o_2d = SpecState("StateAngle",     1,"Sensor extrinsics in 2D: orientation (yaw) of the sensor in robot coordinates [rad].");
-SpecState spec_o_3d = SpecState("StateQuaternion",4,"Sensor extrinsics in 3D: orientation (quaternion) of the sensor in robot coordinates.");
-
 SensorBase::SensorBase(const std::string& _type,
                        const std::string& _unique_name,
                        const SizeEigen& _dim,
                        ParamsSensorBasePtr _params,
-                       const Priors& _priors,
-                       SpecStates _state_specs) :
+                       const Priors& _priors) :
         NodeBase("SENSOR", _type, _unique_name),
         HasStateBlocks("PO"),
         hardware_ptr_(),
@@ -59,50 +53,8 @@ SensorBase::SensorBase(const std::string& _type,
     if (not params_)
         throw std::runtime_error("SensorBase: in constructor, ParamsSensorBase pointer is null");
 
-    // append default P and O specs to _state_specs (if missing)
-    if (_state_specs.count('P') == 0)
-        _state_specs.emplace('P',_dim == 2 ? spec_p_2d : spec_p_3d);
-    if (_state_specs.count('O') == 0)
-        _state_specs.emplace('O', _dim == 2 ? spec_o_2d : spec_o_3d);
-
-    // load priors
-    loadPriors(_priors, _dim, _state_specs);
-}
-
-SensorBase::SensorBase(const std::string& _type,
-                       const std::string& _unique_name,
-                       const SizeEigen& _dim,
-                       ParamsSensorBasePtr _params,
-                       const ParamsServer& _server,
-                       SpecStates _state_specs) :
-        NodeBase("SENSOR", _type, _unique_name),
-        HasStateBlocks("PO"),
-        hardware_ptr_(),
-        sensor_id_(++sensor_id_count_), // simple ID factory
-        params_(_params),
-        state_block_dynamic_(),
-        params_prior_map_(),
-        last_capture_(nullptr)
-{
-    assert(_dim == 2 or _dim == 3);
-
-    // check params valid
-    if (not params_)
-        throw std::runtime_error("SensorBase: in constructor, ParamsSensorBase pointer is null");
-        
-    // append default P and O specs to _state_specs (if missing)
-    if (_state_specs.count('P') == 0)
-        _state_specs.emplace('P',_dim == 2 ? spec_p_2d : spec_p_3d);
-    if (_state_specs.count('O') == 0)
-        _state_specs.emplace('O', _dim == 2 ? spec_o_2d : spec_o_3d);
-    
-    // create priors
-    Priors priors;
-    for (auto pair : _state_specs)
-        priors[pair.first] = Prior("sensor/" + _unique_name + "/states/" + std::string(1, pair.first), pair.second.type, _server);
-
     // load priors
-    loadPriors(priors, _dim, _state_specs);
+    loadPriors(_priors, _dim);
 }
 
 SensorBase::~SensorBase()
@@ -112,64 +64,26 @@ SensorBase::~SensorBase()
 }
 
 void SensorBase::loadPriors(const Priors& _priors, 
-                            SizeEigen _dim,
-                            const SpecStates& _state_specs)
+                            SizeEigen _dim)
 {
     assert(_dim == 2 or _dim == 3);
 
-    // check not specified priors
-    for (auto&& prior_pair : _priors)
+    // Iterate all keys in _priors
+    for (auto state_pair : _priors)
     {
-        if (_state_specs.count(prior_pair.first) == 0)
-            throw std::runtime_error("SensorBase::loadPriors: Given a prior for key " + std::string(1,prior_pair.first) + " that is not required");
-    }
+        auto key = state_pair.first;
+        auto prior = state_pair.second;
 
-    // Iterate all keys in _state_specs
-    for (auto state_spec_pair : _state_specs)
-    {
-        auto key = state_spec_pair.first;
-        auto state_spec = state_spec_pair.second;
-
-        // --- CHECKS ---
-        // Existence
-        if (_priors.count(key) == 0)
-            throw std::runtime_error("SensorBase: No prior found for key '"
-                                     + std::string(1,key) + 
-                                     "' - should have type: " + state_spec.type +
-                                     "\nDoc:\n" + state_spec.doc);
-
-        const Prior& prior = _priors.at(key);
-        
         // type
-        if (key != 'P' and key != 'O' and key != 'V')
-        {
-            if (prior.getType() != state_spec.type)
-                throw std::runtime_error("SensorBase: The provided prior for key '" + 
-                                        std::string(1,key) + "' has wrong type: " + 
-                                        prior.getType() +
-                                        " and should be: " + state_spec.type +
-                                        "\nDoc:\n" + state_spec.doc);
-        }
-        else
-        {
-            if (key == 'P' and prior.getType() != "P" and prior.getType() != "StateBlock")
-                throw std::runtime_error("Prior type for key P has to be 'P' or 'StateBlock'");
-            if (key == 'V' and prior.getType() != "V" and prior.getType() != "StateBlock")
-                throw std::runtime_error("Prior type for key V has to be 'V' or 'StateBlock'");
-            if (key == 'O' and _dim == 2 and prior.getType() != "O" and prior.getType() != "StateAngle")
-                throw std::runtime_error("Prior type for key O in 2D has to be 'O' or 'StateAngle'");
-            if (key == 'O' and _dim == 3 and prior.getType() != "O" and prior.getType() != "StateQuaternion")
-                throw std::runtime_error("Prior type for key O in 3D has to be 'O' or 'StateQuaternion'");
-        }
+        if (key == 'P' and prior.getType() != "P" and prior.getType() != "StateBlock")
+            throw std::runtime_error("Prior type for key P has to be 'P' or 'StateBlock'");
+        if (key == 'V' and prior.getType() != "V" and prior.getType() != "StateBlock")
+            throw std::runtime_error("Prior type for key V has to be 'V' or 'StateBlock'");
+        if (key == 'O' and _dim == 2 and prior.getType() != "O" and prior.getType() != "StateAngle")
+            throw std::runtime_error("Prior type for key O in 2D has to be 'O' or 'StateAngle'");
+        if (key == 'O' and _dim == 3 and prior.getType() != "O" and prior.getType() != "StateQuaternion")
+            throw std::runtime_error("Prior type for key O in 3D has to be 'O' or 'StateQuaternion'");
         
-        // size
-        if (_priors.at(key).getState().size() != state_spec.size)
-            throw std::runtime_error("SensorBase: In constructor, provided '_priors' for key '" + 
-                                     std::string(1,key) + "' has wrong size: " + 
-                                     toString(prior.getState()) +
-                                     " and should be of size: " + toString(state_spec.size) +
-                                     "\nDoc:\n" + state_spec.doc);
-
         // --- CREATE STATE BLOCK ---
         auto sb = FactoryStateBlock::create(prior.getType(), prior.getState(), prior.isFixed());
 
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index c9d192a536151f0340a7fd1f8f9f5e88acf54c26..a9d3ee25ad0cc1e5d7c2a8991c221835fbcd4334 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -57,6 +57,9 @@ SolverManager::~SolverManager()
 
 void SolverManager::update()
 {
+    // lock mutex to prevent Problem::transform() during this update()
+    std::lock_guard<std::mutex> lock_transform (wolf_problem_->mut_transform_);
+
     // Consume notification maps
     std::map<StateBlockPtr,Notification> sb_notification_map;
     std::map<FactorBasePtr,Notification> fac_notification_map;
@@ -147,8 +150,9 @@ void SolverManager::update()
         state_ptr->resetFixUpdated();
         state_ptr->resetLocalParamUpdated();
     }
+    wolf_problem_->resetTransformed();
 
-    #ifdef _WOLF_DEBUG
+#ifdef _WOLF_DEBUG
         assert(check("after update()"));
     #endif
 }
@@ -188,7 +192,15 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
     {
         // Avoid usuless copies
         if (!stateblock_statevector.first->isFixed())
-            stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_
+        {
+            stateblock_statevector.first->setState(stateblock_statevector.second,
+                                                   false);  // false = do not raise the flag state_updated_
+
+            // Transform the whole set of state blocks if necessary
+            // (this may happen when Problem::transform() has been called during the execution of this solve())
+            if (wolf_problem_->isTransformed())
+                stateblock_statevector.first->transform(wolf_problem_->getTransformation());
+        }
     }
     auto duration_state = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_state);
     acc_duration_state_ += duration_state;
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index d152ebcc0b0361d6a222e5679b863edcb1621e2c..7e399e7d48e1af44d75c743c9c8168c5c748b8fc 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -91,6 +91,13 @@ void HasStateBlocks::perturb(double amplitude)
     }
 }
 
+void HasStateBlocks::transform(const VectorComposite& _transformation)
+{
+    for (auto& pair_key_sb : state_block_map_)
+        pair_key_sb.second->transform(_transformation);
+}
+
+
 void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
 {
     if (_metric && _state_blocks)
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
index 249dabd38183def44e8339129405fcc417e2ec98..2aa43047fe569b9c35a23d164ff812e78fa9a60d 100644
--- a/src/utils/params_server.cpp
+++ b/src/utils/params_server.cpp
@@ -108,5 +108,4 @@ std::string toString(long double _arg)
     return std::to_string(_arg);
 }
 
-
-}
\ No newline at end of file
+}
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index ff1ddb6d1ab166f9ee11c2dbc5821669c5114e78..5d9df91f1f67018ef774c3f3ee1eea2833f34a0f 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -103,6 +103,56 @@ TEST(SE3, log_of_power)
     ASSERT_MATRIX_APPROX(tau3, log(pose3), 1e-8);
 }
 
+TEST(SE3, inverse)
+{
+    Vector3d p; p.setRandom();
+    Vector4d qvec; qvec.setRandom().normalized();
+    Quaterniond q(qvec);
+
+    Vector3d pi_true = -(q.conjugate() * p);
+    Quaterniond qi_true = q.conjugate();
+
+    // separate API
+    Vector3d pi_out;
+    Quaterniond qi_out;
+
+    inverse(p, q, pi_out, qi_out);
+    ASSERT_MATRIX_APPROX(pi_out, pi_true, 1e-8);
+    ASSERT_MATRIX_APPROX(qi_out.coeffs(), qi_true.coeffs(), 1e-8);
+
+    // Vector7d API
+    Vector7d pose; pose << p, q.coeffs();
+    Vector7d posei_true; posei_true << pi_true, qi_true.coeffs();
+    Vector7d posei_out;
+    inverse(pose, posei_out);
+    ASSERT_MATRIX_APPROX(posei_out, posei_true, 1e-8);
+
+    posei_out = inverse(pose);
+    ASSERT_MATRIX_APPROX(posei_out, posei_true, 1e-8);
+}
+
+TEST(SE3, inverseComposite)
+{
+    Vector3d p; p.setRandom();
+    Vector4d qvec; qvec.setRandom().normalized();
+    VectorComposite pose_vc("PO", {p, qvec});
+    Quaterniond q(qvec);
+
+    // ground truth
+    Vector3d pi_true = -(q.conjugate() * p);
+    Quaterniond qi_true = q.conjugate();
+
+    VectorComposite pose_vc_out("PO", {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);
+
+    VectorComposite pose_vc_out_bis = inverse(pose_vc);
+    ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('P'), pi_true, 1e-8);
+    ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('O'), qi_true.coeffs(), 1e-8);
+
+}
+
 TEST(SE3, composeBlocks)
 {
     Vector3d p1, p2, pc;
@@ -176,6 +226,10 @@ TEST(SE3, composeVectorComposite)
 
     ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8);
     ASSERT_QUATERNION_VECTOR_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
+
+    VectorComposite vc = compose(x1, x2);
+    ASSERT_MATRIX_APPROX(vc.at('P'), pc, 1e-8);
+    ASSERT_QUATERNION_VECTOR_APPROX(vc.at('O'), qc.coeffs(), 1e-8);
 }
 
 TEST(SE3, composeVectorComposite_Jacobians)
@@ -397,6 +451,7 @@ TEST(SE3, interpolate_third)
     ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
 }
 
+
 TEST(SE3, toSE3_I)
 {
     Vector7d pose; pose << 0,0,0, 0,0,0,1;
diff --git a/test/gtest_factory.cpp b/test/gtest_factory.cpp
index 6527cfd9bd95798cc033948d64b3878c43ce4d81..9e891659916011d79b4b719de92f6a57b3075850 100644
--- a/test/gtest_factory.cpp
+++ b/test/gtest_factory.cpp
@@ -44,6 +44,19 @@ TEST(TestFactory, DummyObjectFactory)
   DummyObjectDerived obj_derived = DummyObjectDerived("AnotherCoolDummyObject", server);
 }
 
+TEST(TestFactory, isCreatorRegistered)
+{
+    ParserYaml   parser = ParserYaml("test/yaml/params_basic.yaml", wolf_root);
+    ParamsServer server = ParamsServer(parser.getParams());
+
+    bool object_creator_registered = FactoryDummyObject::isCreatorRegistered("DummyObjectDerived");
+
+    ASSERT_TRUE(object_creator_registered);
+
+    // FORCE LOADING
+    DummyObjectDerived obj_derived = DummyObjectDerived("AnotherCoolDummyObject", server);
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index bdc5e696b1e4a012abdee9fc93b16bd647b85172..7ec49fdee46c118d8d7907e17a43f5a2c598e661 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -813,6 +813,6 @@ int main(int argc, char **argv)
      */
 
     testing::InitGoogleTest(&argc, argv);
-    ::testing::GTEST_FLAG(filter) = "Conversions.R2q_norm_of_q";
+    // ::testing::GTEST_FLAG(filter) = "Conversions.R2q_norm_of_q";
     return RUN_ALL_TESTS();
 }
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
index 9bce46a4856e123124e3b62aed99c4e2a09a7b01..e1696bcc92f15f5f87a1aeed24fbed554e8cdbab 100644
--- a/test/gtest_state_block.cpp
+++ b/test/gtest_state_block.cpp
@@ -28,21 +28,21 @@
 
 #include "core/utils/utils_gtest.h"
 
-
 #include "core/state_block/state_block.h"
 #include "core/state_block/state_quaternion.h"
 #include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_composite.h"
 
 #include <iostream>
 
-
 using namespace Eigen;
 using namespace std;
 using namespace wolf;
 
 TEST(StateBlock, block_perturb)
 {
-    Vector3d x(10,20,30);
+    Vector3d   x(10, 20, 30);
     StateBlock sb(x);
 
     sb.perturb(0.5);
@@ -50,12 +50,12 @@ TEST(StateBlock, block_perturb)
     WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
 
     ASSERT_NE(x.norm(), sb.getState().norm());
-    ASSERT_MATRIX_APPROX(x , sb.getState() , 0.5 * 4); // 4-sigma test...
+    ASSERT_MATRIX_APPROX(x, sb.getState(), 0.5 * 4);  // 4-sigma test...
 }
 
 TEST(StateBlock, angle_perturb)
 {
-    Vector1d x(1.0);
+    Vector1d   x(1.0);
     StateAngle sb(x(0));
 
     sb.perturb(0.1);
@@ -63,28 +63,174 @@ TEST(StateBlock, angle_perturb)
     WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
 
     ASSERT_NE(x.norm(), sb.getState().norm());
-    ASSERT_MATRIX_APPROX(x , sb.getState() , 0.1 * 4); // 4-sigma test...
+    ASSERT_MATRIX_APPROX(x, sb.getState(), 0.1 * 4);  // 4-sigma test...
 }
 
 TEST(StateBlock, quaternion_perturb)
 {
-    Vector4d x(0.5,0.5,0.5,0.5);
+    Vector4d        x(0.5, 0.5, 0.5, 0.5);
     StateQuaternion sb(x);
 
     sb.perturb(0.01);
 
     WOLF_INFO("original ", x.transpose(), ", perturbed = ", sb.getState().transpose());
 
-    ASSERT_LT((sb.getState().transpose() * x).norm() , 1.0); // quaternions are not parallel ==> not equal
-    ASSERT_MATRIX_APPROX(x , sb.getState() , 0.01 * 4); // 4-sigma test...
+    ASSERT_LT((sb.getState().transpose() * x).norm(), 1.0);  // quaternions are not parallel ==> not equal
+    ASSERT_MATRIX_APPROX(x, sb.getState(), 0.01 * 4);        // 4-sigma test...
 }
 
-int main(int argc, char **argv)
+TEST(StatePoint3d, transformable)
+{
+    Vector3d x(1, 2, 3);
+
+    // default is transformable
+    StatePoint3d sb_tr(x);
+    ASSERT_TRUE(sb_tr.isTransformable());
+
+    // not fixed, non transformable
+    StatePoint3d sb_ntr(x, false, false);
+    ASSERT_FALSE(sb_ntr.isTransformable());
+
+    // not fixed, transformable
+    StatePoint3d sb_tr2(x, false, true);
+    ASSERT_TRUE(sb_tr2.isTransformable());
+}
+
+TEST(StatePoint3d, transform_identity)
+{
+    Vector3d     x0(1, 2, 3);
+    StatePoint3d sb(x0);
+
+    Vector7d xtrf;
+    xtrf << 0, 0, 0, 0, 0, 0, 1;
+    VectorComposite trf(xtrf, "PO", {3, 4});
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState(), x0, 1e-15);
+}
+
+TEST(StatePoint3d, transform_translation_4x5y6z)
+{
+    Vector3d     x0(1, 2, 3);
+    StatePoint3d sb(x0);
+
+    Vector7d xtrf;
+    xtrf << 4, 5, 6, 0, 0, 0, 1;
+    VectorComposite trf(xtrf, "PO", {3, 4});
+
+    Vector3d x(5, 7, 9);  // x0 translated (4,5,6)
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StatePoint3d, transform_rotation_90x)
 {
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
+    Vector3d     x0(1, 2, 3);
+    StatePoint3d sb(x0);
+
+    Vector7d xtrf;
+    xtrf << 0, 0, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+    VectorComposite trf(xtrf, "PO", {3, 4});
+
+    Vector3d x(1, -3, 2);  // x0 rotated (90,0,0)
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
 }
 
+TEST(StatePoint3d, transform_translation_1y_rotation_90x)
+{
+    Vector3d     x0(1, 2, 3);
+    StatePoint3d sb(x0);
+
+    Vector7d xtrf;
+    xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+    VectorComposite trf(xtrf, "PO", {3, 4});
+
+    Vector3d x(1, -2, 2);  // x0 translated (0,1,0) and rotated (90,0,0)
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StatePoint3d, non_transformable_translation_rotation)
+{
+    Vector3d     x0(1, 2, 3);
+    StatePoint3d sb(x0, false, false);  // non transformable
 
+    Vector7d xtrf;
+    xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+    VectorComposite trf(xtrf, "PO", {3, 4});
 
+    Vector3d x(1, 2, 3);  // x0 NOT translated (0,1,0) and NOT rotated (90,0,0)
 
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StateVector3d, transform_translation_1y_rotation_90x)
+{
+    Vector3d      x0(1, 2, 3);
+    StateVector3d sb(x0);
+
+    Vector7d xtrf;
+    xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+    VectorComposite trf(xtrf, "PO", {3, 4});
+
+    Vector3d x(1, -3, 2);  // x0 NOT translated (0,1,0) and rotated (90,0,0)
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StatePoint2d, transform_translation_1y_rotation_90)
+{
+    Vector2d     x0(1, 2);
+    StatePoint2d sb(x0);
+
+    Vector3d xtrf;
+    xtrf << 0, 1, M_PI / 2;
+    VectorComposite trf(xtrf, "PO", {2, 1});
+
+    Vector2d x(-2, 2);  // x0 translated (0,1) and rotated 90 deg
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StateVector2d, transform_translation_1y_rotation_90)
+{
+    Vector2d      x0(1, 2);
+    StateVector2d sb(x0);
+
+    Vector3d xtrf;
+    xtrf << 0, 1, M_PI / 2;
+    VectorComposite trf(xtrf, "PO", {2, 1});
+
+    Vector2d x(-2, 1);  // x0 NON translated (0,1) and rotated 90 deg
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+TEST(StateParams4, transform_translation_1y_rotation_90x)
+{
+    Vector4d       x0(1, 2, 3, 4);
+    StateParams<4> sb(x0);
+
+    Vector7d xtrf;
+    xtrf << 0, 1, 0, sqrt(2) / 2, 0, 0, sqrt(2) / 2;
+    VectorComposite trf(xtrf, "PO", {3, 4});
+
+    Vector4d x(1, 2, 3, 4);  // x0 NOT translated (0,1,0) and NOT rotated (90,0,0)
+
+    sb.transform(trf);
+    ASSERT_MATRIX_APPROX(sb.getState(), x, 1e-15);
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}