diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
index 06469b0b133453e5d3aaeb12160ed5669c8cb2b2..db0f5c12bd75c957665dda889bd6d6cf11f0ee38 100644
--- a/demos/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -103,7 +103,7 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
         Vector2d measurement_rb(range,bearing);
         auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb,
                                                              measurement_rb,
-                                                             getSensor()->getNoiseCov());
+                                                             getSensor()->computeNoiseCov(measurement_rb));
 
         // 6. create factor
         auto prc = shared_from_this();
diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
index d81740193f2ff343cbd2cd592d7c717c07c11c0a..9b680a9777a4c8c1cfd603001c64c9f68f40350a 100644
--- a/demos/hello_wolf/sensor_range_bearing.cpp
+++ b/demos/hello_wolf/sensor_range_bearing.cpp
@@ -35,7 +35,8 @@ SensorRangeBearing::SensorRangeBearing(const std::string& _unique_name,
                                        const SizeEigen& _dim,
                                        ParamsSensorRangeBearingPtr _params,
                                        const Priors& _priors) :
-        SensorBase("SensorRangeBearing",_unique_name, _dim, _params, _priors)
+        SensorBase("SensorRangeBearing",_unique_name, _dim, _params, _priors),
+        params_rb_(_params)
 {
     assert(_dim == 2 && "SensorRangeBearing only for 2D");
 }
@@ -44,7 +45,8 @@ SensorRangeBearing::SensorRangeBearing(const std::string& _unique_name,
                                        const SizeEigen& _dim,
                                        ParamsSensorRangeBearingPtr _params,
                                        const ParamsServer& _server) :
-        SensorBase("SensorRangeBearing",_unique_name, _dim, _params, _server)
+        SensorBase("SensorRangeBearing",_unique_name, _dim, _params, _server),
+        params_rb_(_params)
 {
     assert(_dim == 2 && "SensorRangeBearing only for 2D");
 }
@@ -54,6 +56,12 @@ SensorRangeBearing::~SensorRangeBearing()
     //
 }
 
+Eigen::MatrixXd SensorRangeBearing::computeNoiseCov(const Eigen::VectorXd & _data) const
+{
+    return (Eigen::Vector2d() << params_rb_->noise_range_metres_std,
+                                 params_rb_->noise_bearing_degrees_std).finished().cwiseAbs2().asDiagonal();
+}
+
 } /* namespace wolf */
 
 // Register in the SensorFactory
diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h
index 76170f9b1d8801022b0a5eee5f2cace8bfce70b5..1aa70ac26a69dd9f8f706c3023ef62c87344ded4 100644
--- a/demos/hello_wolf/sensor_range_bearing.h
+++ b/demos/hello_wolf/sensor_range_bearing.h
@@ -60,6 +60,9 @@ WOLF_PTR_TYPEDEFS(SensorRangeBearing)
 
 class SensorRangeBearing : public SensorBase
 {
+    private:
+        ParamsSensorRangeBearingPtr params_rb_;
+
     public:
         SensorRangeBearing(const std::string& _unique_name,
                            const SizeEigen& _dim,
@@ -70,7 +73,10 @@ class SensorRangeBearing : public SensorBase
                            ParamsSensorRangeBearingPtr _params,
                            const ParamsServer& _server);
         WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing);
+        
         ~SensorRangeBearing() override;
+
+        Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override;
 };
 
 } /* namespace wolf */
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 3059521df8e6c6a47b169372396dc8c49a6b40ff..6550a00aa5f72b7648919eae1d07e3dbfb21aa9f 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -151,13 +151,15 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
          * \param _dim Problem dimension
          * \param _params params struct
          * \param _priors priors of the sensor state blocks
+         * \param _keys_types_apart_from_PO Map containing the keys and the types of the state blocks (apart from extrinsics: P, O)
          *
          **/
         SensorBase(const std::string& _type,
                    const std::string& _unique_name,
                    const SizeEigen& _dim,
                    ParamsSensorBasePtr _params,
-                   const Priors& _priors);
+                   const Priors& _priors,
+                   const std::unordered_map<char, std::string>&  _keys_types_apart_from_PO={});
 
         /** \brief Constructor with ParamServer and keys
          *
@@ -280,9 +282,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         Eigen::MatrixXd getDriftCov(char) const;
 
         // noise
-        virtual void setNoiseStd(const Eigen::VectorXd & _noise_std) = 0;
-        virtual Eigen::VectorXd getNoiseStd() const = 0;
-        virtual Eigen::MatrixXd getNoiseCov() const = 0;
+        virtual Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const = 0;
 
         virtual void printHeader(int depth, //
                                  bool constr_by, //
@@ -372,18 +372,6 @@ inline bool SensorBase::isStateBlockDynamic(const char& _key) const
     return state_block_dynamic_.at(_key);
 }
 
-inline Eigen::VectorXd SensorBase::getNoiseStd() const
-{
-    assert(params_ != nullptr);
-    return params_->noise_std;
-}
-
-inline Eigen::MatrixXd SensorBase::getNoiseCov() const
-{
-    assert(params_ != nullptr);
-    return params_->noise_std.cwiseAbs2().asDiagonal();
-}
-
 inline Eigen::VectorXd SensorBase::getDriftStd(char _key) const
 {
     if (drift_covs_.count(_key) == 0)
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index ede10ff4b781e289701734f48880dd125614743e..b386829fec38d7b7e8fe96f1785b2a02e807561d 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/include/core/sensor/sensor_odom_2d.h
@@ -105,7 +105,8 @@ class SensorOdom2d : public SensorBase
          *
          * See implementation for details.
          */
-        Matrix2d computeCovFromMotion (const VectorXd& _data) const;
+        // noise
+        Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override;
 
 };
 
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index e8f48dcd7b52cb8b5d2dc0ba95cc0c0f689de9ea..3e15c1af8ce3eaff8e3bed47f04ce68b498ba67a 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -38,7 +38,7 @@ CaptureMotion::CaptureMotion(const std::string & _type,
                              CaptureBasePtr _capture_origin_ptr) :
         CaptureBase(_type, _ts, _sensor_ptr),
         data_(_data),
-        data_cov_(_sensor_ptr ? _sensor_ptr->getNoiseCov() : Eigen::MatrixXd::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
+        data_cov_(_sensor_ptr ? _sensor_ptr->computeNoiseCov(_data) : Eigen::MatrixXd::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
         buffer_(),
         capture_origin_ptr_(_capture_origin_ptr)
 {
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 0ad4a754dd665d4533303f2e27569b0e7c151048..31b2b770afc2e1c7ac80adc9d793cb24648960b7 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -298,7 +298,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
                                                                  getSensor(),
                                                                  timestamp_from_callback,
                                                                  Eigen::VectorXd::Zero(data_size_),
-                                                                 getSensor()->getNoiseCov(),
+                                                                 getSensor()->computeNoiseCov(Eigen::VectorXd::Zero(data_size_)),
                                                                  calib_origin,
                                                                  calib_origin,
                                                                  capture_origin);
@@ -391,7 +391,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
                                                                 getSensor(),
                                                                 timestamp_from_callback,
                                                                 Eigen::VectorXd::Zero(data_size_),
-                                                                getSensor()->getNoiseCov(),
+                                                                getSensor()->computeNoiseCov(Eigen::VectorXd::Zero(data_size_)),
                                                                 calib_origin,
                                                                 calib_origin,
                                                                 capture_origin);
@@ -486,7 +486,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
                                              getSensor(),
                                              keyframe->getTimeStamp(),
                                              Eigen::VectorXd::Zero(data_size_),
-                                             getSensor()->getNoiseCov(),
+                                             getSensor()->computeNoiseCov(Eigen::VectorXd::Zero(data_size_)),
                                              getCalibration(origin_ptr_),
                                              getCalibration(origin_ptr_),
                                              last_ptr_);
@@ -733,7 +733,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
                                  getSensor(),
                                  origin_ts,
                                  Eigen::VectorXd::Zero(data_size_),
-                                 getSensor()->getNoiseCov(),
+                                 getSensor()->computeNoiseCov(Eigen::VectorXd::Zero(data_size_)),
                                  getCalibration(),
                                  getCalibration(),
                                  nullptr);
@@ -749,7 +749,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
                                getSensor(),
                                origin_ts,
                                Eigen::VectorXd::Zero(data_size_),
-                               getSensor()->getNoiseCov(),
+                               getSensor()->computeNoiseCov(Eigen::VectorXd::Zero(data_size_)),
                                getCalibration(),
                                getCalibration(),
                                origin_ptr_);
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 17bb36595df46e9c48cff4293d5060f614395fb4..cf8191714fc59c19891e107828f51b1baec6b20b 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -37,7 +37,8 @@ SensorBase::SensorBase(const std::string& _type,
                        const std::string& _unique_name,
                        const SizeEigen& _dim,
                        ParamsSensorBasePtr _params,
-                       const Priors& _priors) :
+                       const Priors& _priors,
+                       const std::unordered_map<char, std::string>& _keys_types_apart_from_PO) :
         NodeBase("SENSOR", _type, _unique_name),
         HasStateBlocks(""),
         hardware_ptr_(),
@@ -48,7 +49,22 @@ SensorBase::SensorBase(const std::string& _type,
         last_capture_(nullptr)
 {
     assert(_dim == 2 or _dim == 3);
+    assert(_keys_types_apart_from_PO.count('P') == 0 and 
+           _keys_types_apart_from_PO.count('O') == 0 and 
+           "SensorBase: _keys_types_apart_from_PO should not contain 'P' or 'O' keys");
 
+    // check priors have all _keys_types_apart_from_PO (PO will be checked in loadPriors())
+    for (auto key_type : _keys_types_apart_from_PO)
+    {
+        if (_priors.count(key_type.first) == 0)
+            throw std::runtime_error("SensorBase: In constructor, provided '_priors' does not contain key " + std::string(1,key_type.first));
+    
+        if (_priors.at(key_type.first).getType() != key_type.second)
+            throw std::runtime_error("SensorBase: In constructor, provided '_priors' for key '" + 
+                                     std::string(1,key_type.first) + "' has wrong type: " + 
+                                     _priors.at(key_type.first).getType() +
+                                     " and should be: " + key_type.second);
+    }
     // load priors
     loadPriors(_priors, _dim);
 }
@@ -272,18 +288,6 @@ void SensorBase::registerNewStateBlocks(ProblemPtr _problem)
     }
 }
 
-void SensorBase::setNoiseStd(const Eigen::VectorXd& _noise_std) 
-{
-    assert(params_ != nullptr);
-    params_->noise_std = _noise_std;
-}
-
-void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) 
-{
-    assert(params_ != nullptr);
-    params_->noise_std = _noise_cov.diagonal().cwiseSqrt();
-}
-
 void SensorBase::setDriftStd(char _key, const Eigen::VectorXd & _drift_rate_std)
 {
     drift_covs_[_key] = _drift_rate_std.cwiseAbs2().asDiagonal();
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index fae0fe9a75c6f08d57ed7604b461ca3a1c0ac455..0abef3899bf3ef237139876dafb97cf869c1f5cc 100644
--- a/src/sensor/sensor_odom_2d.cpp
+++ b/src/sensor/sensor_odom_2d.cpp
@@ -69,7 +69,7 @@ double SensorOdom2d::getRotVarToRotNoiseFactor() const
     return params_odom2d_->k_rot_to_rot;
 }
 
-Eigen::Matrix2d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
+Eigen::MatrixXd SensorOdom2d::computeNoiseCov(const Eigen::VectorXd & _data) const
 {
     assert(_data.size() == 2);
     double d = fabs(_data(0));
diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h
index e73f7d3c7837fa6583b26f6639c6f4220468dfe5..5421d72fac0bbd7b53ed9191d91eeb54cdb3014f 100644
--- a/test/dummy/sensor_dummy.h
+++ b/test/dummy/sensor_dummy.h
@@ -33,25 +33,22 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDummy);
 
 struct ParamsSensorDummy : public ParamsSensorBase
 {
-    ~ParamsSensorDummy() override = default;
-
     double noise_p_std;
     double noise_o_std;
-    int param2;
     
+    ParamsSensorDummy() = default;
     ParamsSensorDummy(std::string _unique_name, const ParamsServer& _server):
         ParamsSensorBase(_unique_name, _server)
     {
         noise_p_std = _server.getParam<double>(prefix + _unique_name + "/noise_p_std");
         noise_o_std = _server.getParam<double>(prefix + _unique_name + "/noise_o_std");
-        param2 = _server.getParam<int>(prefix + _unique_name + "/param2");
     }
+    ~ParamsSensorDummy() override = default;
     std::string print() const override
     {
         return ParamsSensorBase::print()                        + "\n"
                 + "noise_p_std: "   + toString(noise_p_std)     + "\n"
-                + "noise_o_std: "   + toString(noise_o_std)     + "\n"
-                + "param2: "        + toString(param2)          + "\n";
+                + "noise_o_std: "   + toString(noise_o_std)     + "\n";
     }
 };
 
@@ -72,8 +69,7 @@ class SensorDummy : public SensorBase
                        _unique_name,
                        _dim,
                        _params,
-                       _server,
-                       std::unordered_map<char, std::string>{{'I',"StateBlock"},{'A',"StateQuaternion"}}),
+                       _server),
             params_dummy_(_params)
         {
         }
@@ -95,34 +91,11 @@ class SensorDummy : public SensorBase
 
         virtual ~SensorDummy(){};
 
-        void setNoiseStd(const Eigen::VectorXd & _noise_std) override
+        Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override
         {
-            assert(_noise_std.size() == (dim_ == 2 ? 3 : 6));
-
-            params_dummy_->noise_p_std = _noise_std(1);
-            params_dummy_->noise_o_std = _noise_std((dim_ == 2 ? 2 : 3));
+            return (Eigen::Vector2d() << params_dummy_->noise_p_std,
+                                         params_dummy_->noise_o_std).finished().cwiseAbs2().asDiagonal();
         }
-
-        Eigen::VectorXd getNoiseStd() const override
-        {
-            if (dim_ == 2)
-                return (Eigen::VectorXd() << params_dummy_->noise_p_std, 
-                                             params_dummy_->noise_p_std, 
-                                             params_dummy_->noise_o_std).finished();
-            else
-                return (Eigen::VectorXd() << params_dummy_->noise_p_std, 
-                                             params_dummy_->noise_p_std,
-                                             params_dummy_->noise_p_std, 
-                                             params_dummy_->noise_o_std, 
-                                             params_dummy_->noise_o_std, 
-                                             params_dummy_->noise_o_std).finished();
-        }
-
-        Eigen::MatrixXd getNoiseCov() const override
-        {
-            return getNoiseStd().cwiseAbs2().asDiagonal();
-        }
-
 };
 
 }
diff --git a/test/dummy/sensor_dummy_poia.h b/test/dummy/sensor_dummy_poia.h
new file mode 100644
index 0000000000000000000000000000000000000000..849231de5b010b324a733ae91481cb7e59f4b6e1
--- /dev/null
+++ b/test/dummy/sensor_dummy_poia.h
@@ -0,0 +1,104 @@
+//--------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_DummyPoi_H_
+#define SENSOR_DummyPoi_H_
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/sensor/sensor_base.h"
+#include "sensor_dummy.h"
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDummyPoia);
+
+struct ParamsSensorDummyPoia : public ParamsSensorDummy
+{
+    ParamsSensorDummyPoia() = default;    
+    ParamsSensorDummyPoia(std::string _unique_name, const ParamsServer& _server):
+        ParamsSensorDummy(_unique_name, _server)
+    {
+    }
+    ~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(const std::string& _unique_name, 
+                    SizeEigen _dim, 
+                    ParamsSensorDummyPoiaPtr _params,
+                    const ParamsServer& _server) :
+            SensorBase("SensorDummyPoia",
+                       _unique_name,
+                       _dim,
+                       _params,
+                       _server,
+                       std::unordered_map<char, std::string>{{'I',"StateBlock"},{'A',"StateQuaternion"}}),
+            params_dummy_poia_(_params)
+        {
+        }
+        WOLF_SENSOR_CREATE(SensorDummyPoia, ParamsSensorDummyPoia);
+
+
+        SensorDummyPoia(const std::string& _unique_name, 
+                    SizeEigen _dim, 
+                    ParamsSensorDummyPoiaPtr _params,
+                    const Priors& _priors) :
+            SensorBase("SensorDummyPoia",
+                       _unique_name,
+                       _dim,
+                       _params,
+                       _priors,
+                       std::unordered_map<char, std::string>{{'I',"StateBlock"},{'A',"StateQuaternion"}}),
+            params_dummy_poia_(_params)
+        {
+        }
+
+        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
+#endif
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index d86c98b8c8d84ab0d06d25f427a1dcba3d173676..559cb5883146cda4ec6a39694a9ba7ca3c1f300a 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -31,6 +31,7 @@
 #include "core/yaml/parser_yaml.h"
 #include "core/utils/params_server.h"
 #include "dummy/sensor_dummy.h"
+#include "dummy/sensor_dummy_poia.h"
 
 using namespace wolf;
 using namespace Eigen;
@@ -52,18 +53,15 @@ MatrixXd o_cov_2D = o_std_2D.cwiseAbs2().asDiagonal();
 MatrixXd o_cov_3D = o_std_3D.cwiseAbs2().asDiagonal();
 double noise_p_std = 0.1;
 double noise_o_std = 0.01;
-VectorXd noise_std_2D = (Vector3d() << noise_p_std, noise_p_std, noise_o_std).finished();
-VectorXd noise_std_3D = (Vector3d() << noise_p_std, noise_p_std,noise_p_std, noise_o_std, noise_o_std, noise_o_std).finished();
-MatrixXd noise_cov_2D = noise_std_2D.cwiseAbs2().asDiagonal();
-MatrixXd noise_cov_3D = noise_std_3D.cwiseAbs2().asDiagonal();
+MatrixXd noise_cov_dummy = Vector2d(noise_p_std, noise_o_std).cwiseAbs2().asDiagonal();
 
 void checkSensor(SensorBasePtr S, 
-                char _key,
-                const VectorXd& _state, 
-                bool _fixed,
-                const VectorXd& _noise_std,
-                bool _dynamic, 
-                const VectorXd& _drift_std)
+                 char _key,
+                 const VectorXd& _state, 
+                 bool _fixed,
+                 const VectorXd& _noise_std,
+                 bool _dynamic, 
+                 const VectorXd& _drift_std)
 {
   MatrixXd noise_cov = _noise_std.cwiseAbs2().asDiagonal();
   MatrixXd drift_cov = _drift_std.cwiseAbs2().asDiagonal();
@@ -101,21 +99,22 @@ void checkSensor(SensorBasePtr S,
   }
 }
 
-// CONSTRUCTOR WITH PRIORS AND PARAMS
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////// CONSTRUCTOR WITH PRIORS AND PARAMS ////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////
 // 2D Fix
-TEST(SensorBase, POfix2D)
+TEST(SensorBase, makeshared_priors_POfix2D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   auto S = std::make_shared<SensorDummy>("sensor1", 2, params, 
-                                        Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
-                                                {'O',Prior("O", o_state_2D)}}));
+                                         Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
+                                                 {'O',Prior("O", o_state_2D)}}));
 
   // noise
-  ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_2D, Constants::EPS);
-  ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // checks
   checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
@@ -123,19 +122,18 @@ TEST(SensorBase, POfix2D)
 }
 
 // 3D Fix
-TEST(SensorBase, POfix3D)
+TEST(SensorBase, makeshared_priors_POfix3D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   auto S = std::make_shared<SensorDummy>("sensor1", 3, params, 
-                                        Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
-                                                {'O',Prior("O", o_state_3D)}}));
+                                         Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
+                                                 {'O',Prior("O", o_state_3D)}}));
 
   // noise
-  ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
-  ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
   ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -146,19 +144,18 @@ TEST(SensorBase, POfix3D)
 }
 
 // 2D Initial guess
-TEST(SensorBase, POinitial_guess2D)
+TEST(SensorBase, makeshared_priors_POinitial_guess2D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   auto S = std::make_shared<SensorDummy>("sensor1", 2, params, 
-                                        Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
-                                                {'O',Prior("O", o_state_2D, "initial_guess")}}));
+                                         Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
+                                                 {'O',Prior("O", o_state_2D, "initial_guess")}}));
 
   // noise
-  ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_2D, Constants::EPS);
-  ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
   ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -169,19 +166,18 @@ TEST(SensorBase, POinitial_guess2D)
 }
 
 // 3D Initial guess
-TEST(SensorBase, POinitial_guess3D)
+TEST(SensorBase, makeshared_priors_POinitial_guess3D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   auto S = std::make_shared<SensorDummy>("sensor1", 3, params, 
-                                        Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
-                                                {'O',Prior("O", o_state_3D, "initial_guess")}}));
+                                         Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
+                                                 {'O',Prior("O", o_state_3D, "initial_guess")}}));
 
   // noise
-  ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
-  ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
   ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -192,19 +188,18 @@ TEST(SensorBase, POinitial_guess3D)
 }
 
 // 2D Factor
-TEST(SensorBase, POfactor2D)
+TEST(SensorBase, makeshared_priors_POfactor2D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   auto S = std::make_shared<SensorDummy>("sensor1", 2, params, 
-                                        Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
-                                                {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
+                                         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->getNoiseStd(), noise_std_2D, Constants::EPS);
-  ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
   ASSERT_EQ(S->getPriorFeatures().size(), 2);
@@ -215,19 +210,18 @@ TEST(SensorBase, POfactor2D)
 }
 
 // 3D Factor
-TEST(SensorBase, POfactor3D)
+TEST(SensorBase, makeshared_priors_POfactor3D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   auto S = std::make_shared<SensorDummy>("sensor1", 3, params, 
-                                        Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
-                                                {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
+                                         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->getNoiseStd(), noise_std_3D, Constants::EPS);
-  ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
   ASSERT_EQ(S->getPriorFeatures().size(), 2);
@@ -238,19 +232,18 @@ TEST(SensorBase, POfactor3D)
 }
 
 // 2D Initial guess dynamic
-TEST(SensorBase, POinitial_guess_dynamic2D)
+TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   auto S = std::make_shared<SensorDummy>("sensor1", 2, params, 
-                                        Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
-                                                {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
+                                         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->getNoiseStd(), noise_std_2D, Constants::EPS);
-  ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
   ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -261,19 +254,18 @@ TEST(SensorBase, POinitial_guess_dynamic2D)
 }
 
 // 3D Initial guess dynamic
-TEST(SensorBase, POinitial_guess_dynamic3D)
+TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   auto S = std::make_shared<SensorDummy>("sensor1", 3, params, 
-                                        Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
-                                                {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
+                                         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->getNoiseStd(), noise_std_3D, Constants::EPS);
-  ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
   ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -284,19 +276,18 @@ TEST(SensorBase, POinitial_guess_dynamic3D)
 }
 
 // 2D Initial guess dynamic drift
-TEST(SensorBase, POinitial_guess_dynamic2D_drift)
+TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D_drift)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   auto S = std::make_shared<SensorDummy>("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)}}));
+                                         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->getNoiseStd(), noise_std_2D, Constants::EPS);
-  ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
   ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -307,19 +298,18 @@ TEST(SensorBase, POinitial_guess_dynamic2D_drift)
 }
 
 // 3D Initial guess dynamic drift
-TEST(SensorBase, POinitial_guess_dynamic3D_drift)
+TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D_drift)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   auto S = std::make_shared<SensorDummy>("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)}}));
+                                         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->getNoiseStd(), noise_std_3D, Constants::EPS);
-  ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
   ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -329,24 +319,23 @@ TEST(SensorBase, POinitial_guess_dynamic3D_drift)
   checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
 }
 
-// 3D POI mixed
-TEST(SensorBase, POI_mixed)
+// 3D POIA mixed
+TEST(SensorBase, makeshared_priors_POIA_mixed)
 {
-  auto params = std::make_shared<ParamsSensorDummy>();
+  auto params = std::make_shared<ParamsSensorDummyPoia>();
   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<SensorDummy>("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)}}));
-
+  auto S = std::make_shared<SensorDummyPoia>("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)}}));
+     
   // noise
-  ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
-  ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
   ASSERT_EQ(S->getPriorFeatures().size(), 2);
@@ -358,13 +347,50 @@ TEST(SensorBase, POI_mixed)
   checkSensor(S, 'A', o_state_3D, false, o_std_3D, false, vector0);
 }
 
-// CONSTRUCTOR WITH PARAM SERVER and KEY_TYPES
-TEST(SensorBase, server_PO)
+// 3D POIA wrong
+TEST(SensorBase, makeshared_priors_POIA_wrong)
+{
+  auto params = std::make_shared<ParamsSensorDummyPoia>();
+  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>("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)}})),
+               std::runtime_error);
+
+  // missing A
+  ASSERT_THROW(std::make_shared<SensorDummyPoia>("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)}*/})),
+               std::runtime_error);
+
+  // wrong A type (expected StateQuaternion)
+  ASSERT_THROW(std::make_shared<SensorDummyPoia>("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("StateBlock", p_state_3D, "factor", p_std_3D)}})),
+               std::runtime_error);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////// CONSTRUCTOR WITH PARAM SERVER and KEY_TYPES ////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST(SensorBase, makeshared_server_PO)
 {
   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;
@@ -376,223 +402,599 @@ TEST(SensorBase, server_PO)
     for (auto mode : modes)
       for (auto dynamic : dynamics)
         for (auto drift : drifts)
-        {
-          // nonsense combination
-          if (not dynamic and drift)
-            continue;
+          for (auto wrong : wrongs)
+          {
+            // nonsense combination
+            if (not dynamic and drift)
+              continue;
+
+            std::string name = "sensor_PO_" + 
+                               to_string(dim) + 
+                               "D_" + 
+                               mode + 
+                               (dynamic ? "_dynamic" : "") + 
+                               (drift ? "_drift" : "") + 
+                               (wrong ? "_wrong" : "");
+
+            // Yaml parser
+            ParserYaml parser   = ParserYaml("test/yaml/sensor_base/" + name + ".yaml", wolf_root, 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<ParamsSensorDummy>("sensor_1", server);
+              auto S = std::make_shared<SensorDummy>("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;
+
+              // noise
+              ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), 
+                                   noise_cov_dummy, 
+                                   Constants::EPS);
+
+              // 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<SensorDummy>("sensor_1", dim, 
+                                                         std::make_shared<ParamsSensorDummy>("sensor_1", server),
+                                                         server),std::runtime_error);
+            }
+          }
 
-          std::string name = "sensor_PO_" + to_string(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
+  // POIA - 3D - CORRECT YAML
+  {
+    // Yaml parser
+    ParserYaml parser   = ParserYaml("test/yaml/sensor_base/sensor_POIA_3D.yaml", wolf_root, true);
+    ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
+    server.print();
 
-          // Yaml parser
-          ParserYaml parser   = ParserYaml("test/yaml/sensor_base/" + name + ".yaml", wolf_root, true);
-          ParamsServer server = ParamsServer(parser.getParams(), "/sensor/" + name);
-          server.print();
+    WOLF_INFO("Creating sensor from sensor_POIA_3D.yaml");
 
-          // CORRECT YAML
-          WOLF_INFO("Creating sensor from name ", name);
+    auto params = std::make_shared<ParamsSensorDummyPoia>("sensor_1", server);
+    auto S = std::make_shared<SensorDummyPoia>("sensor_1", 3, params, server);
 
-          auto params = std::make_shared<ParamsSensorDummy>(name, server);
-          auto S = std::make_shared<SensorDummy>( name, dim, params, server);
+    // noise
+    ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), 
+                          noise_cov_dummy, 
+                          Constants::EPS);
 
-          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(), 2);
 
-          // 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);
+    // check
+    checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true,  vector0);
+    checkSensor(S, 'O', o_state.head(4), true,  vector0,        false, vector0);
+    checkSensor(S, 'I', p_state.head(4), false, vector0,        true,  po_std.head(4));
+    checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true,  po_std.head(3));
+  }
+  // POIA - 3D - INCORRECT YAML
+  {
+    // Yaml parser
+    ParserYaml parser   = ParserYaml("test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml", wolf_root, true);
+    ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
 
-          // factors
-          ASSERT_EQ(S->getPriorFeatures().size(), mode == "factor" ? 2 : 0);
+    WOLF_INFO("Creating sensor from sensor_POIA_3D_wrong.yaml");
 
-          // 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));
+    ASSERT_THROW(std::make_shared<SensorDummyPoia>("sensor_1", 3,
+                                                   std::make_shared<ParamsSensorDummyPoia>("sensor_1", server), 
+                                                   server),
+                 std::runtime_error);
+  }
+}
 
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////// FactorySensor ///////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST(SensorBase, factory)
+{
+  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});
 
-          // INCORRECT YAML
-          WOLF_INFO("Creating sensor from name ", name + "_wrong");
-          ASSERT_THROW(std::make_shared<SensorDummy>( name + "_wrong", dim, 
-                                                    std::make_shared<ParamsSensorDummy>(name + "_wrong", server),
-                                                    server),std::runtime_error);
-        }
+  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_" + 
+                               to_string(dim) + 
+                               "D_" + 
+                               mode + 
+                               (dynamic ? "_dynamic" : "") + 
+                               (drift ? "_drift" : "") + 
+                               (wrong ? "_wrong" : "");
+            // Yaml parser
+            ParserYaml parser   = ParserYaml("test/yaml/sensor_base/" + name + ".yaml", wolf_root, true);
+            ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
+
+            WOLF_INFO("Creating sensor from ", name, ".yaml");
+
+            // CORRECT YAML
+            if (not wrong)
+            {
+              auto S = FactorySensor::create("SensorDummy", "sensor_1", dim, 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;
+
+              // noise
+              ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), 
+                                  noise_cov_dummy, 
+                                  Constants::EPS);
+
+              // 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(FactorySensor::create("SensorDummy", "sensor_1", dim, server),std::runtime_error);
+            }
+          }
+
+  // POIA - 3D - CORRECT YAML
+  {
+    WOLF_INFO("Creating sensor from name sensor_POIA_3D.yaml");
+
+    // Yaml parser
+    ParserYaml parser   = ParserYaml("test/yaml/sensor_base/sensor_POIA_3D.yaml", wolf_root, true);
+    ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
+    server.print();
+
+    // create sensor
+    auto S = FactorySensor::create("SensorDummyPoia","sensor_1", 3, server);
+
+    // 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.head(3), false, po_std.head(3), true,  vector0);
+    checkSensor(S, 'O', o_state.head(4), true,  vector0,        false, vector0);
+    checkSensor(S, 'I', p_state.head(4), false, vector0,        true,  po_std.head(4));
+    checkSensor(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.yaml");
+
+    // Yaml parser
+    ParserYaml parser   = ParserYaml("test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml", wolf_root, true);
+    ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
+
+    // create sensor
+    ASSERT_THROW(FactorySensor::create("SensorDummyPoia", "sensor_1", 3, server),std::runtime_error);
+  }
 }
 
-TEST(SensorBase, server_POIA)
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////// FactorySensorYaml /////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+TEST(SensorBase, factory_yaml)
 {
+  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;
 
-  // Yaml parser
-  ParserYaml parser   = ParserYaml("test/yaml/sensor_base/sensor_POIA_3D.yaml", wolf_root, true);
-  ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_POIA_3D");
-  server.print();
+  // 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 yaml_filepath = wolf_root +
+                                        "/test/yaml/sensor_base/" + 
+                                        "sensor_PO_" + 
+                                        to_string(dim) + 
+                                        "D_" + 
+                                        mode + 
+                                        (dynamic ? "_dynamic" : "") + 
+                                        (drift ? "_drift" : "") + 
+                                        (wrong ? "_wrong" : "") +
+                                        ".yaml";
+
+            WOLF_INFO("Creating sensor from ", yaml_filepath);
+
+            // CORRECT YAML
+            if (not wrong)
+            {
+              auto S = FactorySensorYaml::create("SensorDummy", "sensor_1", dim, yaml_filepath);
+
+              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;
+
+              // noise
+              ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), 
+                                  noise_cov_dummy, 
+                                  Constants::EPS);
+
+              // 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(FactorySensorYaml::create("SensorDummy", "sensor_1", dim, yaml_filepath),std::runtime_error);
+            }
+          }
 
   // POIA - 3D - CORRECT YAML
-  WOLF_INFO("Creating sensor from name sensor_POIA_3D");
+  {
+    WOLF_INFO("Creating sensor from name sensor_POIA_3D.yaml");
+
+    // create sensor
+    auto S = FactorySensorYaml::create("SensorDummyPoia",
+                                       "sensor_1", 
+                                       3,
+                                       wolf_root + "/test/yaml/sensor_base/sensor_POIA_3D.yaml");
+
+    // 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.head(3), false, po_std.head(3), true,  vector0);
+    checkSensor(S, 'O', o_state.head(4), true,  vector0,        false, vector0);
+    checkSensor(S, 'I', p_state.head(4), false, vector0,        true,  po_std.head(4));
+    checkSensor(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.yaml");
+
+    // create sensor
+    ASSERT_THROW(FactorySensorYaml::create("SensorDummyPoia",
+                                           "sensor_1", 
+                                           3,
+                                           wolf_root + "/test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml"),
+                 std::runtime_error);
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////// FactorySensorPriors ////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// 2D Fix
+TEST(SensorBase, factory_priors_POfix2D)
+{
+  auto params = std::make_shared<ParamsSensorDummy>();
+  params->noise_p_std = noise_p_std;
+  params->noise_o_std = noise_o_std;
+  
+  auto S = FactorySensorPriors::create("SensorDummy","sensor1", 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);
+}
 
-  auto params = std::make_shared<ParamsSensorDummy>("sensor_POIA_3D", server);
-  auto S = std::make_shared<SensorDummy>("sensor_POIA_3D", 3, params, server, 
-                                        std::unordered_map<char, std::string>({{'I',"StateBlock"},
-                                                                               {'A',"StateQuaternion"}}));
+// 3D Fix
+TEST(SensorBase, factory_priors_POfix3D)
+{
+  auto params = std::make_shared<ParamsSensorDummy>();
+  params->noise_p_std = noise_p_std;
+  params->noise_o_std = noise_o_std;
+  
+  auto S = FactorySensorPriors::create("SensorDummy","sensor1", 3, params, 
+                                       Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
+                                               {'O',Prior("O", o_state_3D)}}));
 
   // 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);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 2);
+  ASSERT_EQ(S->getPriorFeatures().size(), 0);
 
   // check
-  checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true,  vector0);
-  checkSensor(S, 'O', o_state.head(4), true,  vector0,        false, vector0);
-  checkSensor(S, 'I', p_state.head(4), false, vector0,        true,  po_std.head(4));
-  checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true,  po_std.head(3));
+  checkSensor(S, 'P', p_state_3D, true, vector0, false, vector0);
+  checkSensor(S, 'O', o_state_3D, true, vector0, false, vector0);
 }
 
-TEST(SensorBase, server_POIA_wrong)
+// 2D Initial guess
+TEST(SensorBase, factory_priors_POinitial_guess2D)
 {
-  // Yaml parser
-  ParserYaml parser   = ParserYaml("test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml", wolf_root, true);
-  ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_POIA_3D_wrong");
+  auto params = std::make_shared<ParamsSensorDummy>();
+  params->noise_p_std = noise_p_std;
+  params->noise_o_std = noise_o_std;
+  
+  auto S = FactorySensorPriors::create("SensorDummy","sensor1", 2, params, 
+                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
+                                               {'O',Prior("O", o_state_2D, "initial_guess")}}));
 
-  // POIA - 3D - INCORRECT YAML
-  WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong");
+  // noise
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
-  ASSERT_THROW(std::make_shared<SensorDummy>("sensor_POIA_3D_wrong", 3,
-                                            std::make_shared<ParamsSensorDummy>("sensor_POIA_3D_wrong", server), 
-                                            server,
-                                            std::unordered_map<char, std::string>({{'I',"StateBlock"},
-                                                                                  {'A',"StateQuaternion"}})),
-               std::runtime_error);
+  // 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);
 }
 
-// DUMMY CLASS
-TEST(SensorBase, dummy_make_shared)
+// 3D Initial guess
+TEST(SensorBase, factory_priors_POinitial_guess3D)
 {
-  ParserYaml parser   = ParserYaml("test/yaml/sensor_base/sensor_dummy.yaml", wolf_root, true);
-  ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_dummy_1");
+  auto params = std::make_shared<ParamsSensorDummy>();
+  params->noise_p_std = noise_p_std;
+  params->noise_o_std = noise_o_std;
+  
+  auto S = FactorySensorPriors::create("SensorDummy","sensor1", 3, params, 
+                                       Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
+                                               {'O',Prior("O", o_state_3D, "initial_guess")}}));
 
-  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;
+  // noise
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
-  // POIA - 3D - CORRECT YAML
-  WOLF_INFO("Creating sensor from name sensor_dummy_1");
+  // factors
+  ASSERT_EQ(S->getPriorFeatures().size(), 0);
 
-  auto params = std::make_shared<ParamsSensorDummy>("sensor_dummy_1", server);
-  auto S = std::make_shared<SensorDummy>("sensor_dummy_1", 3, params, server);
+  // 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->noise_p_std = noise_p_std;
+  params->noise_o_std = noise_o_std;
+  
+  auto S = FactorySensorPriors::create("SensorDummy","sensor1", 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->getNoiseStd(), po_std.head<2>(), Constants::EPS);
-  ASSERT_MATRIX_APPROX(S->getNoiseCov(), MatrixXd(po_std.head<2>().cwiseAbs2().asDiagonal()), Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
   ASSERT_EQ(S->getPriorFeatures().size(), 2);
 
   // check
-  checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true,  vector0);
-  checkSensor(S, 'O', o_state.head(4), true,  vector0,        false, vector0);
-  checkSensor(S, 'I', p_state.head(4), false, vector0,        true,  po_std.head(4));
-  checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true,  po_std.head(3));
+  checkSensor(S, 'P', p_state_2D, false, p_std_2D, false, vector0);
+  checkSensor(S, 'O', o_state_2D, false, o_std_2D, false, vector0);
 }
 
-TEST(SensorBase, dummy_make_shared_wrong)
+// 3D Factor
+TEST(SensorBase, factory_priors_POfactor3D)
 {
-  ParserYaml parser   = ParserYaml("test/yaml/sensor_base/sensor_dummy_wrong.yaml", wolf_root, true);
-  ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_dummy_1_wrong");
+  auto params = std::make_shared<ParamsSensorDummy>();
+  params->noise_p_std = noise_p_std;
+  params->noise_o_std = noise_o_std;
+  
+  auto S = FactorySensorPriors::create("SensorDummy","sensor1", 3, params, 
+                                       Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
+                                               {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
 
-  // POIA - 3D - INCORRECT YAML
-  WOLF_INFO("Creating sensor from name sensor_dummy_1_wrong");
+  // noise
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
-  ASSERT_THROW(std::make_shared<SensorDummy>("sensor_dummy_1_wrong", 3,
-                                             std::make_shared<ParamsSensorDummy>("sensor_dummy_1_wrong", server), 
-                                             server),
-               std::runtime_error);
+  // 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);
 }
 
-TEST(SensorBase, dummy_factory)
+// 2D Initial guess dynamic
+TEST(SensorBase, factory_priors_POinitial_guess_dynamic2D)
 {
-  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;
+  auto params = std::make_shared<ParamsSensorDummy>();
+  params->noise_p_std = noise_p_std;
+  params->noise_o_std = noise_o_std;
+  
+  auto S = FactorySensorPriors::create("SensorDummy","sensor1", 2, params, 
+                                       Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
+                                               {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
 
-  ParserYaml parser   = ParserYaml("test/yaml/sensor_base/sensor_dummy.yaml", wolf_root, true);
-  ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_dummy_1");
+  // noise
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
-  // POIA - 3D - CORRECT YAML
-  WOLF_INFO("Creating sensor from name sensor_dummy_1");
+  // 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);
+}
 
-  auto S = FactorySensor::create("SensorDummy", "sensor_dummy_1", 3, server);
+// 3D Initial guess dynamic
+TEST(SensorBase, factory_priors_POinitial_guess_dynamic3D)
+{
+  auto params = std::make_shared<ParamsSensorDummy>();
+  params->noise_p_std = noise_p_std;
+  params->noise_o_std = noise_o_std;
+  
+  auto S = FactorySensorPriors::create("SensorDummy","sensor1", 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->getNoiseStd(), po_std.head<2>(), Constants::EPS);
-  ASSERT_MATRIX_APPROX(S->getNoiseCov(), MatrixXd(po_std.head<2>().cwiseAbs2().asDiagonal()), Constants::EPS);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
-  ASSERT_EQ(S->getPriorFeatures().size(), 2);
+  ASSERT_EQ(S->getPriorFeatures().size(), 0);
 
   // check
-  checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true,  vector0);
-  checkSensor(S, 'O', o_state.head(4), true,  vector0,        false, vector0);
-  checkSensor(S, 'I', p_state.head(4), false, vector0,        true,  po_std.head(4));
-  checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true,  po_std.head(3));
+  checkSensor(S, 'P', p_state_3D, false, vector0, true, vector0);
+  checkSensor(S, 'O', o_state_3D, false, vector0, true, vector0);
 }
 
-TEST(SensorBase, dummy_factory_wrong)
+// 2D Initial guess dynamic drift
+TEST(SensorBase, factory_priors_POinitial_guess_dynamic2D_drift)
 {
-  ParserYaml parser   = ParserYaml("test/yaml/sensor_base/sensor_dummy_wrong.yaml", wolf_root, true);
-  ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_dummy_1_wrong");
+  auto params = std::make_shared<ParamsSensorDummy>();
+  params->noise_p_std = noise_p_std;
+  params->noise_o_std = noise_o_std;
+  
+  auto S = FactorySensorPriors::create("SensorDummy","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)}}));
 
-  // POIA - 3D - INCORRECT YAML
-  WOLF_INFO("Creating sensor from name sensor_dummy_1_wrong");
+  // noise
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
-  ASSERT_THROW(FactorySensor::create("SensorDummy", "sensor_dummy_1_wrong", 3, server),
-               std::runtime_error);
+  // 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);
 }
 
-TEST(SensorBase, dummy_factory_yaml)
+// 3D Initial guess dynamic drift
+TEST(SensorBase, factory_priors_POinitial_guess_dynamic3D_drift)
 {
-  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;
+  auto params = std::make_shared<ParamsSensorDummy>();
+  params->noise_p_std = noise_p_std;
+  params->noise_o_std = noise_o_std;
+  
+  auto S = FactorySensorPriors::create("SensorDummy","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)}}));
 
-  // POIA - 3D - CORRECT YAML
-  WOLF_INFO("Creating sensor from name sensor_dummy_1");
+  // noise
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
+
+  // factors
+  ASSERT_EQ(S->getPriorFeatures().size(), 0);
 
-  auto S = FactorySensorYaml::create("SensorDummy", 
-                                     "sensor_dummy_1", 
-                                     3, 
-                                     wolf_root + "/test/yaml/sensor_base/sensor_dummy.yaml");
+  // 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->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","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)}}));
+     
   // 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);
+  ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS);
 
   // factors
   ASSERT_EQ(S->getPriorFeatures().size(), 2);
 
   // check
-  checkSensor(S, 'P', p_state.head(3), false, po_std.head(3), true,  vector0);
-  checkSensor(S, 'O', o_state.head(4), true,  vector0,        false, vector0);
-  checkSensor(S, 'I', p_state.head(4), false, vector0,        true,  po_std.head(4));
-  checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true,  po_std.head(3));
+  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);
+}
 
-  // POIA - 3D - INCORRECT YAML
-  WOLF_INFO("Creating sensor from name sensor_dummy_1_wrong");
+// 3D POIA wrong
+TEST(SensorBase, factory_priors_POIA_wrong)
+{
+  auto params = std::make_shared<ParamsSensorDummyPoia>();
+  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","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)}})),
+               std::runtime_error);
+
+  // missing A
+  ASSERT_THROW(FactorySensorPriors::create("SensorDummyPoia","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)}*/})),
+               std::runtime_error);
 
-  ASSERT_THROW(FactorySensorYaml::create("SensorDummy", 
-                                         "sensor_dummy_1_wrong", 
-                                         3, 
-                                         wolf_root + "/test/yaml/sensor_base/sensor_dummy_wrong.yaml"),
+  // wrong A type (expected StateQuaternion)
+  ASSERT_THROW(FactorySensorPriors::create("SensorDummyPoia","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("StateBlock", p_state_3D, "factor", p_std_3D)}})),
                std::runtime_error);
 }
 
diff --git a/test/gtest_sensor_odom_2d.cpp b/test/gtest_sensor_odom_2d.cpp
index 7b87fdf69a6b5ea3410f180dbe9f51ba4ee6a3c1..f5ee28b608f483f55a72f8c681e5b38a8c0a3b97 100644
--- a/test/gtest_sensor_odom_2d.cpp
+++ b/test/gtest_sensor_odom_2d.cpp
@@ -131,7 +131,6 @@ void checkSensorOdom2d(SensorBasePtr S_base,
 TEST(SensorOdom2d, fix)
 {
   auto params = std::make_shared<ParamsSensorOdom2d>();
-  params->noise_std = noise_std;
   params->k_disp_to_disp = k_disp_to_disp;
   params->k_rot_to_rot = k_rot_to_rot;
   
@@ -146,7 +145,6 @@ TEST(SensorOdom2d, fix)
 TEST(SensorOdom2d, initial_guess)
 {
   auto params = std::make_shared<ParamsSensorOdom2d>();
-  params->noise_std = noise_std;
   params->k_disp_to_disp = k_disp_to_disp;
   params->k_rot_to_rot = k_rot_to_rot;
   
@@ -161,7 +159,6 @@ TEST(SensorOdom2d, initial_guess)
 TEST(SensorOdom2d, factor)
 {
   auto params = std::make_shared<ParamsSensorOdom2d>();
-  params->noise_std = noise_std;
   params->k_disp_to_disp = k_disp_to_disp;
   params->k_rot_to_rot = k_rot_to_rot;
   
@@ -176,7 +173,6 @@ TEST(SensorOdom2d, factor)
 TEST(SensorOdom2d, initial_guess_dynamic)
 {
   auto params = std::make_shared<ParamsSensorOdom2d>();
-  params->noise_std = noise_std;
   params->k_disp_to_disp = k_disp_to_disp;
   params->k_rot_to_rot = k_rot_to_rot;
   
@@ -191,7 +187,6 @@ TEST(SensorOdom2d, initial_guess_dynamic)
 TEST(SensorOdom2d, initial_guess_dynamic_drift)
 {
   auto params = std::make_shared<ParamsSensorOdom2d>();
-  params->noise_std = noise_std;
   params->k_disp_to_disp = k_disp_to_disp;
   params->k_rot_to_rot = k_rot_to_rot;
   
@@ -206,7 +201,6 @@ TEST(SensorOdom2d, initial_guess_dynamic_drift)
 TEST(SensorOdom2d, mixed)
 {
   auto params = std::make_shared<ParamsSensorOdom2d>();
-  params->noise_std = noise_std;
   params->k_disp_to_disp = k_disp_to_disp;
   params->k_rot_to_rot = k_rot_to_rot;
   
diff --git a/test/yaml/sensor_base/sensor_POIA_3D.yaml b/test/yaml/sensor_base/sensor_POIA_3D.yaml
index 4c04f54c51c1c9edaec2feb5983f6e3285644169..7ec74b1faecd03323b78b47397113019602945a7 100644
--- a/test/yaml/sensor_base/sensor_POIA_3D.yaml
+++ b/test/yaml/sensor_base/sensor_POIA_3D.yaml
@@ -20,4 +20,5 @@ states:
     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
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml b/test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml
index cd811547a5f5e9199a53eebf789f682771d0eec1..5782bcc25c61e4a3dd200081b960c822f84f84a3 100644
--- a/test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml
@@ -20,4 +20,5 @@ states:
   #   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
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor.yaml b/test/yaml/sensor_base/sensor_PO_2D_factor.yaml
index 497085eb8492e27cb57574d23e4820f21e2dc270..a7a59d8ba28336a87846612296557fb3b7d9dbab 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_factor.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_factor.yaml
@@ -10,4 +10,5 @@ states:
     state: [1]
     noise_std: [0.1]
     dynamic: false
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic.yaml b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic.yaml
index 889d53245c286e14e25b532d507f265e925a699f..9bed5e2bfed7ed1b06bca865482ef49e91f9e4ea 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic.yaml
@@ -10,4 +10,5 @@ states:
     state: [1]
     noise_std: [0.1]
     dynamic: true
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift.yaml b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift.yaml
index 733ec88fbc29f6e02eb9877c07c1fd5fd315c9ac..d7b3ecb231ae656b545e31e32ca5d737ddcf0bdb 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift.yaml
@@ -12,4 +12,5 @@ states:
     noise_std: [0.1]
     dynamic: true
     drift_std: [0.1]
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
index a1d4e79eb53fee4e8f7d1fac009c41b14c5ced5f..abf288b221ee56363146efc4f65dfabf206851f6 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
@@ -12,4 +12,5 @@ states:
     noise_std: [0.1]
     dynamic: true
     drift_std: [0.1]
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_wrong.yaml
index d28cb5f1eb471e36d4ac355652c39b5822fe3b68..9ed9a58fa58264bc1db5602ba4e3e6bc88efaa4e 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_wrong.yaml
@@ -10,4 +10,5 @@ states:
     state: [1]
     noise_std: [0.1]
     dynamic: true
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_factor_wrong.yaml
index c02f9dfbc17c4048cf7e25b8d5647b5868e701fa..a781d137fae907f7731cb9d8e09d07f9eb5a6fe5 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_factor_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_factor_wrong.yaml
@@ -10,4 +10,5 @@ states:
     #state: [1] #missing
     noise_std: [0.1]
     dynamic: false
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix.yaml
index 8d9e966274c905a10a591d802d74c3ad1c9938d4..4125b1f4a1ef3db7ed874aafbe1969a65330dde6 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_fix.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_fix.yaml
@@ -8,4 +8,5 @@ states:
     mode: fix
     state: [1]
     dynamic: false
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic.yaml
index a20af82a31cb5fdd67d926d3f29181c6277ca857..f706700135da85f6ceab0083c42e7736dafe4783 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic.yaml
@@ -8,4 +8,5 @@ states:
     mode: fix
     state: [1]
     dynamic: true
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift.yaml
index 410281dbe08d60cc61099a8c006f0b86d1e080b1..25a40db4275bed61a5ecb5543d723eab08f1acd8 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift.yaml
@@ -10,4 +10,5 @@ states:
     state: [1]
     dynamic: true
     drift_std: [0.1]
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
index 2da413660cf9106e33c6ded18e71fdd2331075b5..622158be35117453734590e2b9987bd84e8210e1 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
@@ -10,4 +10,5 @@ states:
     state: [1]
     dynamic: true
     drift_std: [0.1]
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_wrong.yaml
index 27affb496a0e4f213e7911b8dcf32ec5d5bf5c7a..d66f2f21c6bf734aee5f8c5030554b77dde43fba 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_wrong.yaml
@@ -8,4 +8,5 @@ states:
     mode: fix
     state: [1]
     dynamic: true
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix_wrong.yaml
index 10179eae0fe1b151eff97e26a6e60656ac35c0c8..b349248945347e66422f20281b9c714eefa2b0ec 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_fix_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_fix_wrong.yaml
@@ -8,4 +8,5 @@ states:
     mode: fix
     state: [1]
     dynamic: false
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess.yaml b/test/yaml/sensor_base/sensor_PO_2D_initial_guess.yaml
index 901364dfbbe896443bb1e5d074de17c6085006ec..937ddf2ab0946987fd28cf4e68e53ef9e0b450df 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_initial_guess.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_initial_guess.yaml
@@ -8,4 +8,5 @@ states:
     mode: initial_guess
     state: [1]
     dynamic: false
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic.yaml b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic.yaml
index 0bfebd5b4bdadb875db5c819cec4baf5cc84245c..e979574969acf4eb2f2c861ed2ee127c4fdfb3bd 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic.yaml
@@ -8,4 +8,5 @@ states:
     mode: initial_guess
     state: [1]
     dynamic: true
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift.yaml
index ec021b9d46f3c13df799735aa21864fec28bfc75..49aaaaac83f3451c1fde21276768cca629403507 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift.yaml
@@ -10,4 +10,5 @@ states:
     state: [1]
     dynamic: true
     drift_std: [0.1]
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
index 9e8a172f98e5fcfd3e5fc87a29231c6c692c4c4a..cc9e32246bbfc97c6e8739db00399e4a2e4398bc 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
@@ -10,4 +10,5 @@ states:
     state: [1]
     dynamic: true
     drift_std: [0.1]
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
index b4e4c44be2745e69a11aa50bf3b278c79e893b81..046ede375380deaec7e0a98dfcf570cad341bea2 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
@@ -8,4 +8,5 @@ states:
     mode: initial_guess
     state: [1]
     dynamic: true
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_wrong.yaml
index cb55b4446ea8e0ef37b9d7a92d5bfd94006f5fbe..ce2792d2251d9ae705029e36bef3c976f8f42340 100644
--- a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_wrong.yaml
@@ -8,4 +8,5 @@ states:
     #mode: initial_guess #missing
     state: [1]
     dynamic: false
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor.yaml b/test/yaml/sensor_base/sensor_PO_3D_factor.yaml
index 665ac5cd2e2b14f860fbabac39dced993fac19c4..6084ec5d397a7ca54888c967198b73eee7be53bc 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_factor.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_factor.yaml
@@ -10,4 +10,5 @@ states:
     state: [1, 0, 0, 0]
     noise_std: [0.1, 0.2, 0.3]
     dynamic: false
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic.yaml b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic.yaml
index b1fc18e127bcfabaa088f4126e6ccb3d9dfa7776..ab974d37a2abef05b685f7bb918ee2bdf29431a2 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic.yaml
@@ -10,4 +10,5 @@ states:
     state: [1, 0, 0, 0]
     noise_std: [0.1, 0.2, 0.3]
     dynamic: true
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift.yaml b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift.yaml
index f016ed4fda8150e54da25c0c8fea9d7650095b5a..65940ecb858217588bdec4c0a8ba61ea79f05033 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift.yaml
@@ -12,4 +12,5 @@ states:
     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
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
index 89a3409d9f5ebf9a1e7168b61b36173e56f4cdd4..f00748d655526a3f185f8ad91cfd85d699ee3b81 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
@@ -12,4 +12,5 @@ states:
     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]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_wrong.yaml
index d2da5482ddf25cf5ae99ecc4cf4fbbd9b2308689..095862ddc7758660387d5e69e0e9ce5966deeb38 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_wrong.yaml
@@ -10,4 +10,5 @@ states:
     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]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_factor_wrong.yaml
index 05bdc260a54a6909a6deea2df0a6d7895d0e755d..ba143662c6d832f0a4096e9d39d21575b1258baf 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_factor_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_factor_wrong.yaml
@@ -10,4 +10,5 @@ states:
     state: [1, 0, 0, 0]
     noise_std: [0.1, 0.2, 0.3]
     dynamic: false
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix.yaml b/test/yaml/sensor_base/sensor_PO_3D_fix.yaml
index a9e83338120ffdbbe7d8b6ae9a3160242b102504..d3614ede680d2a9346c276077752812a715338b5 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_fix.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_fix.yaml
@@ -8,4 +8,5 @@ states:
     mode: fix
     state: [1, 0, 0, 0]
     dynamic: false
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic.yaml b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic.yaml
index a490378968b7e6540ca71460bb907f4d9b727dce..0329cd837b057de960640354da2baf38979f5a7e 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic.yaml
@@ -8,4 +8,5 @@ states:
     mode: fix
     state: [1, 0, 0, 0]
     dynamic: true
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift.yaml b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift.yaml
index 23f5848b90ab20eaad617cfd934b00de31d2fcca..3c2bedefe52348215f84e8c4f5bdb16eec9078d8 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift.yaml
@@ -10,4 +10,5 @@ states:
     state: [1, 0, 0, 0]
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
index 58df603f77ebb76be2e9c8661f7822e8d5ac4c78..47286195af2d766d52506dc0ab45e35e90c52005 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
@@ -10,4 +10,5 @@ states:
     state: [1, 0, 0, 0]
     #dynamic: true #missing
     drift_std: [0.1, 0.2, 0.3]
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_wrong.yaml
index dd89b3f6cdce6c844b645fef4a8b2ff977e2ad30..c5f6ed7a6e4075b845fae7f4627fa75714aa2291 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_wrong.yaml
@@ -8,4 +8,5 @@ states:
     mode: fix
     state: [1, 0, 0, 1] # not normalized
     dynamic: true
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_fix_wrong.yaml
index 498ecdd9e0721ec413e0adc780c1094d2064a3eb..99c0f477f995c26cc21187dda5c481ff1415d0af 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_fix_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_fix_wrong.yaml
@@ -8,4 +8,5 @@ states:
     mode: fix
     state: [1, 0, 0, 0]
     dynamic: false
-#noise_std: [0.1, 0.2] #missing
\ No newline at end of file
+noise_p_std: 0.1 
+#noise_o_std: 0.01 #missing
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess.yaml b/test/yaml/sensor_base/sensor_PO_3D_initial_guess.yaml
index 9ad6891fb7fcf77e5ea0e29fa1f8646eff9e3f7b..bb396634cc3bd5e72899710f502e2ecf1a3c176e 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_initial_guess.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_initial_guess.yaml
@@ -8,4 +8,5 @@ states:
     mode: initial_guess
     state: [1, 0, 0, 0]
     dynamic: false
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic.yaml b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic.yaml
index 1cf0568a8635bad5454db769918940c3372667bc..ae08f21dbb73fa9992d61940b5699c59d36ac0b8 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic.yaml
@@ -8,4 +8,5 @@ states:
     mode: initial_guess
     state: [1, 0, 0, 0]
     dynamic: true
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift.yaml
index bbc3b2558933466d8c028ab3ff1291133638ec85..2d9773d128d776ef8606cf93901eee2ae91a6ef8 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift.yaml
@@ -10,4 +10,5 @@ states:
     state: [1, 0, 0, 0]
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
index 7c96eb4b42504919b4f01b12901c90968471dba0..d9094f9d2b2ce72da62436efaca27e8b635bc21f 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
@@ -10,4 +10,5 @@ states:
     state: [1, 0, 0, 0]
     dynamic: true
     drift_std: [0.1, 0.2, 0.3]
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
index 41d218bea2adb73f73a66f0637c3ac982482bf73..ced2cbcf5fd3a803b6d0393b002a43a6f9a2fbe6 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
@@ -8,4 +8,5 @@ states:
     mode: initial_guess
     state: [1, 0, 0, 0]
     dynamic: true
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_wrong.yaml
index 243de825be306bfbcba7b4495ffff74401896b1e..842d84d35b0c6a2eaf80136865aa9ce0823c18c1 100644
--- a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_wrong.yaml
+++ b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_wrong.yaml
@@ -8,4 +8,5 @@ states:
   #   mode: initial_guess
   #   state: [1, 0, 0, 0]
   #   dynamic: false
-noise_std: [0.1, 0.2]
\ No newline at end of file
+noise_p_std: 0.1
+noise_o_std: 0.01
\ No newline at end of file