diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
index 3a77700e87d23ffdc58418046c6da4f573d55d93..d51b407e3e33ba37ab087bd12373c68bba6f51a3 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.h
@@ -69,27 +69,21 @@ namespace wolf
  * Sensor creators have the following API:
  *
  *     \code
- *     static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _params_extrinsics, ParamsSensorBasePtr _params_sensor);
+ *     static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const ParamsServer& _server);
  *     \endcode
  *
  * They follow the general implementation shown below:
  *
  *     \code
- *      static SensorBasePtr create(const std::string& _unique_name, Eigen::VectorXd& _params_extrinsics, ParamsSensorBasePtr _params_sensor)
+ *      static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const ParamsServer& _server)
  *      {
- *          // check extrinsics vector --- example: 3D pose
- *          assert(_params_extrinsics.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d.");
- *
- *          // cast sensor parameters to good type --- example: ParamsSensorCamera
- *          auto intrinsics_ptr = std::static_pointer_cast<ParamsSensorCamera>(_params_sensor);
- *
+ *          // Do create the Sensor Parameters --- example: ParamsSensorCamera
+ *          auto params = std::make_shared<ParamsSensorCamera>(_unique_name, _server);
+ *          
  *          // Do create the Sensor object --- example: SensorCamera
- *          auto sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr);
- *
- *          // Complete the sensor setup with a unique name identifying the sensor
- *          sen_ptr->setName(_unique_name);
- *
- *          return sen_ptr;
+ *          auto sensor = std::make_shared<SensorCamera>(_unique_name, _dim, params, _server);
+ *                                                                                                                   
+ *          return sensor;                               
  *      }
  *     \endcode
  *
@@ -97,42 +91,27 @@ namespace wolf
  * Note: Prior to invoking the creation of a sensor of a particular type,
  * you must register the creator for this type into the factory.
  *
- * To create e.g. a SensorCamera, you type:
+ * To create e.g. a SensorCamera in 3D, you type:
  *
  *     \code
- *     auto camera_ptr = FactorySensor::create("SensorCamera", "Front-left camera", params_extrinsics, params_camera);
+ *     auto camera_ptr = FactorySensor::create("SensorCamera", "Front-left camera", 3, param_server);
  *     \endcode
  *
  * where ABSOLUTELY ALL input parameters are important. In particular, the sensor name "Front-left camera" will be used to identify this camera
  * and to assign it the appropriate processors. DO NOT USE IT WITH DUMMY PARAMETERS!
  *
- * #### See also
- *  - FactoryParamsSensor: to create intrinsic structs deriving from ParamsSensorBase directly from YAML files.
- *  - FactoryProcessor: to create processors that will be bound to sensors.
- *  - Problem::installSensor() : to install sensors in WOLF Problem.
- *
- * #### Example 1: writing a SensorCamera creator
- * Here is an example of SensorCamera::create() extracted from sensor_camera.cpp:
- *
+ * We RECOMMEND using the macro WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass) to automatically add the sensor creator, provided in 'sensor_base.h'.
+ * 
+ * To do so, you should implement a constructor with the API:
+ * 
  *     \code
- *      static SensorBasePtr create(const std::string& _unique_name, Eigen::VectorXd& _extrinsics_pq, ParamsSensorBasePtr _intrinsics)
- *      {
- *          // check extrinsics vector
- *          assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d.");
- *
- *          // cast instrinsics to good type
- *          auto intrinsics_ptr = std::static_pointer_cast<ParamsSensorCamera>(_intrinsics);
- *
- *          // Do create the SensorCamera object, and complete its setup
- *          auto sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr);
- *
- *          sen_ptr->setName(_unique_name);
- *
- *          return sen_ptr;
- *      }
+ *     SensorDerived(const std::string& _unique_name, 
+ *                   SizeEigen _dim, 
+ *                   ParamsSensorDummyPtr _params,
+ *                   const ParamsServer& _server)
  *     \endcode
  *
- * #### Example 2: registering a sensor creator into the factory
+ * #### Registering sensor creator into the factory
  * Registration can be done manually or automatically. It involves the call to static functions.
  * It is advisable to put these calls within unnamed namespaces.
  *
@@ -159,7 +138,7 @@ namespace wolf
  *      const bool registered_camera = FactorySensor::registerCreator("SensorCamera", SensorCamera::create);
  *      }
  *      \endcode
- *   Automatic registration is recommended in wolf, and implemented in the classes shipped with it.
+ *   Automatic registration is recommended in wolf, and implemented in the classes shipped with it using the macro WOLF_REGISTER_SENSOR(SensorType).
  *   You are free to comment out these lines and place them wherever you consider it more convenient for your design.
  *
  * #### Example 2: creating sensors
@@ -177,15 +156,8 @@ namespace wolf
  *      // To create a camera, provide:
  *      //    a type = "SensorCamera",
  *      //    a name = "Front-left camera",
- *      //    an extrinsics vector, and
- *      //    a pointer to the intrinsics struct:
- *
- *      Eigen::VectorXd        extrinsics_1(7);        // give it some values...
- *
- *      // Create a pointer to the struct of sensor parameters stored in a YAML file ( see FactoryParamsSensor )
- *      ParamsSensorCameraPtr  intrinsics_1 =
- *          FactoryParamsSensor::create("ParamsSensorCamera",
- *                                            camera_1.yaml);
+ *      //    the problem dimension dim = 3, and
+ *      //    the parameter server or a yaml file
  *
  *      SensorBasePtr camera_1_ptr =
  *          FactorySensor::create ( "SensorCamera" ,
@@ -211,47 +183,34 @@ namespace wolf
  *  }
  *  \endcode
  *
+ * #### See also
+ *  - FactoryProcessor: to create processors that will be bound to sensors.
+ *  - Problem::installSensor() : to install sensors in WOLF Problem.
  */
+
 typedef Factory<SensorBase,
                 const std::string&,
-                const Eigen::VectorXd&,
-                const ParamsSensorBasePtr> FactorySensor;
-
-template<>
-inline std::string FactorySensor::getClass() const
-{
-  return "FactorySensor";
-}
-
-// ParamsSensor factory
-struct ParamsSensorBase;
-typedef Factory<ParamsSensorBase,
-                const std::string&> FactoryParamsSensor;
-template<>
-inline std::string FactoryParamsSensor::getClass() const
-{
-    return "FactoryParamsSensor";
-}
-
-#define WOLF_REGISTER_SENSOR(SensorType)                            \
-  namespace{ const bool WOLF_UNUSED SensorType##Registered =                    \
-    FactorySensor::registerCreator(#SensorType, SensorType::create); }     \
-
+                SizeEigen,
+                const ParamsServer&> FactorySensor;
 
 typedef Factory<SensorBase,
                 const std::string&,
-                const ParamsServer&> AutoConfFactorySensor;
+                SizeEigen,
+                const std::string&> FactorySensorYaml;
 
 template<>
-inline std::string AutoConfFactorySensor::getClass() const
+inline std::string FactorySensor::getClass() const
 {
-  return "AutoConfFactorySensor";
+  return "FactorySensor";
 }
 
-#define WOLF_REGISTER_SENSOR_AUTO(SensorType)                               \
-  namespace{ const bool WOLF_UNUSED SensorType##AutoConfRegistered =                    \
-     AutoConfFactorySensor::registerCreator(#SensorType, SensorType::create); }    \
+#define WOLF_REGISTER_SENSOR(SensorType)                                \
+  namespace{ const bool WOLF_UNUSED SensorType##Registered =            \
+     FactorySensor::registerCreator(#SensorType, SensorType::create); } \
 
+#define WOLF_REGISTER_SENSOR_YAML(SensorType)                               \
+  namespace{ const bool WOLF_UNUSED SensorType##YamlRegistered =            \
+     FactorySensorYaml::registerCreator(#SensorType, SensorType::create); } \
 
 } /* namespace wolf */
 
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 1157e55c2a5613a3bf3e43fdc5bcfca4fc275103..cf693f57c9b8b3fa8de09299d01c7b69ddbe968c 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -37,6 +37,7 @@ class ParamsServer;
 #include "core/common/time_stamp.h"
 #include "core/common/params_base.h"
 #include "core/state_block/has_state_blocks.h"
+#include "core/yaml/parser_yaml.h"
 
 //std includes
 
@@ -51,54 +52,49 @@ namespace wolf {
  * In order to use this macro, the derived sensor class, SensorClass,
  * must have a constructor available with the API:
  *
- *   SensorClass(const VectorXd & _extrinsics, const ParamsSensorClassPtr _intrinsics);
+ *   SensorClass(const std::string& _unique_name, 
+ *               SizeEigen _dim, 
+ *               const ParamsServer& _server,
+ *               ParamsSensorClassPtr _params)
  *
- * We recommend writing one of such constructors in your derived sensors.
  */
-#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass, ExtrinsicsSize)                                                      \
-static                                                                                                                          \
-SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _server)                                              \
-{                                                                                                                               \
-    Eigen::VectorXd extrinsics = _server.template getParam<Eigen::VectorXd>("sensor/" + _unique_name + "/extrinsic/pose");      \
-                                                                                                                                \
-    assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length.");                                             \
-                                                                                                                                \
-    auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server);                                                   \
-                                                                                                                                \
-    auto sensor = std::make_shared<SensorClass>(extrinsics, params);                                                            \
-                                                                                                                                \
-    sensor      ->setName(_unique_name);                                                                                        \
-                                                                                                                                \
-    return sensor;                                                                                                              \
-}                                                                                                                               \
-                                                                                                                                \
-static                                                                                                                          \
-SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, const ParamsSensorBasePtr _intrinsics)\
-{                                                                                                                               \
-    assert(_extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length.");                                            \
-                                                                                                                                \
-    auto params = std::static_pointer_cast<ParamsSensorClass>(_intrinsics);                                                     \
-                                                                                                                                \
-    auto sensor = std::make_shared<SensorClass>(_extrinsics, params);                                                           \
-                                                                                                                                \
-    sensor      ->setName(_unique_name);                                                                                        \
-                                                                                                                                \
-    return sensor;                                                                                                              \
-}                                                                                                                               \
-
+#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass)                                                  \
+static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const ParamsServer& _server)   \
+{                                                                                                           \
+    auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server);                               \
+                                                                                                            \
+    auto sensor = std::make_shared<SensorClass>(_unique_name, _dim, params, _server);                       \
+                                                                                                            \
+    return sensor;                                                                                          \
+}                                                                                                           \
+static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const std::string& _yaml_file) \
+{                                                                                                           \
+    auto parser = ParserYaml(_yaml_file, "", true);                                                         \
+                                                                                                            \
+    auto server = ParamsServer(parser.getParams());                                                         \
+                                                                                                            \
+    auto params = std::make_shared<ParamsSensorClass>(_unique_name, server);                                \
+                                                                                                            \
+    auto sensor = std::make_shared<SensorClass>(_unique_name, _dim, params, server);                        \
+                                                                                                            \
+    return sensor;                                                                                          \
+}                                                                                                           \
 
 /** \brief base struct for intrinsic sensor parameters
  *
  * Derive from this struct to create structs of sensor intrinsic parameters.
  */
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorBase);
 struct ParamsSensorBase: public ParamsBase
 {
+    std::string prefix = "sensor/";
+
     Eigen::VectorXd noise_std;
 
     ParamsSensorBase() = default;
-    ParamsSensorBase(std::string prefix, const wolf::ParamsServer& _server)
+    ParamsSensorBase(std::string _unique_name, const wolf::ParamsServer& _server)
     {
-        noise_std = _server.getParam<Eigen::VectorXd>(prefix + "/noise_std");
+        noise_std = _server.getParam<Eigen::VectorXd>(prefix + _unique_name + "/noise_std");
     }
     ~ParamsSensorBase() override = default;
 
@@ -108,6 +104,7 @@ struct ParamsSensorBase: public ParamsBase
     }
 };
 
+WOLF_PTR_TYPEDEFS(SensorBase);
 class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<SensorBase>
 {
     friend Problem;
@@ -121,6 +118,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
 
         unsigned int sensor_id_;   // sensor ID
 
+        ParamsSensorBasePtr params_; ///< Params struct
+
         std::map<char, bool> state_block_dynamic_; // mark dynamic state blocks
 
         std::map<char, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
@@ -128,11 +127,9 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
 
     protected:
-        Eigen::MatrixXd noise_cov_;      // cov matrix of noise
         std::map<char, Eigen::MatrixXd> drift_covs_; // covariance of the drift of dynamic state blocks [unit²/s]
 
         void setProblem(ProblemPtr _problem) override final;
-        virtual void loadParams(ParamsSensorBasePtr _params);
         virtual void loadPriors(const Priors& _priors, SizeEigen _dim);
 
     public:
@@ -143,15 +140,15 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
          * \param _tp Type of the sensor  (types defined at wolf.h)
          * \param _unique_name Name of the sensor
          * \param _dim Problem dimension
-         * \param _priors priors of the sensor state blocks
          * \param _params params struct
+         * \param _priors priors of the sensor state blocks
          *
          **/
         SensorBase(const std::string& _type,
                    const std::string& _unique_name,
                    const SizeEigen& _dim,
-                   const Priors& _priors,
-                   ParamsSensorBasePtr _params);
+                   ParamsSensorBasePtr _params,
+                   const Priors& _priors);
 
         /** \brief Constructor with ParamServer and keys
          *
@@ -160,6 +157,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
          * \param _tp Type of the sensor  (types defined at wolf.h)
          * \param _unique_name Name of the sensor
          * \param _dim Problem dimension
+         * \param _params params struct
          * \param _server parameter server
          * \param _keys_types_apart_from_PO Map containing the keys and the types of the state blocks (apart from extrinsics: P, O)
          *
@@ -167,6 +165,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         SensorBase(const std::string& _type,
                    const std::string& _unique_name,
                    const SizeEigen& _dim,
+                   ParamsSensorBasePtr _params,
                    const ParamsServer& _server,
                    const std::unordered_map<char, std::string>&  _keys_types_apart_from_PO={});
 
@@ -364,12 +363,14 @@ inline bool SensorBase::isStateBlockDynamic(const char& _key) const
 
 inline Eigen::VectorXd SensorBase::getNoiseStd() const
 {
-    return noise_cov_.diagonal().cwiseSqrt();
+    assert(params_ != nullptr);
+    return params_->noise_std;
 }
 
 inline Eigen::MatrixXd SensorBase::getNoiseCov() const
 {
-    return noise_cov_;
+    assert(params_ != nullptr);
+    return params_->noise_std.cwiseAbs2().asDiagonal();
 }
 
 inline Eigen::VectorXd SensorBase::getDriftStd(char _key) const
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index a734043ce8a94776831d3ed43819d0c321935e16..c06fc2ba6c430232b0bd34ee727e1fffc44faed6 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -32,14 +32,11 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom2d);
 
 struct ParamsSensorOdom2d : public ParamsSensorBase
 {
-        ~ParamsSensorOdom2d() override = default;
+    ~ParamsSensorOdom2d() override = default;
 
-        double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
-        double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
-    ParamsSensorOdom2d()
-    {
-        //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
-    }
+    double k_disp_to_disp;  ///< ratio of displacement variance to displacement, for odometry noise calculation
+    double k_rot_to_rot;    ///< ratio of rotation variance to rotation, for odometry noise calculation
+    
     ParamsSensorOdom2d(std::string _unique_name, const ParamsServer& _server):
         ParamsSensorBase(_unique_name, _server)
     {
@@ -63,9 +60,15 @@ class SensorOdom2d : public SensorBase
         double k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
 
 	public:
-        SensorOdom2d(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2d& _intrinsics);
-        SensorOdom2d(Eigen::VectorXd _extrinsics, ParamsSensorOdom2dPtr _intrinsics);
-        WOLF_SENSOR_CREATE(SensorOdom2d, ParamsSensorOdom2d, 3);
+        SensorOdom2d(const std::string& _unique_name,
+                     const SizeEigen& _dim,
+                     const Priors& _priors,
+                     ParamsSensorOdom2d _params);
+        SensorOdom2d(const std::string& _unique_name,
+                     const SizeEigen& _dim,
+                     const ParamsServer& _server,
+                     ParamsSensorOdom2d _params);
+        WOLF_SENSOR_CREATE(SensorOdom2d, ParamsSensorOdom2d);
 
         ~SensorOdom2d() override;
 
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 604770acc4629ba97125e1da097d6e5871083983..dc064e6b7d6b234132e2ece6127246811d23aa7e 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -214,9 +214,10 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
                                      const Eigen::VectorXd& _extrinsics, //
                                      ParamsSensorBasePtr _intrinsics)
 {
-    SensorBasePtr sen_ptr = FactorySensor::create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics);
-    sen_ptr->link(getHardware());
-    return sen_ptr;
+    //SensorBasePtr sen_ptr = FactorySensor::create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics);
+    // sen_ptr->link(getHardware());
+    // return sen_ptr;
+    return nullptr;
 }
 
 SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
@@ -228,8 +229,8 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
     if (_intrinsics_filename != "")
     {
         assert(file_exists(_intrinsics_filename) && "Cannot install sensor: intrinsics' YAML file does not exist.");
-        ParamsSensorBasePtr intr_ptr = FactoryParamsSensor::create(_sen_type, _intrinsics_filename);
-        return installSensor(_sen_type, _unique_sensor_name, _extrinsics, intr_ptr);
+        //ParamsSensorBasePtr intr_ptr = FactoryParamsSensor::create(_sen_type, _intrinsics_filename);
+        return installSensor(_sen_type, _unique_sensor_name, _extrinsics, ParamsSensorBasePtr());
     }
     else
         return installSensor(_sen_type, _unique_sensor_name, _extrinsics, ParamsSensorBasePtr());
@@ -240,7 +241,8 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
                                      const std::string& _unique_sensor_name, //
                                      const ParamsServer& _server)
 {
-    SensorBasePtr sen_ptr = AutoConfFactorySensor::create(_sen_type, _unique_sensor_name, _server);
+    //ParamsSensorBasePtr params = FactoryParamsSensor::create(_sen_type, _unique_sensor_name, _server);
+    SensorBasePtr sen_ptr = FactorySensor::create(_sen_type, _unique_sensor_name, dim_, _server);
     sen_ptr->link(getHardware());
     return sen_ptr;
 }
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index ecd199c2ef098d4e942dc777018226938b30432e..47fc91f406d062fd3da00df7322d79f76a36fb51 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -36,22 +36,19 @@ unsigned int SensorBase::sensor_id_count_ = 0;
 SensorBase::SensorBase(const std::string& _type,
                        const std::string& _unique_name,
                        const SizeEigen& _dim,
-                       const Priors& _priors,
-                       ParamsSensorBasePtr _params) :
+                       ParamsSensorBasePtr _params,
+                       const Priors& _priors) :
         NodeBase("SENSOR", _type, _unique_name),
         HasStateBlocks(""),
         hardware_ptr_(),
         sensor_id_(++sensor_id_count_), // simple ID factory
+        params_(_params),
         state_block_dynamic_(),
         params_prior_map_(),
-        last_capture_(nullptr),
-        noise_cov_(0,0)
+        last_capture_(nullptr)
 {
     assert(_dim == 2 or _dim == 3);
 
-    // load params
-    loadParams(_params);
-
     // load priors
     loadPriors(_priors, _dim);
 }
@@ -59,12 +56,14 @@ SensorBase::SensorBase(const std::string& _type,
 SensorBase::SensorBase(const std::string& _type,
                        const std::string& _unique_name,
                        const SizeEigen& _dim,
+                       ParamsSensorBasePtr _params,
                        const ParamsServer& _server,
                        const std::unordered_map<char, std::string>& _keys_types_apart_from_PO) :
         NodeBase("SENSOR", _type, _unique_name),
         HasStateBlocks(""),
         hardware_ptr_(),
         sensor_id_(++sensor_id_count_), // simple ID factory
+        params_(_params),
         state_block_dynamic_(),
         params_prior_map_(),
         last_capture_(nullptr)
@@ -74,19 +73,13 @@ SensorBase::SensorBase(const std::string& _type,
            _keys_types_apart_from_PO.count('O') == 0 and 
            "SensorBase: _keys_types_apart_from_PO should not contain 'P' or 'O' keys");
 
-    // create params
-    auto params = std::make_shared<ParamsSensorBase>("sensor/" + _unique_name, _server);
-
-    // load params
-    loadParams(params);
-
     // create priors
     std::unordered_map<char, std::string> keys_types = _keys_types_apart_from_PO;
     keys_types['P'] = "P"; // equivalent to "StateBlock";
     keys_types['O'] = "O"; // equivalent to (_dim == 2 ? "StateAngle" : "StateQuaternion");
     Priors priors;
     for (auto pair : keys_types)
-        priors[pair.first] = Prior("sensor/" + _unique_name + "/" + std::string(1, pair.first), pair.second, _server);
+        priors[pair.first] = Prior("sensor/" + _unique_name + "/states/" + std::string(1, pair.first), pair.second, _server);
 
     // load priors
     loadPriors(priors, _dim);
@@ -98,11 +91,6 @@ SensorBase::~SensorBase()
     removeStateBlocks(getProblem());
 }
 
-void SensorBase::loadParams(ParamsSensorBasePtr _params)
-{
-    setNoiseStd(_params->noise_std);
-}
-
 void SensorBase::loadPriors(const Priors& _priors, SizeEigen _dim)
 {
     assert(_dim == 2 or _dim == 3);
@@ -286,12 +274,14 @@ void SensorBase::registerNewStateBlocks(ProblemPtr _problem)
 
 void SensorBase::setNoiseStd(const Eigen::VectorXd& _noise_std) 
 {
-    noise_cov_ = _noise_std.cwiseAbs2().asDiagonal();
+    assert(params_ != nullptr);
+    params_->noise_std = _noise_std;
 }
 
 void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) 
 {
-    noise_cov_ = _noise_cov;
+    assert(params_ != nullptr);
+    params_->noise_std = _noise_cov.diagonal().cwiseSqrt();
 }
 
 void SensorBase::setDriftStd(char _key, const Eigen::VectorXd & _drift_rate_std)
diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..8b37639c862202dab8014f3f4ad2ef472ee3ad3b
--- /dev/null
+++ b/test/dummy/sensor_dummy.h
@@ -0,0 +1,98 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef SENSOR_DUMMY_H_
+#define SENSOR_DUMMY_H_
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/sensor/sensor_base.h"
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDummy);
+
+struct ParamsSensorDummy : public ParamsSensorBase
+{
+    ~ParamsSensorDummy() override = default;
+
+    double param1;
+    int param2;
+    
+    ParamsSensorDummy(std::string _unique_name, const ParamsServer& _server):
+        ParamsSensorBase(_unique_name, _server)
+    {
+        param1 = _server.getParam<double>(prefix + _unique_name + "/param1");
+        param2 = _server.getParam<int>(prefix + _unique_name + "/param2");
+    }
+    std::string print() const override
+    {
+        return ParamsSensorBase::print()                    + "\n"
+                + "param1: "    + std::to_string(param1)    + "\n"
+                + "param2: "    + std::to_string(param2)    + "\n";
+    }
+};
+
+WOLF_PTR_TYPEDEFS(SensorDummy);
+
+class SensorDummy : public SensorBase
+{
+    public:
+        SensorDummy(const std::string& _unique_name, 
+                    SizeEigen _dim, 
+                    ParamsSensorDummyPtr _params,
+                    const ParamsServer& _server) :
+            SensorBase("SensorDummy",
+                       _unique_name,
+                       _dim,
+                       _params,
+                       _server,
+                       std::unordered_map<char, std::string>{{'I',"StateBlock"},{'A',"StateQuaternion"}})
+        {
+        }
+        WOLF_SENSOR_CREATE(SensorDummy, ParamsSensorDummy);
+
+
+        SensorDummy(const std::string& _unique_name, 
+                    SizeEigen _dim, 
+                    ParamsSensorDummyPtr _params,
+                    const Priors& _priors) :
+            SensorBase("SensorDummy",
+                       _unique_name,
+                       _dim,
+                       _params,
+                       _priors)
+        {
+        }
+
+        virtual ~SensorDummy(){};
+};
+
+}
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorDummy);
+WOLF_REGISTER_SENSOR_YAML(SensorDummy);
+} // namespace wolf
+#endif
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 1b98fed3c06c6f08977be4305116170bae8ebf5b..6d7e6bcab14701fcc3669cfb9b62ff0baaff2189 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -30,6 +30,7 @@
 #include "core/utils/utils_gtest.h"
 #include "core/yaml/parser_yaml.h"
 #include "core/utils/params_server.h"
+#include "dummy/sensor_dummy.h"
 
 using namespace wolf;
 using namespace Eigen;
@@ -103,10 +104,9 @@ TEST(SensorBase, POfix2D)
   auto params = std::make_shared<ParamsSensorBase>();
   params->noise_std = noise_std;
   
-  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2,
+  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, params, 
                                         Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
-                                                {'O',Prior("O", o_state_2D)}}),
-                                        params);
+                                                {'O',Prior("O", o_state_2D)}}));
 
   // noise
   ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
@@ -123,10 +123,9 @@ TEST(SensorBase, POfix3D)
   auto params = std::make_shared<ParamsSensorBase>();
   params->noise_std = noise_std;
   
-  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3,
+  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params, 
                                         Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
-                                                {'O',Prior("O", o_state_3D)}}),
-                                        params);
+                                                {'O',Prior("O", o_state_3D)}}));
 
   // noise
   ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
@@ -146,10 +145,9 @@ TEST(SensorBase, POinitial_guess2D)
   auto params = std::make_shared<ParamsSensorBase>();
   params->noise_std = noise_std;
   
-  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2,
+  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, params, 
                                         Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
-                                                {'O',Prior("O", o_state_2D, "initial_guess")}}),
-                                        params);
+                                                {'O',Prior("O", o_state_2D, "initial_guess")}}));
 
   // noise
   ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
@@ -169,10 +167,9 @@ TEST(SensorBase, POinitial_guess3D)
   auto params = std::make_shared<ParamsSensorBase>();
   params->noise_std = noise_std;
   
-  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3,
+  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params, 
                                         Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
-                                                {'O',Prior("O", o_state_3D, "initial_guess")}}),
-                                        params);
+                                                {'O',Prior("O", o_state_3D, "initial_guess")}}));
 
   // noise
   ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
@@ -192,10 +189,9 @@ TEST(SensorBase, POfactor2D)
   auto params = std::make_shared<ParamsSensorBase>();
   params->noise_std = noise_std;
   
-  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2,
+  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, params, 
                                         Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
-                                                {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}),
-                                        params);
+                                                {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
 
   // noise
   ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
@@ -215,10 +211,9 @@ TEST(SensorBase, POfactor3D)
   auto params = std::make_shared<ParamsSensorBase>();
   params->noise_std = noise_std;
   
-  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3,
+  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params, 
                                         Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
-                                                {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}),
-                                        params);
+                                                {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
 
   // noise
   ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
@@ -238,10 +233,9 @@ TEST(SensorBase, POinitial_guess_dynamic2D)
   auto params = std::make_shared<ParamsSensorBase>();
   params->noise_std = noise_std;
   
-  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2,
+  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, params, 
                                         Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
-                                                {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}),
-                                        params);
+                                                {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
 
   // noise
   ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
@@ -261,10 +255,9 @@ TEST(SensorBase, POinitial_guess_dynamic3D)
   auto params = std::make_shared<ParamsSensorBase>();
   params->noise_std = noise_std;
   
-  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3,
+  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params, 
                                         Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
-                                                {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}),
-                                        params);
+                                                {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
 
   // noise
   ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
@@ -284,10 +277,9 @@ TEST(SensorBase, POinitial_guess_dynamic2D_drift)
   auto params = std::make_shared<ParamsSensorBase>();
   params->noise_std = noise_std;
   
-  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2,
+  auto S = std::make_shared<SensorBase>("SensorBase", "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)}}),
-                                        params);
+                                                {'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
 
   // noise
   ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
@@ -307,10 +299,9 @@ TEST(SensorBase, POinitial_guess_dynamic3D_drift)
   auto params = std::make_shared<ParamsSensorBase>();
   params->noise_std = noise_std;
   
-  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3,
+  auto S = std::make_shared<SensorBase>("SensorBase", "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)}}),
-                                        params);
+                                                {'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
 
   // noise
   ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
@@ -332,12 +323,11 @@ TEST(SensorBase, POI_mixed)
   
   VectorXd i_state_3D = VectorXd::Random(5);
 
-  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3,
+  auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 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)}}),
-                                        params);
+                                                {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
 
   // noise
   ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
@@ -384,7 +374,8 @@ TEST(SensorBase, POfix2D_server)
           std::string name = "sensor_PO_" + to_string(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
           WOLF_INFO("Creating sensor from name ", name);
 
-          auto S = std::make_shared<SensorBase>("SensorBase", name, dim, server);
+          auto params = std::make_shared<ParamsSensorBase>(name, server);
+          auto S = std::make_shared<SensorBase>("SensorBase", name, dim, params, server);
 
           auto p_size = dim;
           auto o_size = dim == 2 ? 1 : 4;
@@ -407,17 +398,16 @@ TEST(SensorBase, POfix2D_server)
 
           // INCORRECT YAML
           WOLF_INFO("Creating sensor from name ", name + "_wrong");
-          ASSERT_THROW(std::make_shared<SensorBase>("SensorBase", name + "_wrong", dim, server),std::runtime_error);
+          ASSERT_THROW(std::make_shared<SensorBase>("SensorBase", name + "_wrong", dim, 
+                                                    std::make_shared<ParamsSensorBase>(name + "_wrong", server),
+                                                    server),std::runtime_error);
         }
 
-  // P & O & I & A - 3D
-  // CORRECT YAML
+  // POIA - 3D - CORRECT YAML
   WOLF_INFO("Creating sensor from name sensor_POIA_3D");
 
-  auto S = std::make_shared<SensorBase>("SensorBase", 
-                                        "sensor_POIA_3D", 
-                                        3, 
-                                        server, 
+  auto params = std::make_shared<ParamsSensorBase>("sensor_POIA_3D", server);
+  auto S = std::make_shared<SensorBase>("SensorBase", "sensor_POIA_3D", 3, params, server, 
                                         std::unordered_map<char, std::string>({{'I',"StateBlock"},
                                                                                {'A',"StateQuaternion"}}));
 
@@ -433,6 +423,125 @@ TEST(SensorBase, POfix2D_server)
   testSensor(S, 'O', o_state.head(4), true,  vector0,        false, vector0);
   testSensor(S, 'I', p_state.head(4), false, vector0,        true,  po_std.head(4));
   testSensor(S, 'A', o_state.head(4), false, po_std.head(3), true,  po_std.head(3));
+
+  // POIA - 3D - INCORRECT YAML
+  WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong");
+
+  ASSERT_THROW(std::make_shared<SensorBase>("SensorBase", "sensor_POIA_3D_wrong", 3,
+                                            std::make_shared<ParamsSensorBase>("sensor_POIA_3D_wrong", server), 
+                                            server,
+                                            std::unordered_map<char, std::string>({{'I',"StateBlock"},
+                                                                                  {'A',"StateQuaternion"}})),
+               std::runtime_error);
+}
+
+// DUMMY CLASS
+TEST(SensorBase, dummy_make_shared)
+{
+  ParserYaml parser   = ParserYaml("test/yaml/sensor_dummy.yaml", wolf_root, true);
+  ParamsServer server = ParamsServer(parser.getParams());
+
+  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;
+
+  // POIA - 3D - CORRECT YAML
+  WOLF_INFO("Creating sensor from name sensor_dummy_1");
+
+  auto params = std::make_shared<ParamsSensorDummy>("sensor_dummy_1", server);
+  auto S = std::make_shared<SensorDummy>("sensor_dummy_1", 3, params, server);
+
+  // noise
+  ASSERT_MATRIX_APPROX(S->getNoiseStd(), po_std.head<2>(), Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->getNoiseCov(), MatrixXd(po_std.head<2>().cwiseAbs2().asDiagonal()), Constants::EPS);
+
+  // factors
+  ASSERT_EQ(S->getPriorFeatures().size(), 2);
+
+  // check
+  testSensor(S, 'P', p_state.head(3), false, po_std.head(3), true,  vector0);
+  testSensor(S, 'O', o_state.head(4), true,  vector0,        false, vector0);
+  testSensor(S, 'I', p_state.head(4), false, vector0,        true,  po_std.head(4));
+  testSensor(S, 'A', o_state.head(4), false, po_std.head(3), true,  po_std.head(3));
+
+
+  // POIA - 3D - INCORRECT YAML
+  WOLF_INFO("Creating sensor from name sensor_dummy_1_wrong");
+
+  ASSERT_THROW(std::make_shared<SensorDummy>("sensor_dummy_1_wrong", 3,
+                                             std::make_shared<ParamsSensorDummy>("sensor_dummy_1_wrong", server), 
+                                             server),
+               std::runtime_error);
+}
+
+TEST(SensorBase, dummy_factory)
+{
+  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;
+
+  ParserYaml parser   = ParserYaml("test/yaml/sensor_dummy.yaml", wolf_root, true);
+  ParamsServer server = ParamsServer(parser.getParams());
+
+  // POIA - 3D - CORRECT YAML
+  WOLF_INFO("Creating sensor from name sensor_dummy_1");
+
+  auto S = FactorySensor::create("SensorDummy", "sensor_dummy_1", 3, server);
+
+  // noise
+  ASSERT_MATRIX_APPROX(S->getNoiseStd(), po_std.head<2>(), Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->getNoiseCov(), MatrixXd(po_std.head<2>().cwiseAbs2().asDiagonal()), Constants::EPS);
+
+  // factors
+  ASSERT_EQ(S->getPriorFeatures().size(), 2);
+
+  // check
+  testSensor(S, 'P', p_state.head(3), false, po_std.head(3), true,  vector0);
+  testSensor(S, 'O', o_state.head(4), true,  vector0,        false, vector0);
+  testSensor(S, 'I', p_state.head(4), false, vector0,        true,  po_std.head(4));
+  testSensor(S, 'A', o_state.head(4), false, po_std.head(3), true,  po_std.head(3));
+
+
+  // POIA - 3D - INCORRECT YAML
+  WOLF_INFO("Creating sensor from name sensor_dummy_1_wrong");
+
+  ASSERT_THROW(FactorySensor::create("SensorDummy", "sensor_dummy_1_wrong", 3, server),
+               std::runtime_error);
+}
+
+TEST(SensorBase, dummy_factory_yaml)
+{
+  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;
+
+  // POIA - 3D - CORRECT YAML
+  WOLF_INFO("Creating sensor from name sensor_dummy_1");
+
+  auto S = FactorySensorYaml::create("SensorDummy", "sensor_dummy_1", 3, wolf_root + "/test/yaml/sensor_dummy.yaml");
+
+  // noise
+  ASSERT_MATRIX_APPROX(S->getNoiseStd(), po_std.head<2>(), Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->getNoiseCov(), MatrixXd(po_std.head<2>().cwiseAbs2().asDiagonal()), Constants::EPS);
+
+  // factors
+  ASSERT_EQ(S->getPriorFeatures().size(), 2);
+
+  // check
+  testSensor(S, 'P', p_state.head(3), false, po_std.head(3), true,  vector0);
+  testSensor(S, 'O', o_state.head(4), true,  vector0,        false, vector0);
+  testSensor(S, 'I', p_state.head(4), false, vector0,        true,  po_std.head(4));
+  testSensor(S, 'A', o_state.head(4), false, po_std.head(3), true,  po_std.head(3));
+
+
+  // POIA - 3D - INCORRECT YAML
+  WOLF_INFO("Creating sensor from name sensor_dummy_1_wrong");
+
+  ASSERT_THROW(FactorySensorYaml::create("SensorDummy", "sensor_dummy_1_wrong", 3, wolf_root + "/test/yaml/sensor_dummy.yaml"),
+               std::runtime_error);
 }
 
 int main(int argc, char **argv)
diff --git a/test/yaml/sensor_base.yaml b/test/yaml/sensor_base.yaml
index 469dbd637fc87f3b381d6e339f87eb13c55852e8..11e3bbe82ce94ea8d48866cb72ca7fb25b78f052 100644
--- a/test/yaml/sensor_base.yaml
+++ b/test/yaml/sensor_base.yaml
@@ -5,248 +5,267 @@ sensor:
   #############################################################################################
 
   sensor_PO_2D_fix:
-    P:
-      mode: fix
-      state: [1, 2]
-      dynamic: false
-    O:
-      mode: fix
-      state: [1]
-      dynamic: false
+    states:
+      P:
+        mode: fix
+        state: [1, 2]
+        dynamic: false
+      O:
+        mode: fix
+        state: [1]
+        dynamic: false
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_fix:
-    P:
-      mode: fix
-      state: [1, 2, 3]
-      dynamic: false
-    O:
-      mode: fix
-      state: [1, 0, 0, 0]
-      dynamic: false
+    states:
+      P:
+        mode: fix
+        state: [1, 2, 3]
+        dynamic: false
+      O:
+        mode: fix
+        state: [1, 0, 0, 0]
+        dynamic: false
     noise_std: [0.1, 0.2]
 
   sensor_PO_2D_initial_guess:
-    P:
-      mode: initial_guess
-      state: [1, 2]
-      dynamic: false
-    O:
-      mode: initial_guess
-      state: [1]
-      dynamic: false
+    states:
+      P:
+        mode: initial_guess
+        state: [1, 2]
+        dynamic: false
+      O:
+        mode: initial_guess
+        state: [1]
+        dynamic: false
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_initial_guess:
-    P:
-      mode: initial_guess
-      state: [1, 2, 3]
-      dynamic: false
-    O:
-      mode: initial_guess
-      state: [1, 0, 0, 0]
-      dynamic: false
+    states:
+      P:
+        mode: initial_guess
+        state: [1, 2, 3]
+        dynamic: false
+      O:
+        mode: initial_guess
+        state: [1, 0, 0, 0]
+        dynamic: false
     noise_std: [0.1, 0.2]
 
   sensor_PO_2D_factor:
-    P:
-      mode: factor
-      state: [1, 2]
-      noise_std: [0.1, 0.2]
-      dynamic: false
-    O:
-      mode: factor
-      state: [1]
-      noise_std: [0.1]
-      dynamic: false
+    states:
+      P:
+        mode: factor
+        state: [1, 2]
+        noise_std: [0.1, 0.2]
+        dynamic: false
+      O:
+        mode: factor
+        state: [1]
+        noise_std: [0.1]
+        dynamic: false
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_factor:
-    P:
-      mode: factor
-      state: [1, 2, 3]
-      noise_std: [0.1, 0.2, 0.3]
-      dynamic: false
-    O:
-      mode: factor
-      state: [1, 0, 0, 0]
-      noise_std: [0.1, 0.2, 0.3]
-      dynamic: false
+    states:
+      P:
+        mode: factor
+        state: [1, 2, 3]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: false
+      O:
+        mode: factor
+        state: [1, 0, 0, 0]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: false
     noise_std: [0.1, 0.2]
   
   sensor_PO_2D_fix_dynamic:
-    P:
-      mode: fix
-      state: [1, 2]
-      dynamic: true
-    O:
-      mode: fix
-      state: [1]
-      dynamic: true
+    states:
+      P:
+        mode: fix
+        state: [1, 2]
+        dynamic: true
+      O:
+        mode: fix
+        state: [1]
+        dynamic: true
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_fix_dynamic:
-    P:
-      mode: fix
-      state: [1, 2, 3]
-      dynamic: true
-    O:
-      mode: fix
-      state: [1, 0, 0, 0]
-      dynamic: true
+    states:
+      P:
+        mode: fix
+        state: [1, 2, 3]
+        dynamic: true
+      O:
+        mode: fix
+        state: [1, 0, 0, 0]
+        dynamic: true
     noise_std: [0.1, 0.2]
 
   sensor_PO_2D_initial_guess_dynamic:
-    P:
-      mode: initial_guess
-      state: [1, 2]
-      dynamic: true
-    O:
-      mode: initial_guess
-      state: [1]
-      dynamic: true
+    states:
+      P:
+        mode: initial_guess
+        state: [1, 2]
+        dynamic: true
+      O:
+        mode: initial_guess
+        state: [1]
+        dynamic: true
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_initial_guess_dynamic:
-    P:
-      mode: initial_guess
-      state: [1, 2, 3]
-      dynamic: true
-    O:
-      mode: initial_guess
-      state: [1, 0, 0, 0]
-      dynamic: true
+    states:
+      P:
+        mode: initial_guess
+        state: [1, 2, 3]
+        dynamic: true
+      O:
+        mode: initial_guess
+        state: [1, 0, 0, 0]
+        dynamic: true
     noise_std: [0.1, 0.2]
 
   sensor_PO_2D_factor_dynamic:
-    P:
-      mode: factor
-      state: [1, 2]
-      noise_std: [0.1, 0.2]
-      dynamic: true
-    O:
-      mode: factor
-      state: [1]
-      noise_std: [0.1]
-      dynamic: true
+    states:
+      P:
+        mode: factor
+        state: [1, 2]
+        noise_std: [0.1, 0.2]
+        dynamic: true
+      O:
+        mode: factor
+        state: [1]
+        noise_std: [0.1]
+        dynamic: true
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_factor_dynamic:
-    P:
-      mode: factor
-      state: [1, 2, 3]
-      noise_std: [0.1, 0.2, 0.3]
-      dynamic: true
-    O:
-      mode: factor
-      state: [1, 0, 0, 0]
-      noise_std: [0.1, 0.2, 0.3]
-      dynamic: true
+    states:
+      P:
+        mode: factor
+        state: [1, 2, 3]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: true
+      O:
+        mode: factor
+        state: [1, 0, 0, 0]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: true
     noise_std: [0.1, 0.2]
   
   sensor_PO_2D_fix_dynamic_drift:
-    P:
-      mode: fix
-      state: [1, 2]
-      dynamic: true
-      drift_std: [0.1, 0.2]
-    O:
-      mode: fix
-      state: [1]
-      dynamic: true
-      drift_std: [0.1]
+    states:
+      P:
+        mode: fix
+        state: [1, 2]
+        dynamic: true
+        drift_std: [0.1, 0.2]
+      O:
+        mode: fix
+        state: [1]
+        dynamic: true
+        drift_std: [0.1]
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_fix_dynamic_drift:
-    P:
-      mode: fix
-      state: [1, 2, 3]
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3]
-    O:
-      mode: fix
-      state: [1, 0, 0, 0]
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3]
+    states:
+      P:
+        mode: fix
+        state: [1, 2, 3]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
+      O:
+        mode: fix
+        state: [1, 0, 0, 0]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
     noise_std: [0.1, 0.2]
 
   sensor_PO_2D_initial_guess_dynamic_drift:
-    P:
-      mode: initial_guess
-      state: [1, 2]
-      dynamic: true
-      drift_std: [0.1, 0.2]
-    O:
-      mode: initial_guess
-      state: [1]
-      dynamic: true
-      drift_std: [0.1]
+    states:
+      P:
+        mode: initial_guess
+        state: [1, 2]
+        dynamic: true
+        drift_std: [0.1, 0.2]
+      O:
+        mode: initial_guess
+        state: [1]
+        dynamic: true
+        drift_std: [0.1]
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_initial_guess_dynamic_drift:
-    P:
-      mode: initial_guess
-      state: [1, 2, 3]
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3]
-    O:
-      mode: initial_guess
-      state: [1, 0, 0, 0]
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3]
+    states:
+      P:
+        mode: initial_guess
+        state: [1, 2, 3]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
+      O:
+        mode: initial_guess
+        state: [1, 0, 0, 0]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
     noise_std: [0.1, 0.2]
 
   sensor_PO_2D_factor_dynamic_drift:
-    P:
-      mode: factor
-      state: [1, 2]
-      noise_std: [0.1, 0.2]
-      dynamic: true
-      drift_std: [0.1, 0.2]
-    O:
-      mode: factor
-      state: [1]
-      noise_std: [0.1]
-      dynamic: true
-      drift_std: [0.1]
+    states:
+      P:
+        mode: factor
+        state: [1, 2]
+        noise_std: [0.1, 0.2]
+        dynamic: true
+        drift_std: [0.1, 0.2]
+      O:
+        mode: factor
+        state: [1]
+        noise_std: [0.1]
+        dynamic: true
+        drift_std: [0.1]
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_factor_dynamic_drift:
-    P:
-      mode: factor
-      state: [1, 2, 3]
-      noise_std: [0.1, 0.2, 0.3]
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3]
-    O:
-      mode: factor
-      state: [1, 0, 0, 0]
-      noise_std: [0.1, 0.2, 0.3]
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3]
+    states:
+      P:
+        mode: factor
+        state: [1, 2, 3]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
+      O:
+        mode: factor
+        state: [1, 0, 0, 0]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
     noise_std: [0.1, 0.2]
 
   sensor_POIA_3D:
-    P:
-      mode: factor
-      state: [1, 2, 3]
-      noise_std: [0.1, 0.2, 0.3]
-      dynamic: true
-    O:
-      mode: fix
-      state: [1, 0, 0, 0]
-      dynamic: false
-    I:
-      mode: initial_guess
-      state: [1, 2, 3, 4]
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3, 0.4]
-    A:
-      mode: factor
-      state: [1, 0, 0, 0]
-      noise_std: [0.1, 0.2, 0.3]
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3]
+    states:
+      P:
+        mode: factor
+        state: [1, 2, 3]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: true
+      O:
+        mode: fix
+        state: [1, 0, 0, 0]
+        dynamic: false
+      I:
+        mode: initial_guess
+        state: [1, 2, 3, 4]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3, 0.4]
+      A:
+        mode: factor
+        state: [1, 0, 0, 0]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
     noise_std: [0.1, 0.2]
 
   #############################################################################################
@@ -254,223 +273,265 @@ sensor:
   #############################################################################################
 
   sensor_PO_2D_fix_wrong:
-    P:
-      mode: fix
-      state: [1, 2]
-      #dynamic: false #missing
-    O:
-      mode: fix
-      state: [1]
-      dynamic: false
+    states:
+      P:
+        mode: fix
+        state: [1, 2]
+        #dynamic: false #missing
+      O:
+        mode: fix
+        state: [1]
+        dynamic: false
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_fix_wrong:
-    P:
-      mode: fix
-      state: [1, 2, 3]
-      dynamic: false
-    O:
-      mode: fix
-      state: [1, 0, 0, 0]
-      dynamic: false
+    states:
+      P:
+        mode: fix
+        state: [1, 2, 3]
+        dynamic: false
+      O:
+        mode: fix
+        state: [1, 0, 0, 0]
+        dynamic: false
     #noise_std: [0.1, 0.2] #missing
 
   sensor_PO_2D_initial_guess_wrong:
-    P:
-      mode: initial_guess
-      state: [1, 2]
-      dynamic: false
-    O:
-      #mode: initial_guess #missing
-      state: [1]
-      dynamic: false
+    states:
+      P:
+        mode: initial_guess
+        state: [1, 2]
+        dynamic: false
+      O:
+        #mode: initial_guess #missing
+        state: [1]
+        dynamic: false
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_initial_guess_wrong:
-    P:
-      mode: initial_guess
-      state: [1, 2, 3]
-      dynamic: false
-    # O: #missing
-    #   mode: initial_guess
-    #   state: [1, 0, 0, 0]
-    #   dynamic: false
+    states:
+      P:
+        mode: initial_guess
+        state: [1, 2, 3]
+        dynamic: false
+      # O: #missing
+      #   mode: initial_guess
+      #   state: [1, 0, 0, 0]
+      #   dynamic: false
     noise_std: [0.1, 0.2]
 
   sensor_PO_2D_factor_wrong:
-    P:
-      mode: factor
-      state: [1, 2]
-      noise_std: [0.1, 0.2]
-      dynamic: false
-    O:
-      mode: factor
-      #state: [1] #missing
-      noise_std: [0.1]
-      dynamic: false
+    states:
+      P:
+        mode: factor
+        state: [1, 2]
+        noise_std: [0.1, 0.2]
+        dynamic: false
+      O:
+        mode: factor
+        #state: [1] #missing
+        noise_std: [0.1]
+        dynamic: false
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_factor_wrong:
-    P:
-      mode: factor
-      state: [1, 2, 3]
-      #noise_std: [0.1, 0.2, 0.3] #missing
-      dynamic: false
-    O:
-      mode: factor
-      state: [1, 0, 0, 0]
-      noise_std: [0.1, 0.2, 0.3]
-      dynamic: false
+    states:
+      P:
+        mode: factor
+        state: [1, 2, 3]
+        #noise_std: [0.1, 0.2, 0.3] #missing
+        dynamic: false
+      O:
+        mode: factor
+        state: [1, 0, 0, 0]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: false
     noise_std: [0.1, 0.2]
   
   sensor_PO_2D_fix_dynamic_wrong:
-    P:
-      mode: fix
-      state: [1, 2, 3] # wrong size
-      dynamic: true
-    O:
-      mode: fix
-      state: [1]
-      dynamic: true
+    states:
+      P:
+        mode: fix
+        state: [1, 2, 3] # wrong size
+        dynamic: true
+      O:
+        mode: fix
+        state: [1]
+        dynamic: true
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_fix_dynamic_wrong:
-    P:
-      mode: fix
-      state: [1, 2, 3]
-      dynamic: true
-    O:
-      mode: fix
-      state: [1, 0, 0, 1] # not normalized
-      dynamic: true
+    states:
+      P:
+        mode: fix
+        state: [1, 2, 3]
+        dynamic: true
+      O:
+        mode: fix
+        state: [1, 0, 0, 1] # not normalized
+        dynamic: true
     noise_std: [0.1, 0.2]
 
   sensor_PO_2D_initial_guess_dynamic_wrong:
-    # P: #missing
-    #   mode: initial_guess
-    #   state: [1, 2]
-    #   dynamic: true
-    O:
-      mode: initial_guess
-      state: [1]
-      dynamic: true
+    states:
+      # P: #missing
+      #   mode: initial_guess
+      #   state: [1, 2]
+      #   dynamic: true
+      O:
+        mode: initial_guess
+        state: [1]
+        dynamic: true
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_initial_guess_dynamic_wrong:
-    P:
-      mode: initial_guess
-      state: [1, 2] # wrong size
-      dynamic: true
-    O:
-      mode: initial_guess
-      state: [1, 0, 0, 0]
-      dynamic: true
+    states:
+      P:
+        mode: initial_guess
+        state: [1, 2] # wrong size
+        dynamic: true
+      O:
+        mode: initial_guess
+        state: [1, 0, 0, 0]
+        dynamic: true
     noise_std: [0.1, 0.2]
 
   sensor_PO_2D_factor_dynamic_wrong:
-    P:
-      mode: factor
-      state: [1, 2]
-      noise_std: [0.1, 0.2, 0.3] #wrong size
-      dynamic: true
-    O:
-      mode: factor
-      state: [1]
-      noise_std: [0.1]
-      dynamic: true
+    states:
+      P:
+        mode: factor
+        state: [1, 2]
+        noise_std: [0.1, 0.2, 0.3] #wrong size
+        dynamic: true
+      O:
+        mode: factor
+        state: [1]
+        noise_std: [0.1]
+        dynamic: true
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_factor_dynamic_wrong:
-    P:
-      mode: factor
-      state: [1, 2, 3]
-      noise_std: [0.1, 0.2, 0.3]
-      dynamic: true
-    O:
-      mode: factor
-      state: [1, 0, 0, 0]
-      noise_std: [0.1, 0.2, 0.3, 0.4] # wrong size
-      dynamic: true
+    states:
+      P:
+        mode: factor
+        state: [1, 2, 3]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: true
+      O:
+        mode: factor
+        state: [1, 0, 0, 0]
+        noise_std: [0.1, 0.2, 0.3, 0.4] # wrong size
+        dynamic: true
     noise_std: [0.1, 0.2]
   
   sensor_PO_2D_fix_dynamic_drift_wrong:
-    P:
-      mode: fix
-      state: [1, 2]
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3] #wrong size
-    O:
-      mode: fix
-      state: [1]
-      dynamic: true
-      drift_std: [0.1]
+    states:
+      P:
+        mode: fix
+        state: [1, 2]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3] #wrong size
+      O:
+        mode: fix
+        state: [1]
+        dynamic: true
+        drift_std: [0.1]
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_fix_dynamic_drift_wrong:
-    P:
-      mode: fix
-      state: [1, 2, 3]
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3]
-    O:
-      mode: fix
-      state: [1, 0, 0, 0]
-      #dynamic: true #missing
-      drift_std: [0.1, 0.2, 0.3]
+    states:
+      P:
+        mode: fix
+        state: [1, 2, 3]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
+      O:
+        mode: fix
+        state: [1, 0, 0, 0]
+        #dynamic: true #missing
+        drift_std: [0.1, 0.2, 0.3]
     noise_std: [0.1, 0.2]
 
   sensor_PO_2D_initial_guess_dynamic_drift_wrong:
-    P:
-      mode: initial_guess
-      state: [1] #wrong size
-      dynamic: true
-      drift_std: [0.1, 0.2]
-    O:
-      mode: initial_guess
-      state: [1]
-      dynamic: true
-      drift_std: [0.1]
+    states:
+      P:
+        mode: initial_guess
+        state: [1] #wrong size
+        dynamic: true
+        drift_std: [0.1, 0.2]
+      O:
+        mode: initial_guess
+        state: [1]
+        dynamic: true
+        drift_std: [0.1]
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_initial_guess_dynamic_drift_wrong:
-    P:
-      mode: initial_guess
-      state: [1, 2, 3]
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3]
-    O:
-      #mode: initial_guess #missing
-      state: [1, 0, 0, 0]
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3]
+    states:
+      P:
+        mode: initial_guess
+        state: [1, 2, 3]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
+      O:
+        #mode: initial_guess #missing
+        state: [1, 0, 0, 0]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
     noise_std: [0.1, 0.2]
 
   sensor_PO_2D_factor_dynamic_drift_wrong:
-    P:
-      mode: factor
-      state: [1, 2]
-      noise_std: [0.1, 0.2]
-      dynamic: true
-      drift_std: [0.1] #wrong size
-    O:
-      mode: factor
-      state: [1]
-      noise_std: [0.1]
-      dynamic: true
-      drift_std: [0.1]
+    states:
+      P:
+        mode: factor
+        state: [1, 2]
+        noise_std: [0.1, 0.2]
+        dynamic: true
+        drift_std: [0.1] #wrong size
+      O:
+        mode: factor
+        state: [1]
+        noise_std: [0.1]
+        dynamic: true
+        drift_std: [0.1]
     noise_std: [0.1, 0.2]
 
   sensor_PO_3D_factor_dynamic_drift_wrong:
-    P:
-      mode: factor
-      state: [1, 2, 3]
-      noise_std: [0.1, 0.2, 0.3]
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3]
-    O:
-      mode: factor
-      state: [1, 0, 0, 0]
-      noise_std: [0.1, 0.2, 0.3, 0.4] #wrong size
-      dynamic: true
-      drift_std: [0.1, 0.2, 0.3]
+    states:
+      P:
+        mode: factor
+        state: [1, 2, 3]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
+      O:
+        mode: factor
+        state: [1, 0, 0, 0]
+        noise_std: [0.1, 0.2, 0.3, 0.4] #wrong size
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
+    noise_std: [0.1, 0.2]
+
+  sensor_POIA_3D_wrong:
+    states:
+      P:
+        mode: factor
+        state: [1, 2, 3]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: true
+      O:
+        mode: fix
+        state: [1, 0, 0, 0]
+        dynamic: false
+      I:
+        mode: initial_guess
+        state: [1, 2, 3, 4]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3, 0.4]
+      # A:
+      #   mode: factor
+      #   state: [1, 0, 0, 0]
+      #   noise_std: [0.1, 0.2, 0.3]
+      #   dynamic: true
+      #   drift_std: [0.1, 0.2, 0.3]
     noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_dummy.yaml b/test/yaml/sensor_dummy.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6fb39c70fe6506e0c822fe29b9ddcc57f0a02c5d
--- /dev/null
+++ b/test/yaml/sensor_dummy.yaml
@@ -0,0 +1,52 @@
+sensor:
+  sensor_dummy_1:
+    param1: 1.2
+    param2: 3
+    noise_std: [0.1, 0.2]
+    states:
+      P:
+        mode: factor
+        state: [1, 2, 3]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: true
+      O:
+        mode: fix
+        state: [1, 0, 0, 0]
+        dynamic: false
+      I:
+        mode: initial_guess
+        state: [1, 2, 3, 4]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3, 0.4]
+      A:
+        mode: factor
+        state: [1, 0, 0, 0]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
+
+  sensor_dummy_1_wrong:
+    param1: 1.2
+    param2: 3
+    noise_std: [0.1, 0.2]
+    states:
+      P:
+        mode: factor
+        state: [1, 2, 3]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: true
+      O:
+        mode: fix
+        state: [1, 0, 0, 0]
+        dynamic: false
+      # I:
+      #   mode: initial_guess
+      #   state: [1, 2, 3, 4]
+      #   dynamic: true
+      #   drift_std: [0.1, 0.2, 0.3, 0.4]
+      A:
+        mode: factor
+        state: [1, 0, 0, 0]
+        noise_std: [0.1, 0.2, 0.3]
+        dynamic: true
+        drift_std: [0.1, 0.2, 0.3]
\ No newline at end of file