diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 2a779c07e150cbb295cb7140767ad485128d141d..027b1b4e8c3e4418a9c702a4300272da046e5e91 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -146,14 +146,6 @@ class Problem : public std::enable_shared_from_this<Problem>
         SensorBasePtr installSensor(const std::string& _sen_type,
                                     const std::string& _params_yaml_filename,
                                     const std::vector<std::string>& _folders_schema);
-        /** \brief Factory method to install (create and add) sensors only from its properties
-         * \param _sen_type type of sensor
-         * \param _params a base-pointer to a derived struct defining the intrinsic parameters.
-         * \param _priors an unordered map of the states priors indexed by their key.
-         */
-        SensorBasePtr installSensor(const std::string& _sen_type,
-                                    ParamsSensorBasePtr _params,
-                                    const Priors& _priors);
 
         /** \brief Factory method to install (create, and add to sensor) processors only from a YAML node -- Helper method loading parameters from file
          * \param _processor_node YAML node containing all necessary information to call the factory and create the processor.
@@ -175,17 +167,6 @@ class Problem : public std::enable_shared_from_this<Problem>
                                           const std::string& _params_yaml_filename,
                                           const std::vector<std::string>& _folders_schema);
 
-        /** \brief Factory method to install (create, and add to sensor) processors only from its properties
-         *
-         * This method creates a Processor, and adds it to the specified sensor's list of processors
-         * \param _prc_type type of processor
-         * \param _corresponding_sensor_ptr pointer to the sensor where the processor will be installed.
-         * \param _prc_params a base-pointer to a derived struct defining the processor parameters.
-         */
-        ProcessorBasePtr installProcessor(const std::string& _prc_type,
-                                          SensorBasePtr _corresponding_sensor_ptr,
-                                          ParamsProcessorBasePtr _prc_params);
-
         /** \brief get a sensor pointer by its name
          * \param _sensor_name The sensor name, as it was installed with installSensor()
          */
diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h
index 253ccb9361aa6a69cb8c67a60ee46211df15d65f..b432824fbbae94391460334009fc09191057acca 100644
--- a/include/core/processor/factory_processor.h
+++ b/include/core/processor/factory_processor.h
@@ -134,20 +134,10 @@ inline std::string FactoryProcessorYaml::getClass() const
     return "FactoryProcessorYaml";
 }
 
-typedef Factory<ProcessorBase,
-                std::shared_ptr<ParamsProcessorBase>> FactoryProcessorParams;
-template<>
-inline std::string FactoryProcessorParams::getClass() const
-{
-    return "FactoryProcessorParams";
-}
-
 #define WOLF_REGISTER_PROCESSOR(ProcessorType)                                                \
   namespace{ const bool WOLF_UNUSED ProcessorType##Registered =                               \
     wolf::FactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); }         \
   namespace{ const bool WOLF_UNUSED ProcessorType##YamlRegistered =                           \
     wolf::FactoryProcessorYaml::registerCreator(#ProcessorType, ProcessorType::create); }     \
-  namespace{ const bool WOLF_UNUSED ProcessorType##ParamsRegistered =                         \
-    wolf::FactoryProcessorParams::registerCreator(#ProcessorType, ProcessorType::create); }   \
 
 } /* namespace wolf */
\ No newline at end of file
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 124744ce38b67ca53be5654aa34b1b8cb9fa593f..adc03e088aaac33223d70f542858e3c3fe4ff9d6 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -80,19 +80,6 @@ static ProcessorBasePtr create(const std::string& _yaml_filepath,
                                                                                     \
     return std::make_shared<ProcessorClass>(params);                                \
 }                                                                                   \
-static ProcessorBasePtr create(const ParamsProcessorBasePtr _params)                \
-{                                                                                   \
-    auto params_derived = std::dynamic_pointer_cast<ParamsProcessorClass>(_params); \
-    if (not params_derived)                                                         \
-    {                                                                               \
-        WOLF_ERROR("In " #ProcessorClass " creator:",                               \
-                   " _params is not of type " #ParamsProcessorClass "!");           \
-        return nullptr;                                                             \
-    }                                                                               \
-                                                                                    \
-    return std::make_shared<ProcessorClass>(params_derived);                        \
-}                                                                                   \
-
 
 /** \brief Buffer for arbitrary type objects
  *
@@ -468,6 +455,7 @@ template<typename classType, typename... T>
 std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all)
 {
     std::shared_ptr<classType> prc = std::make_shared<classType>(std::forward<T>(all)...);
+    prc->configure(_sen_ptr);
     prc->link(_sen_ptr);
     return prc;
 }
diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
index fa8cc69e3cd8dbaa6f00d993b5d735054dcc9d09..976097d4555649ed78315b036bd0b35d5b7637af 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.h
@@ -218,11 +218,6 @@ typedef Factory<SensorBase,
                 const std::string&,
                 const std::vector<std::string>&> FactorySensorYaml;
 
-typedef Factory<SensorBase,
-                SizeEigen,
-                std::shared_ptr<ParamsSensorBase>,
-                const Priors& > FactorySensorPriors;
-
 template<>
 inline std::string FactorySensor::getClass() const
 {
@@ -233,18 +228,15 @@ inline std::string FactorySensorYaml::getClass() const
 {
   return "FactorySensorYaml";
 }
-template<>
-inline std::string FactorySensorPriors::getClass() const
-{
-  return "FactorySensorPriors";
-}
 
-#define WOLF_REGISTER_SENSOR(SensorType)                                     \
-  namespace{ const bool WOLF_UNUSED SensorType##Registered =                 \
-     FactorySensor::registerCreator(#SensorType, SensorType::create);}       \
-  namespace{ const bool WOLF_UNUSED SensorType##YamlRegistered =             \
-     FactorySensorYaml::registerCreator(#SensorType, SensorType::create);}   \
-  namespace{ const bool WOLF_UNUSED SensorType##PriorsRegistered =           \
-     FactorySensorPriors::registerCreator(#SensorType, SensorType::create);} \
+
+
+#define WOLF_REGISTER_SENSOR_WITH_KEY(SensorClassName, SensorType)                \
+  namespace{ const bool WOLF_UNUSED SensorClassName##Registered =                 \
+     FactorySensor::registerCreator(#SensorClassName, SensorType::create);}       \
+  namespace{ const bool WOLF_UNUSED SensorClassName##YamlRegistered =             \
+     FactorySensorYaml::registerCreator(#SensorClassName, SensorType::create);}   \
+
+#define WOLF_REGISTER_SENSOR(SensorType) WOLF_REGISTER_SENSOR_WITH_KEY(SensorType, SensorType) \
 
 } /* namespace wolf */
\ No newline at end of file
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 5910fe3c4d4dad444f29cad67f1da641af643518..cc1f5d408819c796b717349859681ce607870369 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -86,20 +86,6 @@ static SensorBasePtr create(SizeEigen _dim,
                                                                                  \
     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
  *
diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h
index b73c95ce190892d8430195e1d7e31e6b02e555da..6cb43d327e62d67a92cb227d7c5507cf953c934d 100644
--- a/include/core/sensor/sensor_odom.h
+++ b/include/core/sensor/sensor_odom.h
@@ -67,10 +67,10 @@ class SensorOdom : public SensorBase
         SizeEigen dim_;
         ParamsSensorOdomPtr params_odom_;
         
-        // protected constructor to avoid creation of SensorOdom without dimension
+	public:
         SensorOdom(const SizeEigen& _dim, ParamsSensorOdomPtr _params, const Priors& _priors);
+        WOLF_SENSOR_CREATE(SensorOdom, ParamsSensorOdom);
 
-	public:
         ~SensorOdom() override = default;
 
         double getDispVarToDispNoiseFactor() const;
@@ -83,22 +83,6 @@ class SensorOdom : public SensorBase
 
 };
 
-WOLF_PTR_TYPEDEFS(SensorOdom2d);
-class SensorOdom2d : public SensorOdom
-{
-    public:
-        SensorOdom2d(const SizeEigen& _dim, ParamsSensorOdomPtr _params, const Priors& _priors);
-        WOLF_SENSOR_CREATE(SensorOdom2d, ParamsSensorOdom);
-};
-
-WOLF_PTR_TYPEDEFS(SensorOdom3d);
-class SensorOdom3d : public SensorOdom
-{
-    public:
-        SensorOdom3d(const SizeEigen& _dim, ParamsSensorOdomPtr _params, const Priors& _priors);
-        WOLF_SENSOR_CREATE(SensorOdom3d, ParamsSensorOdom);
-};
-
 inline double SensorOdom::getDispVarToDispNoiseFactor() const
 {
     return params_odom_->k_disp_to_disp;
diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h
index 0864028a49f99dd996b01b85aec6c1bf894753b4..67da9837d8fa5bc8743404d26e6c046f2d0de914 100644
--- a/include/core/state_block/prior.h
+++ b/include/core/state_block/prior.h
@@ -74,13 +74,7 @@ class Priors : public PriorMap
     public:
         using PriorMap::PriorMap;
 
-        Priors(const YAML::Node& priors_node)
-        {
-            for (auto prior_pair : priors_node)
-            {
-                this->emplace(prior_pair.first.as<char>(), Prior(prior_pair.second));
-            }
-        }
+        Priors(const YAML::Node& priors_node);
         virtual ~Priors() = default;
 };
 
diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
index edd9a44ef28783255afe3190e751ac92aa4c038e..c4f4e4d5655c6f3519fb75e91cfab8123ac40c0c 100644
--- a/include/core/tree_manager/factory_tree_manager.h
+++ b/include/core/tree_manager/factory_tree_manager.h
@@ -52,20 +52,10 @@ inline std::string FactoryTreeManagerYaml::getClass() const
     return "FactoryTreeManagerYaml";
 }
 
-typedef Factory<TreeManagerBase,
-                ParamsTreeManagerBasePtr> FactoryTreeManagerParams;
-template<>
-inline std::string FactoryTreeManagerParams::getClass() const
-{
-  return "FactoryTreeManagerParams";
-}
-
 #define WOLF_REGISTER_TREE_MANAGER(TreeManagerType)                                               \
   namespace{ const bool WOLF_UNUSED TreeManagerType##Registered =                                 \
     wolf::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); }       \
   namespace{ const bool WOLF_UNUSED TreeManagerType##YamlRegistered =                             \
     wolf::FactoryTreeManagerYaml::registerCreator(#TreeManagerType, TreeManagerType::create); }   \
-  namespace{ const bool WOLF_UNUSED TreeManagerType##ParamsRegistered =                           \
-    wolf::FactoryTreeManagerParams::registerCreator(#TreeManagerType, TreeManagerType::create); } \
 
 } /* namespace wolf */
\ No newline at end of file
diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h
index 508f48438cb50229c919d74a5f4fcd1f29f811fc..588f735c9123d33c99ed71f02c185ae668c0d317 100644
--- a/include/core/tree_manager/tree_manager_base.h
+++ b/include/core/tree_manager/tree_manager_base.h
@@ -64,12 +64,6 @@ static TreeManagerBasePtr create(const std::string& _yaml_filepath,
                                                                                     \
     return std::make_shared<TreeManagerClass>(params);                              \
 }                                                                                   \
-static TreeManagerBasePtr create(ParamsTreeManagerBasePtr _params)                  \
-{                                                                                   \
-    auto params = std::static_pointer_cast<ParamsTreeManagerClass>(_params);        \
-                                                                                    \
-    return std::make_shared<TreeManagerClass>(params);                              \
-}                                                                                   \
 
 struct ParamsTreeManagerBase : public ParamsBase
 {
diff --git a/schema/PriorModeDynamicNoiseDrift.schema b/schema/PriorModeDynamicNoiseDrift.schema
new file mode 100644
index 0000000000000000000000000000000000000000..4c14a56525015f0732ee83a8b7d2c88e2d44a82c
--- /dev/null
+++ b/schema/PriorModeDynamicNoiseDrift.schema
@@ -0,0 +1,26 @@
+mode:
+  type: string
+  yaml_type: scalar
+  mandatory: true
+  options:
+    - "fix"
+    - "factor"
+    - "initial_guess"
+  doc: The prior mode can be 'factor' to add an absolute factor (requires 'noise_std'), 'fix' to set the values constant or 'initial_guess' to just set the values
+dynamic:
+  type: bool
+  yaml_type: scalar
+  mandatory: true
+  doc: If the state is dynamic, i.e. it changes along time.
+noise_std:
+  type: VectorXd
+  yaml_type: scalar
+  mandatory: false
+  default: []
+  doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
+drift_std:
+  type: VectorXd
+  yaml_type: scalar
+  mandatory: false
+  default: []
+  doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
diff --git a/schema/PriorO2d.schema b/schema/PriorO2d.schema
index 08173f33b5780387d5ab81a164c4ab25bb80623e..402d0c37e6ba16513c8905cccc485edae9b52509 100644
--- a/schema/PriorO2d.schema
+++ b/schema/PriorO2d.schema
@@ -10,29 +10,4 @@ state:
   yaml_type: scalar
   mandatory: true
   doc: A vector containing the state values
-mode:
-  type: string
-  yaml_type: scalar
-  mandatory: true
-  options:
-    - "fix"
-    - "factor"
-    - "initial_guess"
-  doc: The prior mode can be 'factor' to add an absolute factor (requires 'noise_std'), 'fix' to set the values constant or 'initial_guess' to just set the values
-dynamic:
-  type: bool
-  yaml_type: scalar
-  mandatory: true
-  doc: If the state is dynamic, i.e. it changes along time.
-noise_std:
-  type: VectorXd
-  yaml_type: scalar
-  mandatory: false
-  default: []
-  doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
-drift_std:
-  type: VectorXd
-  yaml_type: scalar
-  mandatory: false
-  default: []
-  doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
+follow: PriorModeDynamicNoiseDrift.schema
\ No newline at end of file
diff --git a/schema/PriorO3d.schema b/schema/PriorO3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..e7b1dd38bb1c88f297e3d831c7ca132c704888bc
--- /dev/null
+++ b/schema/PriorO3d.schema
@@ -0,0 +1,13 @@
+type:
+  type: string
+  yaml_type: scalar
+  mandatory: false
+  default: StateQuaternion
+  options: [StateQuaternion]
+  doc: The derived type of the State in 'O'
+state:
+  type: Vector4d
+  yaml_type: scalar
+  mandatory: true
+  doc: A vector containing the state values. It should be a quaternion (i.e. four values and normalized)
+follow: PriorModeDynamicNoiseDrift.schema
\ No newline at end of file
diff --git a/schema/PriorP2d.schema b/schema/PriorP2d.schema
index 3e17abd5faedf1681ddfa39d8198c60c1e215e1d..d8f19390fedf3e957ced97e9c63bc66a966c402c 100644
--- a/schema/PriorP2d.schema
+++ b/schema/PriorP2d.schema
@@ -10,29 +10,4 @@ state:
   yaml_type: scalar
   mandatory: true
   doc: A vector containing the state values
-mode:
-  type: string
-  yaml_type: scalar
-  mandatory: true
-  options:
-    - "fix"
-    - "factor"
-    - "initial_guess"
-  doc: The prior mode can be 'factor' to add an absolute factor (requires 'noise_std'), 'fix' to set the values constant or 'initial_guess' to just set the values
-dynamic:
-  type: bool
-  yaml_type: scalar
-  mandatory: true
-  doc: If the state is dynamic, i.e. it changes along time.
-noise_std:
-  type: VectorXd
-  yaml_type: scalar
-  mandatory: false
-  default: []
-  doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
-drift_std:
-  type: VectorXd
-  yaml_type: scalar
-  mandatory: false
-  default: []
-  doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
+follow: PriorModeDynamicNoiseDrift.schema
\ No newline at end of file
diff --git a/schema/PriorP3d.schema b/schema/PriorP3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..d413a902908fcf31e4e9443afe65e78fbd12007e
--- /dev/null
+++ b/schema/PriorP3d.schema
@@ -0,0 +1,13 @@
+type:
+  type: string
+  yaml_type: scalar
+  mandatory: false
+  default: StatePoint3d
+  options: [StatePoint3d]
+  doc: The derived type of the state in 'P'
+state:
+  type: Vector3d
+  yaml_type: scalar
+  mandatory: true
+  doc: A vector containing the state 'P' values
+follow: PriorModeDynamicNoiseDrift.schema
\ No newline at end of file
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 98a12bb81f48e26171cf7117407a788671a0ff61..0442805fe31fa2cfdfc76622c508ca5db0a68d4d 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -259,18 +259,6 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type,
     return sen_ptr;
 }
 
-SensorBasePtr Problem::installSensor(const std::string& _sen_type,
-                                     ParamsSensorBasePtr _params,
-                                     const Priors& _priors)
-{
-    SensorBasePtr sen_ptr = FactorySensorPriors::create(_sen_type, 
-                                                        dim_, 
-                                                        _params, 
-                                                        _priors);
-    sen_ptr->link(getHardware());
-    return sen_ptr;
-}
-
 ProcessorBasePtr Problem::installProcessor(const YAML::Node& _processor_node)
 {
     auto corresponding_sensor_name = _processor_node["sensor_name"].as<std::string>();
@@ -316,30 +304,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type,
     return prc_ptr;
 }
 
-ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type,
-                                           SensorBasePtr _corresponding_sensor_ptr,
-                                           ParamsProcessorBasePtr _prc_params)
-{
-    if (_corresponding_sensor_ptr == nullptr)
-    {
-      WOLF_ERROR("Cannot install processor '", _prc_params->name,
-                 "' since the associated sensor does not exist !");
-      return ProcessorBasePtr();
-    }
-
-    ProcessorBasePtr prc_ptr = FactoryProcessorParams::create(_prc_type, _prc_params);
-
-    //Dimension check
-    int prc_dim = prc_ptr->getDim();
-    auto prb = this;
-    assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
-
-    prc_ptr->configure(_corresponding_sensor_ptr);
-    prc_ptr->link(_corresponding_sensor_ptr);
-
-    return prc_ptr;
-}
-
 SensorBaseConstPtr Problem::findSensor(const std::string& _sensor_name) const
 {
     return getHardware()->getSensor(_sensor_name);
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 9ecb93796fdf739fe3ae253a1c8d19b9502aa4e4..d6ad0ad51397794b095543f4a0ab14e01c4b2ad4 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -69,10 +69,14 @@ void SensorBase::loadPriors(const Priors& _priors,
         auto prior = state_pair.second;
 
         // type
-        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 == 'P' and _dim == 2 and prior.getType() != "P" and prior.getType() != "StatePoint2d")
+            throw std::runtime_error("Prior type for key P has to be 'P' or 'StatePoint2d'");
+        if (key == 'P' and _dim == 3 and prior.getType() != "P" and prior.getType() != "StatePoint3d")
+            throw std::runtime_error("Prior type for key P has to be 'P' or 'StatePoint3d'");
+        if (key == 'V' and _dim == 2 and prior.getType() != "V" and prior.getType() != "StatePoint2d")
+            throw std::runtime_error("Prior type for key V has to be 'V' or 'StatePoint2d'");
+        if (key == 'V' and _dim == 3 and prior.getType() != "V" and prior.getType() != "StatePoint3d")
+            throw std::runtime_error("Prior type for key V has to be 'V' or 'StatePoint3d'");
         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")
diff --git a/src/sensor/sensor_odom.cpp b/src/sensor/sensor_odom.cpp
index 1b18d951b9307ce10d9918d07856ac76f1056e29..c3ac836ca19d31d17524aac9b2463d65e0a46f3b 100644
--- a/src/sensor/sensor_odom.cpp
+++ b/src/sensor/sensor_odom.cpp
@@ -35,22 +35,6 @@ SensorOdom::SensorOdom(const SizeEigen& _dim,
 {
 }
 
-SensorOdom2d::SensorOdom2d(const SizeEigen& _dim,
-                           ParamsSensorOdomPtr _params,
-                           const Priors& _priors) :
-        SensorOdom(_dim,_params,_priors)
-{
-    assert(_dim == 2);
-}
-
-SensorOdom3d::SensorOdom3d(const SizeEigen& _dim,
-                           ParamsSensorOdomPtr _params,
-                           const Priors& _priors) :
-        SensorOdom(_dim,_params,_priors)
-{
-    assert(_dim == 3);
-}
-
 Eigen::MatrixXd SensorOdom::computeNoiseCov(const Eigen::VectorXd & _data) const
 {
     double d; // displacement
@@ -101,6 +85,6 @@ Eigen::MatrixXd SensorOdom::computeNoiseCov(const Eigen::VectorXd & _data) const
 // Register in the FactorySensor
 #include "core/sensor/factory_sensor.h"
 namespace wolf {
-WOLF_REGISTER_SENSOR(SensorOdom2d);
-WOLF_REGISTER_SENSOR(SensorOdom3d);
+WOLF_REGISTER_SENSOR_WITH_KEY(SensorOdom2d, SensorOdom);
+WOLF_REGISTER_SENSOR_WITH_KEY(SensorOdom3d, SensorOdom);
 } // namespace wolf
diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp
index 098855b99b94d515612a39a46c40dfa5932e875e..40fcb72e3094627316aa77f57b5afa7c8b11bb0e 100644
--- a/src/state_block/prior.cpp
+++ b/src/state_block/prior.cpp
@@ -26,6 +26,20 @@
 
 namespace wolf
 {
+
+Priors::Priors(const YAML::Node& priors_node)
+{
+    WOLF_INFO("Priors: \n", priors_node);
+    if (not priors_node.IsMap())
+    {
+        throw std::runtime_error("Priors: constructor with a non-map yaml node");
+    }
+    for (auto prior_pair : priors_node)
+    {
+        this->emplace(prior_pair.first.as<char>(), Prior(prior_pair.second));
+    }
+}
+
 Prior::Prior(const std::string&     _type,
              const Eigen::VectorXd& _state,
              const std::string&     _mode,
@@ -42,7 +56,6 @@ Prior::Prior(const YAML::Node& prior_node)
     type_  = prior_node["type"].as<std::string>();
     state_ = prior_node["state"].as<Eigen::VectorXd>();
     mode_  = prior_node["mode"].as<std::string>();
-
     if (mode_ == "factor") 
       noise_std_ = prior_node["noise_std"].as<Eigen::VectorXd>();
     else 
@@ -86,12 +99,12 @@ void Prior::check() const
     if (state_.size() != sb->getSize())
         throw std::runtime_error("Prior::check() Prior " + type_ + " state size different of StateBlock size. " + print());
 
-    // check sigma size
+    // check factor sigma size
     if (mode_ == "factor" and noise_std_.size() != sb->getLocalSize())
         throw std::runtime_error("Prior::check() Prior " + type_ + " noise_std size different of StateBlock local size. " +
                                  print());
 
-    // check sigma size
+    // check drift sigma size
     if (dynamic_ and drift_std_.size() > 0 and drift_std_.size() != sb->getLocalSize())
         throw std::runtime_error("Prior::check() Prior " + type_ +
                                  " drift_std size different of StateBlock local size. " + print());
diff --git a/test/dummy/SensorDummy.schema b/test/dummy/SensorDummy2d.schema
similarity index 100%
rename from test/dummy/SensorDummy.schema
rename to test/dummy/SensorDummy2d.schema
diff --git a/test/dummy/SensorDummy3d.schema b/test/dummy/SensorDummy3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..941cb53d050d8d25ebb26c8ead01a79a05bb750f
--- /dev/null
+++ b/test/dummy/SensorDummy3d.schema
@@ -0,0 +1,16 @@
+follow: SensorBase.schema
+states:
+  P:
+    follow: PriorP3d.schema
+  O:
+    follow: PriorO3d.schema
+noise_p_std:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: noise std in P.
+noise_o_std:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: noise std in O.
\ No newline at end of file
diff --git a/test/dummy/SensorDummyPoia3d.schema b/test/dummy/SensorDummyPoia3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..ac24447e53e658a3d01e3e306938bfdab99c19fc
--- /dev/null
+++ b/test/dummy/SensorDummyPoia3d.schema
@@ -0,0 +1,44 @@
+follow: SensorBase.schema
+states:
+  P:
+    follow: PriorP3d.schema
+  O:
+    follow: PriorO3d.schema
+  I:
+    type:
+      type: string
+      yaml_type: scalar
+      mandatory: false
+      default: StateParam5
+      options: [StateParam5]
+      doc: The derived type of the state in 'I'
+    state:
+      type: Vector5d
+      yaml_type: scalar
+      mandatory: true
+      doc: A vector containing the state 'I' values
+    follow: PriorModeDynamicNoiseDrift.schema
+  A:
+    type:
+      type: string
+      yaml_type: scalar
+      mandatory: false
+      default: StateQuaternion
+      options: [StateQuaternion]
+      doc: The derived type of the state in 'A'
+    state:
+      type: Vector4d
+      yaml_type: scalar
+      mandatory: true
+      doc: A vector containing the state 'A' values
+    follow: PriorModeDynamicNoiseDrift.schema
+noise_p_std:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: noise std in p.
+noise_o_std:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: noise std in o.
\ No newline at end of file
diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h
index 96e8b5777a61a69e5773759c5ffa9a2cd461e163..23ba2a36cc36d719b71c6dae13d768452119851f 100644
--- a/test/dummy/sensor_dummy.h
+++ b/test/dummy/sensor_dummy.h
@@ -86,5 +86,8 @@ class SensorDummy : public SensorBase
 // Register in the FactorySensor
 #include "core/sensor/factory_sensor.h"
 namespace wolf {
-WOLF_REGISTER_SENSOR(SensorDummy);
+WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummy2d, SensorDummy);
+WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummy3d, SensorDummy);
+WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummyPoia2d, SensorDummy);
+WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummyPoia3d, SensorDummy);
 } // namespace wolf
\ No newline at end of file
diff --git a/test/dummy/sensor_dummy_poia.h b/test/dummy/sensor_dummy_poia.h
deleted file mode 100644
index f81eca9efb48f8fcbc69254d1062347402c28600..0000000000000000000000000000000000000000
--- a/test/dummy/sensor_dummy_poia.h
+++ /dev/null
@@ -1,87 +0,0 @@
-//--------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
-
-/**************************
- *      WOLF includes     *
- **************************/
-#include "core/sensor/sensor_base.h"
-#include "sensor_dummy.h"
-
-namespace wolf {
-
-// SpecStates specs_states_dummy_poia({{'I',SpecState("StateBlock", 5, "some doc for state I")},
-                                    // {'A',SpecState("StateQuaternion", 4, "some doc for state A")}});
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDummyPoia);
-
-struct ParamsSensorDummyPoia : public ParamsSensorDummy
-{
-    ParamsSensorDummyPoia() = default;    
-    ParamsSensorDummyPoia(const YAML::Node& _n):
-        ParamsSensorDummy(_n)
-    {
-    }
-    ~ParamsSensorDummyPoia() override = default;
-    std::string print() const override
-    {
-        return ParamsSensorDummy::print();
-    }
-};
-
-WOLF_PTR_TYPEDEFS(SensorDummyPoia);
-
-class SensorDummyPoia : public SensorBase
-{
-    private:
-        ParamsSensorDummyPoiaPtr params_dummy_poia_;
-        SizeEigen dim_;
-
-    public:
-        SensorDummyPoia(SizeEigen _dim, 
-                        ParamsSensorDummyPoiaPtr _params,
-                        const Priors& _priors) :
-            SensorBase("SensorDummyPoia",
-                       _dim,
-                       _params,
-                       _priors),
-            params_dummy_poia_(_params)
-        {
-        }
-        WOLF_SENSOR_CREATE(SensorDummyPoia, ParamsSensorDummyPoia);
-
-        virtual ~SensorDummyPoia(){};
-
-        Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override
-        {
-            return (Eigen::Vector2d() << params_dummy_poia_->noise_p_std,
-                                         params_dummy_poia_->noise_o_std).finished().cwiseAbs2().asDiagonal();
-        }
-};
-
-}
-
-// Register in the FactorySensor
-#include "core/sensor/factory_sensor.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR(SensorDummyPoia);
-} // namespace wolf
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index e71a0a1e75d5992c8b1b39d2d77eda60db0259e0..fa60723ff68b6eb86106403ea0c828b2844365a3 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -66,7 +66,7 @@ TEST(ProcessorBase, MotionProvider)
                                            {wolf_root});
     auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
     proc_trk_params->name = "proc tracker";
-    auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", sens_trk, proc_trk_params);
+    auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params);
 
     // Install odometer (sensor and processor)
     auto sens_odo = problem->installSensor("SensorOdom",
@@ -75,9 +75,7 @@ TEST(ProcessorBase, MotionProvider)
     auto proc_odo_params = make_shared<ParamsProcessorOdom2d>();
     proc_odo_params->time_tolerance = dt/2;
     proc_odo_params->name = "odom processor";
-    auto proc_odo = problem->installProcessor("ProcessorOdom2d",
-                                              sens_odo,
-                                              proc_odo_params);
+    auto proc_odo = ProcessorBase::emplace<ProcessorOdom2d>(sens_odo, proc_odo_params);
 
     ASSERT_FALSE(proc_trk->isMotionProvider());
     ASSERT_TRUE (proc_odo->isMotionProvider());
@@ -106,7 +104,7 @@ TEST(ProcessorBase, KeyFrameCallback)
                                            {wolf_root});
     auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>();
     proc_trk_params->name = "proc tracker";
-    auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", sens_trk, proc_trk_params);
+    auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params);
 
     // Install odometer (sensor and processor)
     auto sens_odo = problem->installSensor("SensorOdom",
@@ -115,9 +113,7 @@ TEST(ProcessorBase, KeyFrameCallback)
     auto proc_odo_params = make_shared<ParamsProcessorOdom2d>();
     proc_odo_params->time_tolerance = dt/2;
     proc_odo_params->name = "odom processor";
-    auto proc_odo = problem->installProcessor("ProcessorOdom2d",
-                                              sens_odo,
-                                              proc_odo_params);
+    auto proc_odo = ProcessorBase::emplace<ProcessorOdom2d>(sens_odo, proc_odo_params);
 
     std::cout << "sensor & processor created and added to wolf problem" << std::endl;
 
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 2154adcb77a2cff52f90b6ded74ce10e3eeb8e3b..0771d6b26f12945177e4616e889fc6b6f4850ccc 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -23,7 +23,6 @@
 #include "core/sensor/sensor_base.h"
 #include "core/utils/utils_gtest.h"
 #include "dummy/sensor_dummy.h"
-#include "dummy/sensor_dummy_poia.h"
 #include "yaml-schema-cpp/yaml_server.hpp"
 
 using namespace wolf;
@@ -326,18 +325,18 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D_drift)
 // 3D POIA mixed
 TEST(SensorBase, makeshared_priors_POIA_mixed)
 {
-  auto params = std::make_shared<ParamsSensorDummyPoia>();
+  auto params = std::make_shared<ParamsSensorDummy>();
   params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   VectorXd i_state_3D = VectorXd::Random(5);
 
-  auto S = std::make_shared<SensorDummyPoia>(3, params, 
-                                             Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
-                                                     {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
-                                                     {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
-                                                     {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
+  auto S = std::make_shared<SensorDummy>(3, params, 
+                                         Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
+                                                 {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
+                                                 {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
+                                                 {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
      
   // noise
   ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
@@ -352,41 +351,6 @@ TEST(SensorBase, makeshared_priors_POIA_mixed)
   checkSensor(S, 'A', o_state_3D, false, o_std_3D, false, vector0);
 }
 
-// 3D POIA wrong
-TEST(SensorBase, makeshared_priors_POIA_wrong)
-{
-  auto params = std::make_shared<ParamsSensorDummyPoia>();
-  params->name = "sensor_1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  VectorXd i_state_3D = VectorXd::Random(5);
-
-  // missing I
-  ASSERT_THROW(std::make_shared<SensorDummyPoia>(3, params, 
-                                                 Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
-                                                         {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
-                                                         //{'I',Prior("StateBlock", i_state_3D, "initial_guess")},
-                                                         {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}})),
-               std::runtime_error);
-
-  // missing A
-  ASSERT_THROW(std::make_shared<SensorDummyPoia>(3, params, 
-                                                 Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
-                                                         {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
-                                                         {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
-                                                         /*{'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}*/})),
-               std::runtime_error);
-
-  // wrong A type (expected StateQuaternion)
-  ASSERT_THROW(std::make_shared<SensorDummyPoia>(3, params, 
-                                                 Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
-                                                         {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
-                                                         {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
-                                                         {'A',Prior("StateBlock", p_state_3D, "factor", p_std_3D)}})),
-               std::runtime_error);
-}
-
 //////////////////////////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////// FactorySensor ///////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -428,12 +392,15 @@ TEST(SensorBase, factory)
 
             WOLF_INFO("Creating sensor from ", name, ".yaml");
 
-            ASSERT_EQ(server.applySchema("SensorDummy"), not wrong);
+            auto valid = server.applySchema("SensorDummy"+toString(dim)+"d");
+            WOLF_WARN_COND(not valid and not wrong, server.getLog().str());
 
             // CORRECT YAML
             if (not wrong)
             {
-              auto S = FactorySensor::create("SensorDummy", dim, server.getNode());
+              ASSERT_TRUE(valid);
+
+              auto S = FactorySensor::create("SensorDummy"+toString(dim)+"d", dim, server.getNode());
 
               auto p_size = dim;
               auto o_size = dim == 2 ? 1 : 4;
@@ -444,8 +411,8 @@ TEST(SensorBase, factory)
 
               // noise
               ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), 
-                                  noise_cov_dummy, 
-                                  Constants::EPS);
+                                   noise_cov_dummy, 
+                                   Constants::EPS);
 
               // factors
               ASSERT_EQ(S->getPriorFeatures().size(), mode == "factor" ? 2 : 0);
@@ -457,7 +424,11 @@ TEST(SensorBase, factory)
             // INCORRECT YAML
             else
             {
-              ASSERT_THROW(FactorySensor::create("SensorDummy", dim, server.getNode()),std::runtime_error);
+              // either is not valid (schema) or it throws an error
+              if (valid)
+              {
+                ASSERT_THROW(FactorySensor::create("SensorDummy"+toString(dim)+"d", dim, server.getNode()),std::runtime_error);
+              }
             }
           }
 
@@ -469,7 +440,7 @@ TEST(SensorBase, factory)
     YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml");
 
     // create sensor
-    auto S = FactorySensor::create("SensorDummyPoia", 3, server.getNode());
+    auto S = FactorySensor::create("SensorDummyPoia3d", 3, server.getNode());
 
     // noise
     ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), 
@@ -493,7 +464,7 @@ TEST(SensorBase, factory)
     YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml");
 
     // create sensor
-    ASSERT_THROW(FactorySensor::create("SensorDummyPoia", 3, server.getNode()),std::runtime_error);
+    ASSERT_THROW(FactorySensor::create("SensorDummyPoia3d", 3, server.getNode()),std::runtime_error);
   }
 }
 
@@ -542,7 +513,7 @@ TEST(SensorBase, factory_yaml)
             // CORRECT YAML
             if (not wrong)
             {
-              auto S = FactorySensorYaml::create("SensorDummy", dim, yaml_filepath, {wolf_root});
+              auto S = FactorySensorYaml::create("SensorDummy"+toString(dim)+"d", dim, yaml_filepath, {wolf_root});
 
               auto p_size = dim;
               auto o_size = dim == 2 ? 1 : 4;
@@ -566,7 +537,7 @@ TEST(SensorBase, factory_yaml)
             // INCORRECT YAML
             else
             {
-              ASSERT_THROW(FactorySensorYaml::create("SensorDummy", dim, yaml_filepath, {wolf_root}),std::runtime_error);
+              ASSERT_THROW(FactorySensorYaml::create("SensorDummy"+toString(dim)+"d", dim, yaml_filepath, {wolf_root}),std::runtime_error);
             }
           }
 
@@ -575,7 +546,7 @@ TEST(SensorBase, factory_yaml)
     WOLF_INFO("Creating sensor from name sensor_POIA_3D.yaml");
 
     // create sensor
-    auto S = FactorySensorYaml::create("SensorDummyPoia",
+    auto S = FactorySensorYaml::create("SensorDummy3d",
                                        3,
                                        wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml",
                                        {wolf_root});
@@ -599,7 +570,7 @@ TEST(SensorBase, factory_yaml)
     WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong.yaml");
 
     // create sensor
-    ASSERT_THROW(FactorySensorYaml::create("SensorDummyPoia",
+    ASSERT_THROW(FactorySensorYaml::create("SensorDummyPoia3d",
                                            3,
                                            wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml",
                                            {wolf_root}),
@@ -607,301 +578,6 @@ TEST(SensorBase, factory_yaml)
   }
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////// FactorySensorPriors ////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-
-// 2D Fix
-TEST(SensorBase, factory_priors_POfix2D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
-                                               {'O',Prior("O", o_state_2D)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // checks
-  checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_2D, true, vector0, false, vector0);
-}
-
-// 3D Fix
-TEST(SensorBase, factory_priors_POfix3D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
-                                               {'O',Prior("O", o_state_3D)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, true, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_3D, true, vector0, false, vector0);
-}
-
-// 2D Initial guess
-TEST(SensorBase, factory_priors_POinitial_guess2D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
-                                               {'O',Prior("O", o_state_2D, "initial_guess")}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_2D, false, vector0, false, vector0);
-}
-
-// 3D Initial guess
-TEST(SensorBase, factory_priors_POinitial_guess3D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
-                                               {'O',Prior("O", o_state_3D, "initial_guess")}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_3D, false, vector0, false, vector0);
-}
-
-// 2D Factor
-TEST(SensorBase, factory_priors_POfactor2D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
-                                               {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 2);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, p_std_2D, false, vector0);
-  checkSensor(S, 'O', o_state_2D, false, o_std_2D, false, vector0);
-}
-
-// 3D Factor
-TEST(SensorBase, factory_priors_POfactor3D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
-                                               {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 2);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, p_std_3D, false, vector0);
-  checkSensor(S, 'O', o_state_3D, false, o_std_3D, false, vector0);
-}
-
-// 2D Initial guess dynamic
-TEST(SensorBase, factory_priors_POinitial_guess_dynamic2D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
-                                               {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, vector0, true, vector0);
-  checkSensor(S, 'O', o_state_2D, false, vector0, true, vector0);
-}
-
-// 3D Initial guess dynamic
-TEST(SensorBase, factory_priors_POinitial_guess_dynamic3D)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
-                                               {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, vector0, true, vector0);
-  checkSensor(S, 'O', o_state_3D, false, vector0, true, vector0);
-}
-
-// 2D Initial guess dynamic drift
-TEST(SensorBase, factory_priors_POinitial_guess_dynamic2D_drift)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
-                                               {'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, vector0, true, p_std_2D);
-  checkSensor(S, 'O', o_state_2D, false, vector0, true, o_std_2D);
-}
-
-// 3D Initial guess dynamic drift
-TEST(SensorBase, factory_priors_POinitial_guess_dynamic3D_drift)
-{
-  auto params = std::make_shared<ParamsSensorDummy>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  auto S = FactorySensorPriors::create("SensorDummy", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
-                                               {'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
-
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, vector0, true, p_std_3D);
-  checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
-}
-
-// 3D POIA mixed
-TEST(SensorBase, factory_priors_POIA_mixed)
-{
-  auto params = std::make_shared<ParamsSensorDummyPoia>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  VectorXd i_state_3D = VectorXd::Random(5);
-
-  auto S = FactorySensorPriors::create("SensorDummyPoia", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
-                                               {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
-                                               {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
-                                               {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
-     
-  // noise
-  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 2);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, true, vector0, true, vector0);
-  checkSensor(S, 'O', o_state_3D, false, o_std_3D, true, o_std_3D);
-  checkSensor(S, 'I', i_state_3D, false, vector0, false, vector0);
-  checkSensor(S, 'A', o_state_3D, false, o_std_3D, false, vector0);
-}
-
-// 3D POIA wrong
-TEST(SensorBase, factory_priors_POIA_wrong)
-{
-  auto params = std::make_shared<ParamsSensorDummyPoia>();
-  params->name = "sensor1";
-  params->noise_p_std = noise_p_std;
-  params->noise_o_std = noise_o_std;
-  
-  VectorXd i_state_3D = VectorXd::Random(5);
-
-  // missing I
-  ASSERT_THROW(FactorySensorPriors::create("SensorDummyPoia", 3, params, 
-                                           Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
-                                                   {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
-                                                   //{'I',Prior("StateBlock", i_state_3D, "initial_guess")},
-                                                   {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}})),
-               std::runtime_error);
-
-  // missing A
-  ASSERT_THROW(FactorySensorPriors::create("SensorDummyPoia", 3, params, 
-                                           Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
-                                                   {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
-                                                   {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
-                                                   /*{'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}*/})),
-               std::runtime_error);
-
-  // wrong A type (expected StateQuaternion)
-  ASSERT_THROW(FactorySensorPriors::create("SensorDummyPoia", 3, params, 
-                                           Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
-                                                   {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
-                                                   {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
-                                                   {'A',Prior("StateBlock", p_state_3D, "factor", p_std_3D)}})),
-               std::runtime_error);
-}
-
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp
index f3349edfbdc682efa42e32cc2f7884056c172a2e..23e6b46046628a022cf4fc80c7d950675a6b9927 100644
--- a/test/gtest_sensor_odom.cpp
+++ b/test/gtest_sensor_odom.cpp
@@ -316,77 +316,6 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_drift_3D)
   checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////// CONSTRUCTOR WITH PARAM SERVER and KEY_TYPES ////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-TEST(SensorOdom, makeshared_server)
-{
-  std::vector<int> dims({2, 3});
-  std::vector<std::string> modes({"fix", "initial_guess", "factor"});
-  std::vector<bool> dynamics({false, true});
-  std::vector<bool> drifts({false, true});
-  std::vector<bool> wrongs({false, true});
-
-  VectorXd p_state(4), o_state(4), po_std(4);
-  p_state << 1, 2, 3, 4;
-  o_state << 1, 0, 0, 0;
-  po_std << 0.1, 0.2, 0.3, 0.4;
-
-  // P & O
-  for (auto dim : dims)
-    for (auto mode : modes)
-      for (auto dynamic : dynamics)
-        for (auto drift : drifts)
-          for (auto wrong : wrongs)
-          {
-            // nonsense combination
-            if (not dynamic and drift)
-              continue;
-
-            std::string name = "sensor_PO_" + 
-                               toString(dim) + 
-                               "D_" + 
-                               mode + 
-                               (dynamic ? "_dynamic" : "") + 
-                               (drift ? "_drift" : "") + 
-                               (wrong ? "_wrong" : "");
-
-            // Yaml parser
-            ParserYaml parser   = ParserYaml(wolf_root + "/test/yaml/sensor_tests/" + name + ".yaml", true);
-            ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
-
-            WOLF_INFO("Creating sensor from ", name, ".yaml");
-
-            // CORRECT YAML
-            if (not wrong)
-            {
-              auto params = std::make_shared<ParamsSensorOdom>("sensor_1", server);
-              auto S = std::make_shared<SensorOdom>("sensor_1", dim, params, server);
-
-              auto p_size = dim;
-              auto o_size = dim == 2 ? 1 : 4;
-              auto p_size_std = mode == "factor" ? dim : 0;
-              auto o_size_std = mode == "factor" ? (dim == 2 ? 1 : 3) : 0;
-              auto p_size_std_drift = drift ? dim : 0;
-              auto o_size_std_drift = drift ? (dim == 2 ? 1 : 3) : 0;
-
-              // factors
-              ASSERT_EQ(S->getPriorFeatures().size(), mode == "factor" ? 2 : 0);
-
-              // check
-              checkSensor(S, 'P', p_state.head(p_size), mode == "fix", po_std.head(p_size_std), dynamic, po_std.head(p_size_std_drift));
-              checkSensor(S, 'O', o_state.head(o_size), mode == "fix", po_std.head(o_size_std), dynamic, po_std.head(o_size_std_drift));
-            }
-            // INCORRECT YAML
-            else
-            {
-              ASSERT_THROW(std::make_shared<SensorOdom>("sensor_1", dim, 
-                                                         std::make_shared<ParamsSensorOdom>("sensor_1", server),
-                                                         server),std::runtime_error);
-            }
-          }
-}
-
 //////////////////////////////////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////// FactorySensor ///////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -521,227 +450,6 @@ TEST(SensorOdom, factory_yaml)
           }
 }
 
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////// FactorySensorPriors ////////////////////////////////////////
-//////////////////////////////////////////////////////////////////////////////////////////////////////
-
-// 2D Fix
-TEST(SensorOdom, factory_priors_fix_2D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
-                                               {'O',Prior("O", o_state_2D)}}));
-
-  // checks
-  checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_2D, true, vector0, false, vector0);
-}
-
-// 3D Fix
-TEST(SensorOdom, factory_priors_fix_3D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
-                                               {'O',Prior("O", o_state_3D)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, true, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_3D, true, vector0, false, vector0);
-}
-
-// 2D Initial guess
-TEST(SensorOdom, factory_priors_initial_guess_2D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
-                                               {'O',Prior("O", o_state_2D, "initial_guess")}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_2D, false, vector0, false, vector0);
-}
-
-// 3D Initial guess
-TEST(SensorOdom, factory_priors_initial_guess_3D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
-                                               {'O',Prior("O", o_state_3D, "initial_guess")}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, vector0, false, vector0);
-  checkSensor(S, 'O', o_state_3D, false, vector0, false, vector0);
-}
-
-// 2D Factor
-TEST(SensorOdom, factory_priors_factor_2D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
-                                               {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 2);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, p_std_2D, false, vector0);
-  checkSensor(S, 'O', o_state_2D, false, o_std_2D, false, vector0);
-}
-
-// 3D Factor
-TEST(SensorOdom, factory_priors_factor_3D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
-                                               {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 2);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, p_std_3D, false, vector0);
-  checkSensor(S, 'O', o_state_3D, false, o_std_3D, false, vector0);
-}
-
-// 2D Initial guess dynamic
-TEST(SensorOdom, factory_priors_initial_guess_dynamic_2D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
-                                               {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, vector0, true, vector0);
-  checkSensor(S, 'O', o_state_2D, false, vector0, true, vector0);
-}
-
-// 3D Initial guess dynamic
-TEST(SensorOdom, factory_priors_initial_guess_dynamic_3D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
-                                               {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, vector0, true, vector0);
-  checkSensor(S, 'O', o_state_3D, false, vector0, true, vector0);
-}
-
-// 2D Initial guess dynamic drift
-TEST(SensorOdom, factory_priors_initial_guess_dynamic_drift_2D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, 
-                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
-                                               {'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_2D, false, vector0, true, p_std_2D);
-  checkSensor(S, 'O', o_state_2D, false, vector0, true, o_std_2D);
-}
-
-// 3D Initial guess dynamic drift
-TEST(SensorOdom, factory_priors_initial_guess_dynamic_drift_3D)
-{
-  auto params = std::make_shared<ParamsSensorOdom>();
-  params->k_disp_to_disp = k_disp_to_disp;
-  params->k_disp_to_rot  = k_disp_to_rot;
-  params->k_rot_to_rot   = k_rot_to_rot;
-  params->min_disp_var   = min_disp_var;
-  params->min_rot_var    = min_rot_var;
-  
-  auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, 
-                                       Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
-                                               {'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
-
-  // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 0);
-
-  // check
-  checkSensor(S, 'P', p_state_3D, false, vector0, true, p_std_3D);
-  checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
-}
-
 //////////////////////////////////////////////////////////////////////////////////////////////////////
 ////////////////////////////////////////// COMPUTE NOISE COV /////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 0c3f031eb264cc8772dd2f99b9834a466e98066e..463b8298727332612945a168a69e0646db47a15a 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -44,22 +44,6 @@ TEST(TreeManager, make_shared)
     ASSERT_EQ(P->getTreeManager(), GM);
 }
 
-TEST(TreeManager, createParams)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-
-    auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
-
-    auto GM = TreeManagerDummy::create(ParamsGM);
-
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
-
-    P->setTreeManager(GM);
-
-    ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
-    ASSERT_EQ(P->getTreeManager(), GM);
-}
-
 TEST(TreeManager, createNode)
 {
     ProblemPtr P = Problem::create("PO", 2);
@@ -92,22 +76,6 @@ TEST(TreeManager, createYaml)
     ASSERT_EQ(P->getTreeManager(), GM);
 }
 
-TEST(TreeManager, FactoryParams)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-
-    auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
-
-    auto GM = FactoryTreeManagerParams::create("TreeManagerDummy",ParamsGM);
-
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
-
-    P->setTreeManager(GM);
-
-    ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
-    ASSERT_EQ(P->getTreeManager(), GM);
-}
-
 TEST(TreeManager, FactoryParam)
 {
     ProblemPtr P = Problem::create("PO", 2);
diff --git a/test/yaml/sensor_tests/sensor_POIA_3D.yaml b/test/yaml/sensor_tests/sensor_POIA_3D.yaml
index abe47172093a4e409660180baa507a469ea813f6..b2530ce9a3cd3263d9469b765f06c3db8bd5a615 100644
--- a/test/yaml/sensor_tests/sensor_POIA_3D.yaml
+++ b/test/yaml/sensor_tests/sensor_POIA_3D.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_POIA_3D:
 states:
   P:
@@ -21,8 +23,6 @@ states:
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
 
-
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml b/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
index 5cf6bffb495b7cb98ba739c6de6581668541348b..9eec9a2013de081bdf98cc1026c04d788c228f87 100644
--- a/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_POIA_3D_wrong:
 states:
   P:
@@ -21,7 +23,6 @@ states:
   #   dynamic: true
   #   drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml
index 9bfc63bcdf637b1f6081484cd45a657adce38621..96c51ec98857f076a13fdf1452b983ead63a3291 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_factor:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml
index ce4287ed5836202b2fe4717dec9007423c497e5b..aa801e760147f89b95a9be3eb6873bed37be0db1 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_factor_dynamic:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml
index d4c05d48d052a977b814f2a21454cf8d07d660e2..4c703e7da653007c9e31d798a5453fefddc8a8ab 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_factor_dynamic_drift:
 states:
   P:
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
index a6787204c54157700a27ce4f051a3404eaa8c9de..a897b3566fe78b3436603b59acf4776924903734 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 #  sensor_PO_2D_factor_dynamic_drift_wrong:
 states:
   P:
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml
index c66c96442b96409021e8dd22199354d574210db3..0f6c0630f5afbb338c2f0a3dd655e548bcd4aa33 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_factor_dynamic_wrong:
 states:
   P:
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml
index b99691f908e478f3bd0d148a21c7fc9b26773e14..1a6591ae9563c3cc7533c00daaf5a15f56563977 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_factor_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml
index e96ee1849a3a5034e05e0ce725d4dcbd7fa98828..296fd0bc3e30809d9d0996a243c88ef3f99129ec 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_fix:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml
index 715da540e07244f4e384e8189dc4b19b68d0d7ef..74f8fd2fb7cc9e25206e5e1a956c6b7d7594b674 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_fix_dynamic:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml
index 4e668002900941f2d896f323c24220931a0a02c6..2fcf456a41a983485350ff2adda00f544b1784cc 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_fix_dynamic_drift:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
index 0bcee886ca1f46fe9ec042261a498fd81ec13b58..0d257867a3d98875f466c5f10c28f84914e99959 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_fix_dynamic_drift_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml
index 1c9d8167985835870cbd2cc089de68e1006954c2..381414ba6959b81003647c3396c91b875986c90d 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_fix_dynamic_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml
index 229f89094fb3846ae70aa2bb8a5a5271d5ecd645..8f7b6cd331dff51776242bed217ca4a5e1fc686b 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_fix_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml
index f593fc53c121358f71a7b9f30d7b4d70f1735c3e..bffef6ad2b000e2da70f2cda481147d00e1e8973 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_initial_guess:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml
index 50705368a30d24e1b72e9c9af72d0f405af9225c..0ceb01c8a91c682b5c85a01ac7c5b7223bb995e7 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_initial_guess_dynamic:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml
index d5060eeaffbc0aabad00ffe8368a90cb32955cb1..faeceb5481acc6147a04b6a9c117ae290f6aae37 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_initial_guess_dynamic_drift:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
index 9a53f8300eba2490c4464822396303e0a56824e5..615a03f76f0e5e9b89dd406acefb12add8f6293b 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_initial_guess_dynamic_drift_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
index 9b3da538f19f6ed71374af434650e78aae8fe1fd..0f93bc7ba39371f83645311ac47614722cec98bd 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_initial_guess_dynamic_wrong:
 states:
   # P: #missing
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml
index 65d170426b4b6821e0667ffe196c4ddf6b8304db..d2a1cd57ae27d24521877f84b6793fc3ced00551 100644
--- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_2D_initial_guess_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml
index 23737df980572e74aa0c84e85c5cd29256d5e05a..d1b61af3b8c91d711aa78e62a29bd4cd22458279 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_factor:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1, 0.2, 0.3]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml
index 8c9342ab801b23a97cb68dd74e9577d57858e568..ccd18fe7abaf2de541f4066c1039d9a94a3da6f3 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_factor_dynamic:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1, 0.2, 0.3]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml
index 9f7e14d047a4ac2b9e2d4f783d088edd97005e35..c6840055acc9c667726f0cfae234be51fef5f690 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_factor_dynamic_drift:
 states:
   P:
@@ -13,7 +15,6 @@ states:
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
index a4980b27d66a48f1573013cb052d6ebc7f0f56fe..28c0b17450024f63676b71cf664c51d552b9f410 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_factor_dynamic_drift_wrong:
 states:
   P:
@@ -13,7 +15,6 @@ states:
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml
index 104237208910962c2753cd1a28305c89f5c10821..ec9ab37c1ae97ee7f430ca67f8bec648627ab8d2 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_factor_dynamic_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1, 0.2, 0.3, 0.4] # wrong size
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml
index d99e580743dea4589ce17618bc99024c68cd6bf1..f8440b8c7bdae155f4c6dd10eacb22cc2b974bb4 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_factor_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     noise_std: [0.1, 0.2, 0.3]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml
index 44eef97f1280399a7fe500f6695aef5b171d0143..cd91be67d1e2042efdbae999f5c67e7c74e575a7 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_fix:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 0]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml
index 606751a7ba35e7faa66c1752244c05c55a8c4e4d..52ea8fc4077dc64400c9291c2e1619805c932585 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_fix_dynamic:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 0]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml
index f6166c91b00ab312bc72fa54e1e117553ce8ee2d..4864053e65fb4868a8fbae956d76a817d07d58f8 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_fix_dynamic_drift:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
index 17467074e5baef59a958cdf2c7c1b438de2ef371..3536ded545a1ee0ca9181e1bf85296301dc6712e 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_fix_dynamic_drift_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     #dynamic: true #missing
     drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml
index 4e6da9d215eaab580db71827212cfab78f19db28..f7cd8d86288a37914935fb9f7b5465e925de7a1b 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_fix_dynamic_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 1] # not normalized
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml
index acc56356647c3791c3745520f8218bf0f005cd89..23aae62a001e31d7fea3a01b744b593619f8fd86 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_fix_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 0]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 # noise_o_std: 0.01 #missing
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml
index 7b5e1398db21a1fb70147ddf976a1f0c801a4e24..86488c0b2aee778d669ebddf319a0dddbb6a72d9 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_initial_guess:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 0]
     dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml
index e758e87035caac6b14b72ac9baf4e7fef054ed0c..1a0c2d774ebdc7bef0b6e62b7aef5c948117fca3 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_initial_guess_dynamic:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 0]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml
index 1de0b8d93e365ceb6584a2bf2eaedef2e764171f..1991d7089e2dc37bc465188f45f78c0f4cb25a35 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_initial_guess_dynamic_drift:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
index 5322cd09f7b22c20c7acb43d7fe45c00495e8ee6..6427498061af221bd3e0784488a02442b4b1d34e 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_initial_guess_dynamic_drift_wrong:
 states:
   P:
@@ -11,7 +13,6 @@ states:
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
index 873cf74259327908d2840c070d087a53fc8778cd..ee02d66071af5be52c9cc50d56b97ac7c59a7a3e 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_initial_guess_dynamic_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
     state: [1, 0, 0, 0]
     dynamic: true
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01
diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml
index 5eed19d6210ca7729c35e813edf94c2b23dd26ba..8d74c207de543a738d296acf12bbf6dfe4cb5f96 100644
--- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml
+++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml
@@ -1,3 +1,5 @@
+name: sensor_1
+
 # sensor_PO_3D_initial_guess_wrong:
 states:
   P:
@@ -9,7 +11,6 @@ states:
   #   state: [1, 0, 0, 0]
   #   dynamic: false
 
-
 # used in gtest_sensor_base
 noise_p_std: 0.1
 noise_o_std: 0.01