diff --git a/include/core/math/covariance.h b/include/core/math/covariance.h
index acb4c086282ea48d37c0394aab2c18b75daaab0d..06ae067ccb0e7be12ead33c56839efc15256d19e 100644
--- a/include/core/math/covariance.h
+++ b/include/core/math/covariance.h
@@ -37,6 +37,8 @@ template <typename T, int N, int RC>
 inline bool isSymmetric(const Eigen::Matrix<T, N, N, RC>& M,
                  const T eps = wolf::Constants::EPS)
 {
+  if (M.cols() != M.rows()) 
+    return false;
   return M.isApprox(M.transpose(), eps);
 }
 
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index f57725decd14bc50d4bec57936a8d2de5bf8c316..8bb25c437970d14dc7cb48b30567b990c0ae386b 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -128,8 +128,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
 
     protected:
-        Eigen::VectorXd noise_std_; // std of sensor noise
-        Eigen::MatrixXd noise_cov_; // cov matrix of noise
+        Eigen::MatrixXd noise_cov_;      // cov matrix of noise
+        std::map<char, Eigen::MatrixXd> drift_rate_covs_; // covariance rate of the drift of dynamic state blocks
 
         void setProblem(ProblemPtr _problem) override final;
         virtual void loadParams(ParamsSensorBasePtr _params);
@@ -163,7 +163,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
                    const std::string& _unique_name,
                    const SizeEigen& _dim,
                    const ParamsServer& _server,
-                   std::unordered_map<char, std::string> _keys_types);
+                   const std::unordered_map<char, std::string>&  _keys_types_apart_from_PO={});
 
         ~SensorBase() override;
 
@@ -245,23 +245,24 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         void addPriorParameter(const char& _key,
                                const Eigen::VectorXd& _x,
                                const Eigen::MatrixXd& _cov,
-                               unsigned int _start_idx = 0,
-                               int _size = -1);
+                               unsigned int _start_idx = 0);
         void addPriorP(const Eigen::VectorXd& _x,
                        const Eigen::MatrixXd& _cov,
-                       unsigned int _start_idx = 0,
-                       int _size = -1);
+                       unsigned int _start_idx = 0);
         void addPriorO(const Eigen::VectorXd& _x,
                        const Eigen::MatrixXd& _cov);
         void addPriorIntrinsics(const Eigen::VectorXd& _x,
                                 const Eigen::MatrixXd& _cov,
-                                unsigned int _start_idx = 0,
-                                int _size = -1);
+                                unsigned int _start_idx = 0);
 
         void setNoiseStd(const Eigen::VectorXd & _noise_std);
-        void setNoiseCov(const Eigen::MatrixXd & _noise_std);
+        void setNoiseCov(const Eigen::MatrixXd & _noise_cov);
+        void setDriftRateStd(char, const Eigen::VectorXd & _drift_rate_std);
+        void setDriftRateCov(char, const Eigen::MatrixXd & _drift_rate_cov);
         Eigen::VectorXd getNoiseStd() const;
         Eigen::MatrixXd getNoiseCov() const;
+        Eigen::VectorXd getDriftRateStd(char) const;
+        Eigen::MatrixXd getDriftRateCov(char) const;
 
         virtual void printHeader(int depth, //
                                  bool constr_by, //
@@ -353,7 +354,7 @@ inline bool SensorBase::isStateBlockDynamic(const char& _key) const
 
 inline Eigen::VectorXd SensorBase::getNoiseStd() const
 {
-    return noise_std_;
+    return noise_cov_.diagonal().cwiseSqrt();
 }
 
 inline Eigen::MatrixXd SensorBase::getNoiseCov() const
@@ -361,6 +362,25 @@ inline Eigen::MatrixXd SensorBase::getNoiseCov() const
     return noise_cov_;
 }
 
+inline Eigen::VectorXd SensorBase::getDriftRateStd(char _key) const
+{
+    if (drift_rate_covs_.count(_key) == 0)
+        return Eigen::MatrixXd(0,0);
+    else
+        return drift_rate_covs_.at(_key).diagonal().cwiseSqrt();
+}
+
+inline Eigen::MatrixXd SensorBase::getDriftRateCov(char _key) const
+{
+    if (drift_rate_covs_.count(_key) == 0)
+        return Eigen::MatrixXd(0,0);
+    else
+        return drift_rate_covs_.at(_key);
+}
+
+void setDriftRateStd(char, const Eigen::VectorXd & _drift_rate_std);
+void setDriftRateCov(char, const Eigen::MatrixXd & _drift_rate_cov);
+
 inline HardwareBaseConstPtr SensorBase::getHardware() const
 {
     return hardware_ptr_.lock();
@@ -376,9 +396,9 @@ inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr)
     hardware_ptr_ = _hw_ptr;
 }
 
-inline void SensorBase::addPriorP(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
+inline void SensorBase::addPriorP(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx)
 {
-    addPriorParameter('P', _x, _cov, _start_idx, _size);
+    addPriorParameter('P', _x, _cov, _start_idx);
 }
 
 inline void SensorBase::addPriorO(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov)
@@ -386,9 +406,9 @@ inline void SensorBase::addPriorO(const Eigen::VectorXd& _x, const Eigen::Matrix
     addPriorParameter('O', _x, _cov);
 }
 
-inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
+inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx)
 {
-    addPriorParameter('I', _x, _cov);
+    addPriorParameter('I', _x, _cov, _start_idx);
 }
 
 } // namespace wolf
diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h
index e1bab00d989329004bef89c074b9c95e07be7ced..8f422da10bb8ad37fa2714c4db7a3c025cc33b6f 100644
--- a/include/core/state_block/prior.h
+++ b/include/core/state_block/prior.h
@@ -38,21 +38,21 @@ typedef std::unordered_map<char, Prior> Priors;
 class Prior
 {
   private:
-    std::string type_;             // State type
-    std::string mode_;             // Prior mode. Can be 'nothing', 'initial_guess', 'fix' and 'factor'
-    Eigen::VectorXd state_;        // state values (only filled from server if mode != 'nothing')
-    Eigen::VectorXd sigma_;        // factor sigmas (only filled from server if mode == 'factor')
-    bool dynamic_;                 // State dynamic
-    Eigen::VectorXd sigma_drift_;  // drift of the state (only filled from server if dynamic)
+    std::string type_;                // State type
+    std::string mode_;                // Prior mode. Can be 'initial_guess', 'fix' and 'factor'
+    Eigen::VectorXd state_;           // state values
+    Eigen::VectorXd sigma_std_;       // factor sigmas (only filled from server if mode == 'factor')
+    bool dynamic_;                    // State dynamic
+    Eigen::VectorXd sigma_drift_std_; // drift of the state (only filled from server if dynamic)
 
   public:
     Prior() = default;
     Prior(const std::string& _type, 
-          const std::string& _mode,  
-          const Eigen::VectorXd& _state, 
-          const Eigen::VectorXd& _sigma = Eigen::VectorXd(0), 
-          bool _dynamic = false, 
-          const Eigen::VectorXd& _sigma_drift = Eigen::VectorXd(0));
+          const Eigen::VectorXd& _state,
+          const std::string& _mode = "fix",  
+          const Eigen::VectorXd& _sigma_std = Eigen::VectorXd(0), 
+          bool _dynamic = false,
+          const Eigen::VectorXd& _sigma_drift_std = Eigen::VectorXd(0));
 
     Prior(const std::string& _prefix, const std::string& _type, const ParamsServer& _server);
 
@@ -61,12 +61,12 @@ class Prior
     const std::string& getType() const;
     const std::string& getMode() const;
     const Eigen::VectorXd& getState() const;
-    const Eigen::VectorXd& getSigma() const;
+    const Eigen::VectorXd& getSigmaStd() const;
     bool isDynamic() const;
     bool isFixed() const;
     bool isInitialGuess() const;
     bool isFactor() const;
-    const Eigen::VectorXd& getSigmaDrift() const;
+    const Eigen::VectorXd& getSigmaDriftStd() const;
 
     void check() const;
 
@@ -88,9 +88,9 @@ inline const Eigen::VectorXd& Prior::getState() const
   return state_;
 }
 
-inline const Eigen::VectorXd& Prior::getSigma() const
+inline const Eigen::VectorXd& Prior::getSigmaStd() const
 {
-  return sigma_;
+  return sigma_std_;
 }
 
 inline bool Prior::isDynamic() const
@@ -113,9 +113,9 @@ inline bool Prior::isFactor() const
   return mode_ == "factor";
 }
 
-inline const Eigen::VectorXd& Prior::getSigmaDrift() const
+inline const Eigen::VectorXd& Prior::getSigmaDriftStd() const
 {
-  return sigma_drift_;
+  return sigma_drift_std_;
 }
 }
 #endif
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index dc92cbe1d59a68c106d041fa0049e07f65a08903..8282a9c5c1a4fae1ac5b09585a827c93f3ea5cde 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -27,6 +27,7 @@
 #include "core/state_block/state_quaternion.h"
 #include "core/factor/factor_block_absolute.h"
 #include "core/factor/factor_quaternion_absolute.h"
+#include "core/math/covariance.h"
 
 namespace wolf {
 
@@ -58,7 +59,7 @@ SensorBase::SensorBase(const std::string& _type,
                        const std::string& _unique_name,
                        const SizeEigen& _dim,
                        const ParamsServer& _server,
-                       std::unordered_map<char, std::string> _keys_types) :
+                       const std::unordered_map<char, std::string>& _keys_types_apart_from_PO) :
         NodeBase("SENSOR", _type, _unique_name),
         HasStateBlocks(""),
         hardware_ptr_(),
@@ -67,6 +68,10 @@ SensorBase::SensorBase(const std::string& _type,
         params_prior_map_(),
         last_capture_(nullptr)
 {
+    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");
+
     // create params
     auto params = std::make_shared<ParamsSensorBase>("sensor/" + _unique_name, _server);
 
@@ -74,8 +79,11 @@ SensorBase::SensorBase(const std::string& _type,
     loadParams(params);
 
     // create priors
+    std::unordered_map<char, std::string> keys_types = _keys_types_apart_from_PO;
+    keys_types['P'] = "P"; // equivalent to "StateBlock";
+    keys_types['O'] = "O"; // equivalent to (_dim == 2 ? "StateAngle" : "StateQuaternion");
     Priors priors;
-    for (auto pair : _keys_types)
+    for (auto pair : keys_types)
         priors[pair.first] = Prior("sensor/" + _unique_name, pair.second, _server);
 
     // load priors
@@ -102,29 +110,43 @@ void SensorBase::loadPriors(const Priors& _priors, SizeEigen _dim)
         auto key = prior_pair.first;
         const Prior& prior = prior_pair.second;
 
-        // Checks
-        if (key == 'P' and prior.getState().size() != _dim)
-            throw std::runtime_error("Prior state for P has wrong size");
+        // Check consistency of keys
+        if (key == 'P' and prior.getType() != "P" and prior.getType() != "StateBlock")
+            throw std::runtime_error("Prior type for key P has to be 'P' or 'StateBlock'");
+        if (key == 'V' and prior.getType() != "V" and prior.getType() != "StateBlock")
+            throw std::runtime_error("Prior type for key V has to be 'V' or 'StateBlock'");
+        if (key == 'O' and _dim == 2 and prior.getType() != "O" and prior.getType() != "StateAngle")
+            throw std::runtime_error("Prior type for key O in 2D has to be 'O' or 'StateAngle'");
+        if (key == 'O' and _dim == 3 and prior.getType() != "O" and prior.getType() != "StateQuaternion")
+            throw std::runtime_error("Prior type for key O in 3D has to be 'O' or 'StateQuaternion'");
+
+        // Checks state size (P, O, V)
+        if ((key == 'P' or key == 'V') and prior.getState().size() != _dim)
+            throw std::runtime_error("Prior state for P or V has wrong size: " + to_string(prior.getState().size()) + " in  " + (_dim == 2 ? "2D" : "3D"));
         if (key == '0' and prior.getState().size() != (_dim == 2 ? 1 : 4))
-            throw std::runtime_error("Prior state for O has wrong size");
+            throw std::runtime_error("Prior state for O has wrong size: " + to_string(prior.getState().size()) + " in " + (_dim == 2 ? "2D" : "3D"));
 
         // create state block
         auto sb = FactoryStateBlock::create(prior.getType(), prior.getState(), prior.isFixed());
 
-        // check local param
-        if (not sb->isValid())
-        {
-            WOLF_ERROR("The created stateblock ", prior.getType(), " for key ", std::string(1,key), " is not valid (local param violation)");
-            throw std::runtime_error("StateBlock not valid");
-        }
+        // check local param 
+        if (not sb->isValid()) // always false I think...
+            throw std::runtime_error("The created stateblock " + prior.getType() + " for key " + std::string(1,key) + " is not valid (local param violation)");
 
         // Add state block
         addStateBlock(key, sb, prior.isDynamic());
 
         // Factor
         if (prior.isFactor())
-            addPriorParameter(key, prior.getState(), prior.getSigma());
+            addPriorParameter(key, prior.getState(), prior.getSigmaStd().cwiseAbs2().asDiagonal());
+
+        // Drift covariance rates
+        if (prior.isDynamic())
+            setDriftRateStd(key, prior.getSigmaDriftStd().cwiseAbs2().asDiagonal());
     }
+
+    if (not hasStateBlock('P') or not hasStateBlock('O'))
+        throw std::runtime_error("SensorBase prior should have at least 'P' and 'O' keys defined");
 }
 
 void SensorBase::fixExtrinsics()
@@ -173,26 +195,54 @@ void SensorBase::unfixIntrinsics()
     }
 }
 
-void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
+void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx)
 {
-    assert(!isStateBlockInCapture(_key) && "SensorBase::addPriorParameter only allowed for static parameters");
+    assert(isCovariance(_cov) and "SensorBase::addPriorParameter: provided _cov is not a covariance matrix");
 
     StateBlockPtr _sb = getStateBlock(_key);
-    bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
 
-    assert(((!is_quaternion && _x.size() == _cov.rows() && _x.size() == _cov.cols()) ||
-            (is_quaternion && _x.size() == 4 &&_cov.rows() == 3 && _cov.cols() == 3)) && "bad prior/covariance dimensions");
-    assert((_size == -1 && _start_idx == 0) || (_size+_start_idx <= _sb->getSize()));
-    assert(_size == -1 || _size == _x.size());
-    assert(!(_size != -1 && is_quaternion) && "prior of a segment of state not available for quaternion");
+    // CHECKS
+    // missing key
+    if (not _sb)
+        throw std::runtime_error("SensorBase::addPriorParameter: missing key " + std::string(1,_key));
+
+    // Overparametrized states
+    if (_sb->getSize() != _sb->getLocalSize())
+    {
+        // not segment of state
+        if (_start_idx != 0)
+            throw std::runtime_error("SensorBase::addPriorParameter: Prior for a segment of a state with local size different from size not allowed");
+        // state size
+        if (_x.size() != _sb->getSize())
+            throw std::runtime_error("SensorBase::addPriorParameter: Bad state size");
+        // cov size
+        if (_cov.cols() != _sb->getLocalSize())
+            throw std::runtime_error("SensorBase::addPriorParameter: Bad covariance size");
+    }
+    else
+    {
+        // cov and state size
+        if (_cov.cols() != _x.size())
+            throw std::runtime_error("SensorBase::addPriorParameter: Inconsistend measurement and covariance sizes");
+
+        // state size
+        if (_x.size() + _start_idx > _sb->getSize())
+            throw std::runtime_error("SensorBase::addPriorParameter: Prior for a segment of a state with local size different from size not allowed");
+    }
+    
+    // dynamic state blocks
+    if (isStateBlockInCapture(_key))
+        throw std::runtime_error("SensorBase::addPriorParameter only allowed for static parameters");
+
 
+    // APPLY PRIOR
     // set StateBlock state
-    if (_size == -1)
+    if (_x.size() == _sb->getSize())
         _sb->setState(_x);
     else
     {
         Eigen::VectorXd new_x = _sb->getState();
-        new_x.segment(_start_idx,_size) = _x;
+        new_x.segment(_start_idx,_x.size()) = _x;
         _sb->setState(new_x);
     }
 
@@ -201,16 +251,17 @@ void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x,
         params_prior_map_[_key]->remove();
 
     // create feature
-    FeatureBasePtr ftr_prior = FeatureBase::emplace<FeatureBase>(nullptr, "ABSOLUTE",_x,_cov); //std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
+    FeatureBasePtr ftr_prior = FeatureBase::emplace<FeatureBase>(nullptr, "ABSOLUTE",_x,_cov);
 
     // set feature problem
-    if(getProblem()) ftr_prior->setProblem(getProblem());
+    ftr_prior->setProblem(getProblem());
 
     // create & add factor absolute
+    bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
     if (is_quaternion)
         FactorBase::emplace<FactorQuaternionAbsolute>(ftr_prior, ftr_prior, _sb, nullptr, false);
     else
-        FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, ftr_prior, _sb, _start_idx, _size, nullptr, false);
+        FactorBase::emplace<FactorBlockAbsolute>(ftr_prior, ftr_prior, _sb, _start_idx, _x.size(), nullptr, false);
 
     // store feature in params_prior_map_
     params_prior_map_[_key] = ftr_prior;
@@ -233,16 +284,25 @@ void SensorBase::registerNewStateBlocks(ProblemPtr _problem)
 
 void SensorBase::setNoiseStd(const Eigen::VectorXd& _noise_std) 
 {
-    noise_std_ = _noise_std;
     noise_cov_ = _noise_std.array().square().matrix().asDiagonal();
 }
 
 void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) 
 {
-    noise_std_ = _noise_cov.diagonal().array().sqrt();
     noise_cov_ = _noise_cov;
 }
 
+void SensorBase::setDriftRateStd(char _key, const Eigen::VectorXd & _drift_rate_std)
+{
+    drift_rate_covs_[_key] = _drift_rate_std.cwiseAbs2().asDiagonal();
+}
+
+void SensorBase::setDriftRateCov(char _key, const Eigen::MatrixXd & _drift_rate_cov)
+{
+    drift_rate_covs_[_key] = _drift_rate_cov;
+}
+
+
 void SensorBase::setLastCapture(CaptureBasePtr cap)
 {
     assert(cap);
diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp
index 172d49aaa05c816cbf19bacac25c8a372118fbd7..7517c27f3e7f238a9b2e3373fd499c1a7b7a4696 100644
--- a/src/state_block/prior.cpp
+++ b/src/state_block/prior.cpp
@@ -27,17 +27,17 @@
 namespace wolf {
 
 Prior::Prior(const std::string& _type, 
-             const std::string& _mode,  
              const Eigen::VectorXd& _state, 
+             const std::string& _mode,  
              const Eigen::VectorXd& _sigma, 
              bool _dynamic, 
-             const Eigen::VectorXd& _sigma_drift) : 
+             const Eigen::VectorXd& _sigma_drift_std) : 
   type_(_type),
   mode_(_mode),
   state_(_state),
-  sigma_(_sigma),
+  sigma_std_(_sigma),
   dynamic_(_dynamic),
-  sigma_drift_(_sigma_drift)
+  sigma_drift_std_(_sigma_drift_std)
 {
   check();
 }
@@ -45,35 +45,28 @@ Prior::Prior(const std::string& _type,
 Prior::Prior(const std::string& _prefix, const std::string& _type, const ParamsServer& _server)
 {
   type_ = _type;
+  state_  = _server.getParam<Eigen::VectorXd>(_prefix + "/state");
   mode_   = _server.getParam<std::string>(_prefix + "/mode");
 
-  if (mode_ != "nothing")
-    state_ = _server.getParam<Eigen::VectorXd>(_prefix + "/state");
-  else
-    state_ = Eigen::VectorXd(0);
-
   if (mode_ == "factor")
-    sigma_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma");
+    sigma_std_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma");
   else
-    sigma_ = Eigen::VectorXd(0);
+    sigma_std_ = Eigen::VectorXd(0);
 
   dynamic_ = _server.getParam<bool>(_prefix + "/dynamic");
 
   if (dynamic_)
-    sigma_drift_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma_drift");
+    sigma_drift_std_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma_drift_std");
   else
-    sigma_drift_ = Eigen::VectorXd(0);
+    sigma_drift_std_ = Eigen::VectorXd(0);
 
   check();
 }
 
 void Prior::check() const
 {
-    if (mode_ != "nothing" and mode_ != "initial_guess" and mode_ != "fix" and mode_ != "factor") 
-        throw std::runtime_error("Prior::check() wrong mode value: " + mode_ + ", it should be: 'nothing', 'initial_guess', 'fix' or 'factor'. " + print());
-
-    if (mode_ == "nothing" and state_.size() == 0)
-      return;
+    if (mode_ != "initial_guess" and mode_ != "fix" and mode_ != "factor") 
+        throw std::runtime_error("Prior::check() wrong mode value: " + mode_ + ", it should be: 'initial_guess', 'fix' or 'factor'. " + print());
 
     // try to create a state block and check for local parameterization and dimensions consistency
     StateBlockPtr sb;
@@ -91,16 +84,16 @@ void Prior::check() const
         throw std::runtime_error("Prior::check() Prior " + type_ + " state is not valid (local param violation). " + print());
 
     // check state size
-    if (mode_ != "nothing" and state_.size() != sb->getSize())
+    if (state_.size() != sb->getSize())
         throw std::runtime_error("Prior::check() Prior " + type_ + " state size different of state size. " + print());
 
     // check sigma size
-    if (mode_ == "factor" and sigma_.size() != sb->getLocalSize())
+    if (mode_ == "factor" and sigma_std_.size() != sb->getLocalSize())
         throw std::runtime_error("Prior::check() Prior " + type_ + " sigma size different of state local size. " + print());
 
     // check sigma size
-    if (dynamic_ and sigma_drift_.size() != sb->getLocalSize())
-        throw std::runtime_error("Prior::check() Prior " + type_ + " sigma_drift size different of state local size. " + print());
+    if (dynamic_ and sigma_drift_std_.size() != sb->getLocalSize())
+        throw std::runtime_error("Prior::check() Prior " + type_ + " sigma_drift_std size different of state local size. " + print());
 }
 
 std::string Prior::print() const
@@ -108,9 +101,9 @@ std::string Prior::print() const
     return "Prior " + type_ + "\n"
         + "mode: " + mode_  + "\n"
         + "state: "      + to_string(state_)       + "\n"
-        + (mode_ == "factor" ? "sigma: "      + to_string(sigma_)       + "\n" : "")
+        + (mode_ == "factor" ? "sigma: "      + to_string(sigma_std_)       + "\n" : "")
         + "dynamic: "    + to_string(dynamic_)     + "\n"
-        + (dynamic_ ? "sigma_drift: "+ to_string(sigma_drift_) + "\n" : "");
+        + (dynamic_ ? "sigma_drift_std: "+ to_string(sigma_drift_std_) + "\n" : "");
 }
 
 } // namespace wolf
\ No newline at end of file
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index 6332a69928a558cfdede9125a73fd7ab7f835338..8c15d647982fe4cefaa98ecf07e68d860edb4984 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.cpp
@@ -162,7 +162,7 @@ TEST(FactoryStateBlock, creator_O_is_angle)
 
 TEST(FactoryStateBlock, creator_O_is_wrong_size)
 {
-    ASSERT_THROW(auto sba = FactoryStateBlock::create("O", Eigen::Vector2d(1,2), false) , std::length_error);
+    ASSERT_THROW(auto sba = FactoryStateBlock::create("O", Eigen::Vector2d(1,2), false) , std::runtime_error);
 }
 
 int main(int argc, char **argv)
diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp
index 43a507e274269f9ce19cbc07fa2c42c6a1670592..5121b7acfaf85988282654259a33904f459a6dae 100644
--- a/test/gtest_prior.cpp
+++ b/test/gtest_prior.cpp
@@ -57,22 +57,22 @@ void testPrior(const Prior& P,
 
   if (pstruct.mode == "factor")
   {
-    ASSERT_MATRIX_APPROX(pstruct.sigma, P.getSigma(), 1e-8);
+    ASSERT_MATRIX_APPROX(pstruct.sigma, P.getSigmaStd(), 1e-8);
   }
   else
   {
-    ASSERT_EQ(0,P.getSigma().size());
+    ASSERT_EQ(0,P.getSigmaStd().size());
   }
 
   ASSERT_EQ(pstruct.dynamic, P.isDynamic());
   
   if (pstruct.dynamic)
   {
-    ASSERT_MATRIX_APPROX(pstruct.sigma_drift, P.getSigmaDrift(), 1e-8);
+    ASSERT_MATRIX_APPROX(pstruct.sigma_drift, P.getSigmaDriftStd(), 1e-8);
   }
   else
   {
-    ASSERT_EQ(0,P.getSigmaDrift().size());
+    ASSERT_EQ(0,P.getSigmaDriftStd().size());
   }
 };
 
@@ -89,10 +89,10 @@ void testPriors(const std::vector<PriorAsStruct>& setups, bool should_work)
               "\n\tdynamic: ", setup.dynamic,
               "\n\tsigma_drift: ", setup.sigma_drift.transpose());
     if (should_work)
-      testPrior(Prior(setup.type, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup);
+      testPrior(Prior(setup.type, setup.state, setup.mode, setup.sigma, setup.dynamic, setup.sigma_drift), setup);
     else
     {
-      ASSERT_THROW(testPrior(Prior(setup.type, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup),std::runtime_error);
+      ASSERT_THROW(testPrior(Prior(setup.type, setup.state, setup.mode, setup.sigma, setup.dynamic, setup.sigma_drift), setup),std::runtime_error);
     }
   }
 }
@@ -101,17 +101,6 @@ TEST(Prior, P)
 {
   std::vector<PriorAsStruct> setups_ok, setups_death;
 
-  // Nothing - not dynamic (not allowed for P and O)
-  setups_ok   .push_back(PriorAsStruct({"P","nothing",vector2,vector0,false}));
-  setups_ok   .push_back(PriorAsStruct({"P","nothing",vector3,vector0,false}));
-  setups_death.push_back(PriorAsStruct({"P","nothing",vector4,vector0,false})); // wrong state size
-
-  // Nothing - dynamic
-  setups_ok   .push_back(PriorAsStruct({"P","nothing",vector2,vector0,true,vector2}));
-  setups_ok   .push_back(PriorAsStruct({"P","nothing",vector3,vector0,true,vector3}));
-  setups_death.push_back(PriorAsStruct({"P","nothing",vector4,vector0,true,vector4})); // wrong state size
-  setups_death.push_back(PriorAsStruct({"P","nothing",vector3,vector0,true,vector4})); // inconsistent size
-  
   // Initial guess - not dynamic
   setups_ok   .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,false}));
   setups_ok   .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,false}));
@@ -156,19 +145,6 @@ TEST(Prior, O)
 {
   std::vector<PriorAsStruct> setups_ok, setups_death;
 
-  // nothing - not dynamic
-  setups_ok   .push_back(PriorAsStruct({"O","nothing",vector1,vector0,false}));
-  setups_ok   .push_back(PriorAsStruct({"O","nothing",vectorq,vector0,false}));
-  setups_death.push_back(PriorAsStruct({"O","nothing",vector4,vector0,false})); // not normalized
-  setups_death.push_back(PriorAsStruct({"O","nothing",vector3,vector0,false})); // wrong size
-
-  // Nothing - dynamic
-  setups_ok   .push_back(PriorAsStruct({"O","nothing",vector1,vector0,true,vector1}));
-  setups_ok   .push_back(PriorAsStruct({"O","nothing",vectorq,vector0,true,vector3}));
-  setups_death.push_back(PriorAsStruct({"O","nothing",vector4,vector0,true,vector3})); // not normalized
-  setups_death.push_back(PriorAsStruct({"O","nothing",vector3,vector0,true,vector3})); // wrong size
-  setups_death.push_back(PriorAsStruct({"O","nothing",vectorq,vector0,true,vector4})); // inconsistent size
-  
   // Initial guess - not dynamic
   setups_ok   .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,false}));
   setups_ok   .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,false}));
@@ -220,19 +196,8 @@ TEST(Prior, StateBlock)
   std::vector<PriorAsStruct> setups_ok, setups_death;
 
   // Unknown type -> StateBlock
-  setups_ok   .push_back(PriorAsStruct({"K","nothing",vector1,vector0,false}));
-
-  // Nothing - not dynamic (not allowed for P and O)
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","nothing",vector2,vector0,false}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","nothing",vector3,vector0,false}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","nothing",vector4,vector0,false}));
-
-  // Nothing - dynamic
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","nothing",vector2,vector0,true,vector2}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","nothing",vector3,vector0,true,vector3}));
-  setups_ok   .push_back(PriorAsStruct({"StateBlock","nothing",vector4,vector0,true,vector4}));
-  setups_death.push_back(PriorAsStruct({"StateBlock","nothing",vector3,vector0,true,vector4})); // inconsistent size
-  
+  setups_ok   .push_back(PriorAsStruct({"K","initial_guess",vector1,vector0,false}));
+
   // Initial guess - not dynamic
   setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector2,vector0,false}));
   setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector3,vector0,false}));
@@ -276,15 +241,6 @@ TEST(Prior, StateAngle)
 {
   std::vector<PriorAsStruct> setups_ok, setups_death;
 
-  // nothing - not dynamic
-  setups_ok   .push_back(PriorAsStruct({"StateAngle","nothing",vector1,vector0,false}));
-  setups_death.push_back(PriorAsStruct({"StateAngle","nothing",vector3,vector0,false})); // wrong size
-
-  // Nothing - dynamic
-  setups_ok   .push_back(PriorAsStruct({"StateAngle","nothing",vector1,vector0,true,vector1}));
-  setups_death.push_back(PriorAsStruct({"StateAngle","nothing",vector3,vector0,true,vector3})); // wrong size
-  setups_death.push_back(PriorAsStruct({"StateAngle","nothing",vector1,vector0,true,vector4})); // inconsistent size
-  
   // Initial guess - not dynamic
   setups_ok   .push_back(PriorAsStruct({"StateAngle","initial_guess",vector1,vector0,false}));
   setups_death.push_back(PriorAsStruct({"StateAngle","initial_guess",vector3,vector0,false})); // wrong size
@@ -323,17 +279,6 @@ TEST(Prior, StateQuaternion)
 {
   std::vector<PriorAsStruct> setups_ok, setups_death;
 
-  // nothing - not dynamic
-  setups_ok   .push_back(PriorAsStruct({"StateQuaternion","nothing",vectorq,vector0,false}));
-  setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vector4,vector0,false})); // not normalized
-  setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vector3,vector0,false})); // wrong size
-
-  // Nothing - dynamic
-  setups_ok   .push_back(PriorAsStruct({"StateQuaternion","nothing",vectorq,vector0,true,vector3}));
-  setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vector4,vector0,true,vector3})); // not normalized
-  setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vector3,vector0,true,vector3})); // wrong size
-  setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vectorq,vector0,true,vector4})); // inconsistent size
-  
   // Initial guess - not dynamic
   setups_ok   .push_back(PriorAsStruct({"StateQuaternion","initial_guess",vectorq,vector0,false}));
   setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vector4,vector0,false})); // not normalized
@@ -381,7 +326,7 @@ TEST(Prior, ParamsServer)
 
   std::vector<std::string> keys({"P","O"});
   std::vector<int> dims({2,3});
-  std::vector<std::string> modes({"nothing","initial_guess","fix","factor"});
+  std::vector<std::string> modes({"initial_guess","fix","factor"});
   std::vector<bool> dynamics({false,true});
 
   VectorXd p_state(4), o_state(4), po_sigma(4);
@@ -412,17 +357,15 @@ TEST(Prior, ParamsServer)
             sigma = po_sigma.head(dim == 2 ? 1 : 3);
           }
 
-          if (mode != "nothing")
-          {
-            ASSERT_MATRIX_APPROX(P.getState(),state,wolf::Constants::EPS);
-          }
+          ASSERT_MATRIX_APPROX(P.getState(),state,wolf::Constants::EPS);
+          
           if (dynamic)
           {
-            ASSERT_MATRIX_APPROX(P.getSigmaDrift(),sigma,wolf::Constants::EPS);
+            ASSERT_MATRIX_APPROX(P.getSigmaDriftStd(),sigma,wolf::Constants::EPS);
           }
           if (mode == "factor")
           {
-            ASSERT_MATRIX_APPROX(P.getSigma(),sigma,wolf::Constants::EPS);
+            ASSERT_MATRIX_APPROX(P.getSigmaStd(),sigma,wolf::Constants::EPS);
           }
         }
 
@@ -435,17 +378,14 @@ TEST(Prior, ParamsServer)
       auto P = Prior(prefix, "StateBlock", server);
       ASSERT_EQ(P.getMode(), mode);
       ASSERT_EQ(P.isDynamic(), dynamic);
-      if (mode != "nothing")
-      {
-        ASSERT_MATRIX_APPROX(P.getState(),p_state,wolf::Constants::EPS);
-      }
+      ASSERT_MATRIX_APPROX(P.getState(),p_state,wolf::Constants::EPS);
       if (dynamic)
       {
-        ASSERT_MATRIX_APPROX(P.getSigmaDrift(),po_sigma,wolf::Constants::EPS);
+        ASSERT_MATRIX_APPROX(P.getSigmaDriftStd(),po_sigma,wolf::Constants::EPS);
       }
       if (mode == "factor")
       {
-        ASSERT_MATRIX_APPROX(P.getSigma(),po_sigma,wolf::Constants::EPS);
+        ASSERT_MATRIX_APPROX(P.getSigmaStd(),po_sigma,wolf::Constants::EPS);
       }
     }
 }
@@ -457,7 +397,7 @@ TEST(Prior, ParamsServerWrong)
 
   std::vector<std::string> keys({"P","O"});
   std::vector<int> dims({2,3});
-  std::vector<std::string> modes({"nothing","initial_guess","fix","factor"});
+  std::vector<std::string> modes({"initial_guess","fix","factor"});
   std::vector<bool> dynamics({false,true});
 
   VectorXd p_state(4), o_state(4), po_sigma(4);
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 0a367cd3047d706bbf46ce4697a4ea9c67fbd388..f2ef98beed7eeae2c9928bc0abcdd982df69e435 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -31,24 +31,188 @@
 #include "core/utils/utils_gtest.h"
 
 using namespace wolf;
+using namespace Eigen;
 
-TEST(SensorBase, POfix)
+VectorXd p_state_2D = VectorXd::Random(2);
+VectorXd p_state_3D = VectorXd::Random(3);
+VectorXd o_state_2D = VectorXd::Random(1);
+VectorXd o_state_3D = VectorXd::Random(4).normalized();
+VectorXd p_sigma_2D = VectorXd::Random(2).cwiseAbs();
+VectorXd p_sigma_3D = VectorXd::Random(3).cwiseAbs();
+VectorXd o_sigma_2D = VectorXd::Random(1).cwiseAbs();
+VectorXd o_sigma_3D = VectorXd::Random(3).cwiseAbs();
+
+VectorXd noise_std = Vector2d::Constant(0.1);
+MatrixXd noise_cov = noise_std.cwiseAbs2().asDiagonal();
+
+
+// CONSTRUCTOR WITH PRIORS AND PARAMS
+// 2D Fix
+TEST(SensorBase, POfix2D)
+{
+  auto params = std::make_shared<ParamsSensorBase>();
+  params->noise_std = noise_std;
+  
+  auto S = std::make_shared<SensorBase>("SensorBase",
+                                        "sensor1",
+                                        2,
+                                        Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
+                                                {'O',Prior("O", o_state_2D)}}),
+                                        params);
+
+  ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
+  ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
+  ASSERT_MATRIX_APPROX(p_state_2D, S->getP()->getState(), 1e-8);
+  ASSERT_MATRIX_APPROX(o_state_2D, S->getO()->getState(), 1e-8);
+  ASSERT_TRUE(S->getP()->isFixed());
+  ASSERT_TRUE(S->getO()->isFixed());
+}
+
+// 3D Fix
+TEST(SensorBase, POfix3D)
+{
+  auto params = std::make_shared<ParamsSensorBase>();
+  params->noise_std = noise_std;
+  
+  auto S = std::make_shared<SensorBase>("SensorBase",
+                                        "sensor1",
+                                        3,
+                                        Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
+                                                {'O',Prior("O", o_state_3D)}}),
+                                        params);
+
+  ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
+  ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
+  ASSERT_MATRIX_APPROX(p_state_3D, S->getP()->getState(), 1e-8);
+  ASSERT_MATRIX_APPROX(o_state_3D, S->getO()->getState(), 1e-8);
+  ASSERT_TRUE(S->getP()->isFixed());
+  ASSERT_TRUE(S->getO()->isFixed());
+}
+
+// 2D Initial guess
+TEST(SensorBase, POinitial_guess2D)
+{
+  auto params = std::make_shared<ParamsSensorBase>();
+  params->noise_std = noise_std;
+  
+  auto S = std::make_shared<SensorBase>("SensorBase",
+                                        "sensor1",
+                                        2,
+                                        Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
+                                                {'O',Prior("O", o_state_2D, "initial_guess")}}),
+                                        params);
+
+  ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
+  ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
+  ASSERT_MATRIX_APPROX(p_state_2D, S->getP()->getState(), 1e-8);
+  ASSERT_MATRIX_APPROX(o_state_2D, S->getO()->getState(), 1e-8);
+  ASSERT_FALSE(S->getP()->isFixed());
+  ASSERT_FALSE(S->getO()->isFixed());
+}
+
+// 3D Initial guess
+TEST(SensorBase, POinitial_guess3D)
+{
+  auto params = std::make_shared<ParamsSensorBase>();
+  params->noise_std = noise_std;
+  
+  auto S = std::make_shared<SensorBase>("SensorBase",
+                                        "sensor1",
+                                        3,
+                                        Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
+                                                {'O',Prior("O", o_state_3D, "initial_guess")}}),
+                                        params);
+
+  ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
+  ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
+  ASSERT_MATRIX_APPROX(p_state_3D, S->getP()->getState(), 1e-8);
+  ASSERT_MATRIX_APPROX(o_state_3D, S->getO()->getState(), 1e-8);
+  ASSERT_FALSE(S->getP()->isFixed());
+  ASSERT_FALSE(S->getO()->isFixed());
+}
+
+// 2D Factor
+TEST(SensorBase, POfactor2D)
 {
-  auto noise_std = Eigen::Vector2d::Constant(0.1);
-  Eigen::MatrixXd noise_cov = noise_std.cwiseAbs2().asDiagonal();
+  auto params = std::make_shared<ParamsSensorBase>();
+  params->noise_std = noise_std;
+  
+  auto S = std::make_shared<SensorBase>("SensorBase",
+                                        "sensor1",
+                                        2,
+                                        Priors({{'P',Prior("P", p_state_2D, "factor", p_sigma_2D)},
+                                                {'O',Prior("O", o_state_2D, "factor", o_sigma_2D)}}),
+                                        params);
+
+  ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
+  ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
+  ASSERT_MATRIX_APPROX(p_state_2D, S->getP()->getState(), 1e-8);
+  ASSERT_MATRIX_APPROX(o_state_2D, S->getO()->getState(), 1e-8);
+  ASSERT_FALSE(S->getP()->isFixed());
+  ASSERT_FALSE(S->getO()->isFixed());
+}
 
+// 3D Factor
+TEST(SensorBase, POfactor3D)
+{
   auto params = std::make_shared<ParamsSensorBase>();
-  params->noise_std = Eigen::Vector2d::Constant(0.1);
+  params->noise_std = noise_std;
+  
+  auto S = std::make_shared<SensorBase>("SensorBase",
+                                        "sensor1",
+                                        3,
+                                        Priors({{'P',Prior("P", p_state_3D, "factor", p_sigma_3D)},
+                                                {'O',Prior("O", o_state_3D, "factor", o_sigma_3D)}}),
+                                        params);
+
+  ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
+  ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
+  ASSERT_MATRIX_APPROX(p_state_3D, S->getP()->getState(), 1e-8);
+  ASSERT_MATRIX_APPROX(o_state_3D, S->getO()->getState(), 1e-8);
+  ASSERT_FALSE(S->getP()->isFixed());
+  ASSERT_FALSE(S->getO()->isFixed());
+}
+
+// 2D Initial guess dynamic
+TEST(SensorBase, POinitial_guess_dynamic2D)
+{
+  auto params = std::make_shared<ParamsSensorBase>();
+  params->noise_std = noise_std;
   
   auto S = std::make_shared<SensorBase>("SensorBase",
                                         "sensor1",
                                         2,
-                                        Priors({{'P',Prior("P","fix", Eigen::Vector2d::Zero())},
-                                                {'O',Prior("O","fix", Eigen::Vector1d::Zero())}}),
+                                        Priors({{'P',Prior("P", p_state_2D, "initial_guess", p_sigma_2D, true, p_sigma_2D)},
+                                                {'O',Prior("O", o_state_2D, "initial_guess", o_sigma_2D, true, o_sigma_2D)}}),
+                                        params);
+
+  ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
+  ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
+  ASSERT_MATRIX_APPROX(p_state_2D, S->getP()->getState(), 1e-8);
+  ASSERT_MATRIX_APPROX(o_state_2D, S->getO()->getState(), 1e-8);
+  ASSERT_FALSE(S->getP()->isFixed());
+  ASSERT_FALSE(S->getO()->isFixed());
+}
+
+// 3D Initial guess dynamic
+TEST(SensorBase, POinitial_guess_dynamic3D)
+{
+  auto params = std::make_shared<ParamsSensorBase>();
+  params->noise_std = noise_std;
+  
+  auto S = std::make_shared<SensorBase>("SensorBase",
+                                        "sensor1",
+                                        3,
+                                        Priors({{'P',Prior("P", p_state_3D, "initial_guess", p_sigma_3D, true, p_sigma_3D)},
+                                                {'O',Prior("O", o_state_3D, "initial_guess", o_sigma_3D, true, o_sigma_3D)}}),
                                         params);
 
   ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
   ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
+  ASSERT_MATRIX_APPROX(p_state_3D, S->getP()->getState(), 1e-8);
+  ASSERT_MATRIX_APPROX(o_state_3D, S->getO()->getState(), 1e-8);
+  ASSERT_FALSE(S->getP()->isFixed());
+  ASSERT_FALSE(S->getO()->isFixed());
 }
 
 int main(int argc, char **argv)
diff --git a/test/yaml/params_prior.yaml b/test/yaml/params_prior.yaml
index 1b1955cdd541043243419ea56ed0e4be17201404..17b77f93c4b8bc8aacc91db7f5ff2ed148bbbece 100644
--- a/test/yaml/params_prior.yaml
+++ b/test/yaml/params_prior.yaml
@@ -3,10 +3,6 @@ config:
     dim: 2
   
   # P in 2D
-  P2_nothing:
-    mode: nothing
-    dynamic: false
-  
   P2_initial_guess:
     mode: initial_guess
     state: [1, 2]
@@ -22,11 +18,6 @@ config:
     state: [1, 2]
     sigma: [0.1, 0.2]
     dynamic: false
-    
-  P2_nothing_dynamic:
-    mode: nothing
-    dynamic: true
-    sigma_drift: [0.1, 0.2]
   
   P2_initial_guess_dynamic:
     mode: initial_guess
@@ -48,10 +39,6 @@ config:
     sigma_drift: [0.1, 0.2]
 
   # P in 3D
-  P3_nothing:
-    mode: nothing
-    dynamic: false
-  
   P3_initial_guess:
     mode: initial_guess
     state: [1, 2, 3]
@@ -67,11 +54,6 @@ config:
     state: [1, 2, 3]
     sigma: [0.1, 0.2, 0.3]
     dynamic: false
-    
-  P3_nothing_dynamic:
-    mode: nothing
-    dynamic: true
-    sigma_drift: [0.1, 0.2, 0.3]
   
   P3_initial_guess_dynamic:
     mode: initial_guess
@@ -93,10 +75,6 @@ config:
     sigma_drift: [0.1, 0.2, 0.3]
 
   # O in 2D
-  O2_nothing:
-    mode: nothing
-    dynamic: false
-  
   O2_initial_guess:
     mode: initial_guess
     state: [1]
@@ -112,11 +90,6 @@ config:
     state: [1]
     sigma: [0.1]
     dynamic: false
-    
-  O2_nothing_dynamic:
-    mode: nothing
-    dynamic: true
-    sigma_drift: [0.1]
   
   O2_initial_guess_dynamic:
     mode: initial_guess
@@ -138,10 +111,6 @@ config:
     sigma_drift: [0.1]
 
   # O in 3D
-  O3_nothing:
-    mode: nothing
-    dynamic: false
-  
   O3_initial_guess:
     mode: initial_guess
     state: [1, 0, 0, 0]
@@ -157,11 +126,6 @@ config:
     state: [1, 0, 0, 0]
     sigma: [0.1, 0.2, 0.3]
     dynamic: false
-    
-  O3_nothing_dynamic:
-    mode: nothing
-    dynamic: true
-    sigma_drift: [0.1, 0.2, 0.3]
   
   O3_initial_guess_dynamic:
     mode: initial_guess
@@ -183,10 +147,6 @@ config:
     sigma_drift: [0.1, 0.2, 0.3]
 
   # I
-  I_nothing:
-    mode: nothing
-    dynamic: false
-  
   I_initial_guess:
     mode: initial_guess
     state: [1, 2, 3, 4]
@@ -202,11 +162,6 @@ config:
     state: [1, 2, 3, 4]
     sigma: [0.1, 0.2, 0.3, 0.4]
     dynamic: false
-    
-  I_nothing_dynamic:
-    mode: nothing
-    dynamic: true
-    sigma_drift: [0.1, 0.2, 0.3, 0.4]
   
   I_initial_guess_dynamic:
     mode: initial_guess
diff --git a/test/yaml/params_prior_wrong.yaml b/test/yaml/params_prior_wrong.yaml
index 5a40d4743123e8e777016d86cbcb7aa178a0ed66..1d83bad82680c0deb719c1c4948bbbbb73449a44 100644
--- a/test/yaml/params_prior_wrong.yaml
+++ b/test/yaml/params_prior_wrong.yaml
@@ -3,10 +3,6 @@ config:
     dim: 2
   
   # P in 2D
-  P2_nothing:
-    mode: no  # wrong mode
-    dynamic: false
-  
   P2_initial_guess:
     mode: initial_guess
     #state: [1, 2] # missing
@@ -22,11 +18,6 @@ config:
     state: [1, 2]
     sigma: [0.1, 0.2, 0.3] # wrong size
     dynamic: false
-    
-  P2_nothing_dynamic:
-    mode: nothing
-    dynamic: true
-    #sigma_drift: [0.1, 0.2] # missing
   
   P2_initial_guess_dynamic:
     mode: initial_guess
@@ -48,10 +39,6 @@ config:
     sigma_drift: [0.1, 0.2]
 
   # P in 3D
-  P3_nothing:
-    mode: nothing
-    #dynamic: false # missing
-  
   P3_initial_guess:
     #mode: initial_guess # missing
     state: [1, 2, 3]
@@ -67,11 +54,6 @@ config:
     state: [1, 2, 3]
     sigma: [0.1, 0.2] # wrong size
     dynamic: false
-    
-  P3_nothing_dynamic:
-    mode: no  # wrong mode
-    dynamic: true
-    sigma_drift: [0.1, 0.2, 0.3]
   
   P3_initial_guess_dynamic:
     mode: initial_guess
@@ -93,10 +75,6 @@ config:
     sigma_drift: [0.1, 0.2, 0.3]
 
   # O in 2D
-  O2_nothing:
-    mode: no # wrong mode
-    dynamic: false
-  
   O2_initial_guess:
     mode: initial_guess
     state: [1, 2]  # wrong size
@@ -112,11 +90,6 @@ config:
     state: [1]
     sigma: [0.1, 0.2] # wrong size
     dynamic: false
-    
-  O2_nothing_dynamic:
-    mode: nothing
-    dynamic: true
-    sigma_drift: [0.1 0.2] # wrong size
   
   O2_initial_guess_dynamic:
     mode: initial_guess
@@ -138,10 +111,6 @@ config:
     sigma_drift: [0.1]
 
   # O in 3D
-  O3_nothing:
-    mode: no # wrong mode
-    dynamic: false
-  
   O3_initial_guess:
     mode: initial_guess
     #state: [1, 0, 0, 0] # missing
@@ -157,11 +126,6 @@ config:
     state: [1, 0, 0, 0]
     sigma: [0.1, 0.2]  # wrong size
     dynamic: false
-    
-  O3_nothing_dynamic:
-    mode: nothing
-    #dynamic: true # missing
-    sigma_drift: [0.1, 0.2, 0.3]
   
   O3_initial_guess_dynamic:
     mode: initial_guess
@@ -183,18 +147,14 @@ config:
     sigma_drift: [0.1, 0.2] # wrong size
 
   # I
-  I_nothing:
-    mode: nothing
-    #dynamic: false # missing
-  
   I_initial_guess:
     #mode: initial_guess # missing
     state: [1, 2, 3, 4]
     dynamic: false
   
   I_fix:
-    #mode: fix # missing
-    state: [1, 2, 3, 4]
+    mode: fix
+    #state: [1, 2, 3, 4] # missing
     dynamic: false
 
   I_factor:
@@ -202,11 +162,6 @@ config:
     state: [1, 2, 3, 4]
     sigma: [0.1, 0.2, 0.3]  # wrong size
     dynamic: false
-    
-  I_nothing_dynamic:
-    mode: nothing
-    dynamic: true
-    #sigma_drift: [0.1, 0.2, 0.3, 0.4] # missing
   
   I_initial_guess_dynamic:
     mode: initial_guess