From 1d570cc51e4fdc0691fa9d50d454790057cadeda Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Thu, 26 May 2022 14:35:26 +0200
Subject: [PATCH] [skip ci] added computeNoiseCov(..)=0

---
 demos/hello_wolf/processor_range_bearing.cpp  |   2 +-
 demos/hello_wolf/sensor_range_bearing.cpp     |  12 +-
 demos/hello_wolf/sensor_range_bearing.h       |   6 +
 include/core/sensor/sensor_base.h             |  20 +-
 include/core/sensor/sensor_odom_2d.h          |   3 +-
 src/capture/capture_motion.cpp                |   2 +-
 src/processor/processor_motion.cpp            |  10 +-
 src/sensor/sensor_base.cpp                    |  30 +-
 src/sensor/sensor_odom_2d.cpp                 |   2 +-
 test/dummy/sensor_dummy.h                     |  41 +-
 test/dummy/sensor_dummy_poia.h                | 104 +++
 test/gtest_sensor_base.cpp                    | 822 +++++++++++++-----
 test/gtest_sensor_odom_2d.cpp                 |   6 -
 test/yaml/sensor_base/sensor_POIA_3D.yaml     |   3 +-
 .../sensor_base/sensor_POIA_3D_wrong.yaml     |   3 +-
 .../yaml/sensor_base/sensor_PO_2D_factor.yaml |   3 +-
 .../sensor_PO_2D_factor_dynamic.yaml          |   3 +-
 .../sensor_PO_2D_factor_dynamic_drift.yaml    |   3 +-
 ...nsor_PO_2D_factor_dynamic_drift_wrong.yaml |   3 +-
 .../sensor_PO_2D_factor_dynamic_wrong.yaml    |   3 +-
 .../sensor_PO_2D_factor_wrong.yaml            |   3 +-
 test/yaml/sensor_base/sensor_PO_2D_fix.yaml   |   3 +-
 .../sensor_base/sensor_PO_2D_fix_dynamic.yaml |   3 +-
 .../sensor_PO_2D_fix_dynamic_drift.yaml       |   3 +-
 .../sensor_PO_2D_fix_dynamic_drift_wrong.yaml |   3 +-
 .../sensor_PO_2D_fix_dynamic_wrong.yaml       |   3 +-
 .../sensor_base/sensor_PO_2D_fix_wrong.yaml   |   3 +-
 .../sensor_PO_2D_initial_guess.yaml           |   3 +-
 .../sensor_PO_2D_initial_guess_dynamic.yaml   |   3 +-
 ...sor_PO_2D_initial_guess_dynamic_drift.yaml |   3 +-
 ..._2D_initial_guess_dynamic_drift_wrong.yaml |   3 +-
 ...sor_PO_2D_initial_guess_dynamic_wrong.yaml |   3 +-
 .../sensor_PO_2D_initial_guess_wrong.yaml     |   3 +-
 .../yaml/sensor_base/sensor_PO_3D_factor.yaml |   3 +-
 .../sensor_PO_3D_factor_dynamic.yaml          |   3 +-
 .../sensor_PO_3D_factor_dynamic_drift.yaml    |   3 +-
 ...nsor_PO_3D_factor_dynamic_drift_wrong.yaml |   3 +-
 .../sensor_PO_3D_factor_dynamic_wrong.yaml    |   3 +-
 .../sensor_PO_3D_factor_wrong.yaml            |   3 +-
 test/yaml/sensor_base/sensor_PO_3D_fix.yaml   |   3 +-
 .../sensor_base/sensor_PO_3D_fix_dynamic.yaml |   3 +-
 .../sensor_PO_3D_fix_dynamic_drift.yaml       |   3 +-
 .../sensor_PO_3D_fix_dynamic_drift_wrong.yaml |   3 +-
 .../sensor_PO_3D_fix_dynamic_wrong.yaml       |   3 +-
 .../sensor_base/sensor_PO_3D_fix_wrong.yaml   |   3 +-
 .../sensor_PO_3D_initial_guess.yaml           |   3 +-
 .../sensor_PO_3D_initial_guess_dynamic.yaml   |   3 +-
 ...sor_PO_3D_initial_guess_dynamic_drift.yaml |   3 +-
 ..._3D_initial_guess_dynamic_drift_wrong.yaml |   3 +-
 ...sor_PO_3D_initial_guess_dynamic_wrong.yaml |   3 +-
 .../sensor_PO_3D_initial_guess_wrong.yaml     |   3 +-
 51 files changed, 846 insertions(+), 328 deletions(-)
 create mode 100644 test/dummy/sensor_dummy_poia.h

diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
index 06469b0b1..db0f5c12b 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 d81740193..9b680a977 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 76170f9b1..1aa70ac26 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 3059521df..6550a00aa 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 ede10ff4b..b386829fe 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 e8f48dcd7..3e15c1af8 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 0ad4a754d..31b2b770a 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 17bb36595..cf8191714 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 fae0fe9a7..0abef3899 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 e73f7d3c7..5421d72fa 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 000000000..849231de5
--- /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 d86c98b8c..559cb5883 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 7b87fdf69..f5ee28b60 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 4c04f54c5..7ec74b1fa 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 cd811547a..5782bcc25 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 497085eb8..a7a59d8ba 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 889d53245..9bed5e2bf 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 733ec88fb..d7b3ecb23 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 a1d4e79eb..abf288b22 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 d28cb5f1e..9ed9a58fa 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 c02f9dfbc..a781d137f 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 8d9e96627..4125b1f4a 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 a20af82a3..f70670013 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 410281dbe..25a40db42 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 2da413660..622158be3 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 27affb496..d66f2f21c 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 10179eae0..b34924894 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 901364dfb..937ddf2ab 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 0bfebd5b4..e97957496 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 ec021b9d4..49aaaaac8 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 9e8a172f9..cc9e32246 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 b4e4c44be..046ede375 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 cb55b4446..ce2792d22 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 665ac5cd2..6084ec5d3 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 b1fc18e12..ab974d37a 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 f016ed4fd..65940ecb8 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 89a3409d9..f00748d65 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 d2da5482d..095862ddc 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 05bdc260a..ba143662c 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 a9e833381..d3614ede6 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 a49037896..0329cd837 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 23f5848b9..3c2bedefe 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 58df603f7..47286195a 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 dd89b3f6c..c5f6ed7a6 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 498ecdd9e..99c0f477f 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 9ad6891fb..bb396634c 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 1cf0568a8..ae08f21db 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 bbc3b2558..2d9773d12 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 7c96eb4b4..d9094f9d2 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 41d218bea..ced2cbcf5 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 243de825b..842d84d35 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
-- 
GitLab