diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 931db3689e731d433bf1686b04c00e66bcddbd66..f57725decd14bc50d4bec57936a8d2de5bf8c316 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -104,7 +104,7 @@ struct ParamsSensorBase: public ParamsBase
 
     std::string print() const override
     {
-        return "noise_std: ";// + to_string(noise_std) + "\n";
+        return "noise_std: " + to_string(noise_std) + "\n";
     }
 };
 
@@ -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::string _keys);
+                   std::unordered_map<char, std::string> _keys_types);
 
         ~SensorBase() override;
 
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index 3cc093665df69b32ed10d2d49921fdacedea7871..1a307ce72310bcee0bf7d5b21b833aec8caf9340 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -58,7 +58,9 @@ inline StateAngle::~StateAngle()
 
 inline StateBlockPtr StateAngle::create (const Eigen::VectorXd& _state, bool _fixed)
 {
-    MatrixSizeCheck<1,1>::check(_state);
+    if(_state.size() != 1)
+        throw std::runtime_error("The angle state vector must be of size 1!");
+    
     return std::make_shared<StateAngle>(_state(0), _fixed);
 }
 
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index a1317fd2936dedf39d79301526fa3d0829b509a5..d5278e1a6f4e3580c22056db7cc2c9ef073c21fb 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -61,7 +61,8 @@ inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, b
 inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed) :
         StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
 {
-    assert(state_.size() == 4 && "The quaternion state vector must be of size 4");
+    if(state_.size() != 4)
+        throw std::runtime_error("The quaternion state vector must be of size 4!");
 
     // TODO: remove these lines after issue #381 is addressed
     if(not isValid(1e-5))
@@ -95,7 +96,6 @@ inline Eigen::VectorXd StateQuaternion::identity() const
 
 inline StateBlockPtr StateQuaternion::create (const Eigen::VectorXd& _state, bool _fixed)
 {
-    MatrixSizeCheck<4,1>::check(_state);
     return std::make_shared<StateQuaternion>(_state, _fixed);
 }
 
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 64860f706a2e9f709230758f26bee39eec093ffc..dc92cbe1d59a68c106d041fa0049e07f65a08903 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -99,30 +99,31 @@ void SensorBase::loadPriors(const Priors& _priors, SizeEigen _dim)
 
     for (auto&& prior_pair : _priors)
     {
+        auto key = prior_pair.first;
         const Prior& prior = prior_pair.second;
 
         // Checks
-        if (prior.getKey() == 'P' and prior.getState().size() != _dim)
+        if (key == 'P' and prior.getState().size() != _dim)
             throw std::runtime_error("Prior state for P has wrong size");
-        if (prior.getKey() == '0' and prior.getState().size() != (_dim == 2 ? 1 : 4))
+        if (key == '0' and prior.getState().size() != (_dim == 2 ? 1 : 4))
             throw std::runtime_error("Prior state for O has wrong size");
 
         // create state block
-        auto sb = FactoryStateBlock::create(std::string(1, prior.getKey()), prior.getState(), prior.isFixed());
+        auto sb = FactoryStateBlock::create(prior.getType(), prior.getState(), prior.isFixed());
 
         // check local param
         if (not sb->isValid())
         {
-            WOLF_ERROR("The created stateblock ", std::string(1, prior.getKey()), " is not valid (local param violation)");
+            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");
         }
 
         // Add state block
-        addStateBlock(prior.getKey(), sb, prior.isDynamic());
+        addStateBlock(key, sb, prior.isDynamic());
 
         // Factor
         if (prior.isFactor())
-            addPriorParameter(prior.getKey(), prior.getState(), prior.getSigma());
+            addPriorParameter(key, prior.getState(), prior.getSigma());
     }
 }
 
diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp
index 1d7fdbbdb8b8b29dcdb752e2b2c7d07d61429209..172d49aaa05c816cbf19bacac25c8a372118fbd7 100644
--- a/src/state_block/prior.cpp
+++ b/src/state_block/prior.cpp
@@ -42,10 +42,10 @@ Prior::Prior(const std::string& _type,
   check();
 }
 
-Prior::Prior(const std::string& _prefix, const ParamsServer& _server, const std::string& _type)
+Prior::Prior(const std::string& _prefix, const std::string& _type, const ParamsServer& _server)
 {
   type_ = _type;
-  mode_   = _server.getParam<double>(_prefix + "/mode");
+  mode_   = _server.getParam<std::string>(_prefix + "/mode");
 
   if (mode_ != "nothing")
     state_ = _server.getParam<Eigen::VectorXd>(_prefix + "/state");
@@ -70,42 +70,47 @@ Prior::Prior(const std::string& _prefix, const ParamsServer& _server, const std:
 void Prior::check() const
 {
     if (mode_ != "nothing" and mode_ != "initial_guess" and mode_ != "fix" and mode_ != "factor") 
-        throw std::runtime_error("wrong mode value: " + mode_ + ", it should be: 'nothing', 'initial_guess', 'fix' or '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 (type_ == "P" or type_ == "O"))
-        throw std::runtime_error("For P and O keys, mode 'nothing' is not valid");
-
-    if (dynamic_ and (type_ == "P" or type_ == "O") )
-        throw std::runtime_error("Dynamic state blocks not implemented for extrinsics");
+    if (mode_ == "nothing" and state_.size() == 0)
+      return;
 
     // try to create a state block and check for local parameterization and dimensions consistency
-    auto sb = FactoryStateBlock::create(type_, state_, mode_ == "fix");
+    StateBlockPtr sb;
+    try
+    {
+      sb = FactoryStateBlock::create(type_, state_, mode_ == "fix");
+    }
+    catch(const std::exception& e)
+    {
+      throw std::runtime_error("Prior::check() could not create state block from prior with error: " + std::string(e.what()) + print());
+    }
 
     // check local param
     if (not sb->isValid())
-        throw std::runtime_error("Prior " + type_ + " state is not valid (local param violation)");
+        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())
-        throw std::runtime_error("Prior " + type_ + " state size different of state size");
+        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())
-        throw std::runtime_error("Prior " + type_ + " sigma size different of state local size");
+        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 " + type_ + " sigma_drift size different of state local size");
+        throw std::runtime_error("Prior::check() Prior " + type_ + " sigma_drift size different of state local size. " + print());
 }
 
 std::string Prior::print() const
 {
     return "Prior " + type_ + "\n"
         + "mode: " + mode_  + "\n"
-        //+ "state: "      + to_string(state_)       + "\n"
-        //+ (mode_ == "factor" ? "sigma: "      + to_string(sigma_)       + "\n" : "")
-        + "dynamic: "    + to_string(dynamic_)     + "\n";
-        //+ (dynamic_ ? "sigma_drift: "+ to_string(sigma_drift_) + "\n" : "");
+        + "state: "      + to_string(state_)       + "\n"
+        + (mode_ == "factor" ? "sigma: "      + to_string(sigma_)       + "\n" : "")
+        + "dynamic: "    + to_string(dynamic_)     + "\n"
+        + (dynamic_ ? "sigma_drift: "+ to_string(sigma_drift_) + "\n" : "");
 }
 
 } // namespace wolf
\ No newline at end of file
diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp
index 974600d199321f38b815a3b0c0c1ca206bd8141f..43a507e274269f9ce19cbc07fa2c42c6a1670592 100644
--- a/test/gtest_prior.cpp
+++ b/test/gtest_prior.cpp
@@ -23,13 +23,24 @@
 #include "core/utils/utils_gtest.h"
 #include "core/common/wolf.h"
 #include "core/state_block/prior.h"
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
 
 using namespace wolf;
 using namespace Eigen;
 
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+auto vector0 = VectorXd(0);
+auto vector1 = VectorXd::Random(1);
+auto vector2 = VectorXd::Random(2);
+auto vector3 = VectorXd::Random(3);
+auto vector4 = VectorXd::Random(4);
+auto vectorq = VectorXd::Random(4).normalized();
+
 struct PriorAsStruct
 {
-  char key;
+  std::string type;
   std::string mode;
   VectorXd state; 
   VectorXd sigma; 
@@ -40,7 +51,7 @@ struct PriorAsStruct
 void testPrior(const Prior& P,
                const PriorAsStruct& pstruct)
 {
-  ASSERT_EQ(pstruct.key, P.getKey());
+  ASSERT_EQ(pstruct.type, P.getType());
   ASSERT_EQ(pstruct.mode, P.getMode());
   ASSERT_MATRIX_APPROX(pstruct.state, P.getState(), 1e-8);
 
@@ -70,18 +81,18 @@ void testPriors(const std::vector<PriorAsStruct>& setups, bool should_work)
   WOLF_INFO("Testing setups that should ", (should_work ? "" : "NOT"), " work..."); 
   for (auto setup : setups)
   {
-    WOLF_DEBUG("prior setup: ", 
-              "\n\tkey: ", std::string(1,setup.key),
+    WOLF_INFO("prior setup: ", 
+              "\n\ttype: ", setup.type,
               "\n\tmode: ", setup.mode,
               "\n\tstate: ", setup.state.transpose(),
               "\n\tsigma: ", setup.sigma.transpose(),
               "\n\tdynamic: ", setup.dynamic,
               "\n\tsigma_drift: ", setup.sigma_drift.transpose());
     if (should_work)
-      testPrior(Prior(setup.key, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup);
+      testPrior(Prior(setup.type, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup);
     else
     {
-      ASSERT_THROW(testPrior(Prior(setup.key, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup),std::runtime_error);
+      ASSERT_THROW(testPrior(Prior(setup.type, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup),std::runtime_error);
     }
   }
 }
@@ -90,41 +101,51 @@ TEST(Prior, P)
 {
   std::vector<PriorAsStruct> setups_ok, setups_death;
 
-  // SHOULD FAIL: Nothing - not dynamic (not allowed for P and O)
-  setups_death.push_back(PriorAsStruct({'P',"nothing",VectorXd::Random(2),VectorXd::Random(0),false}));
-  setups_death.push_back(PriorAsStruct({'P',"nothing",VectorXd::Random(3),VectorXd::Random(0),false}));
+  // 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
 
-  // SHOULD FAIL: Nothing - dynamic (not implemented)
-  setups_death.push_back(PriorAsStruct({'P',"nothing",VectorXd::Random(2),VectorXd::Random(0),true}));
-  setups_death.push_back(PriorAsStruct({'P',"nothing",VectorXd::Random(3),VectorXd::Random(0),true}));
+  // 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",VectorXd::Random(2),VectorXd::Random(0),false}));
-  setups_ok   .push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),false}));
-  setups_death.push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(4),VectorXd::Random(0),false})); // wrong size
+  setups_ok   .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,false}));
+  setups_death.push_back(PriorAsStruct({"P","initial_guess",vector4,vector0,false})); // wrong state size
 
-  // SHOULD FAIL: Initial guess - dynamic (not implemented)
-  setups_death.push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(2),VectorXd::Random(0),true}));
-  setups_death.push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),true}));
+  // Initial guess - dynamic
+  setups_ok   .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,true,vector2}));
+  setups_ok   .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,true,vector3}));
+  setups_death.push_back(PriorAsStruct({"P","initial_guess",vector4,vector0,true,vector4})); // wrong state size
+  setups_death.push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,true,vector4})); // inconsistent size
   
   // Fix - not dynamic
-  setups_ok   .push_back(PriorAsStruct({'P',"fix",VectorXd::Random(2),VectorXd::Random(0),false}));
-  setups_ok   .push_back(PriorAsStruct({'P',"fix",VectorXd::Random(3),VectorXd::Random(0),false}));
-  setups_death.push_back(PriorAsStruct({'P',"fix",VectorXd::Random(4),VectorXd::Random(0),false})); // wrong size
-
-  // SHOULD FAIL: Fix - dynamic (not implemented)
-  setups_death.push_back(PriorAsStruct({'P',"fix",VectorXd::Random(2),VectorXd::Random(0),true}));
-  setups_death.push_back(PriorAsStruct({'P',"fix",VectorXd::Random(3),VectorXd::Random(0),true}));
+  setups_ok   .push_back(PriorAsStruct({"P","fix",vector2,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"P","fix",vector3,vector0,false}));
+  setups_death.push_back(PriorAsStruct({"P","fix",vector4,vector0,false})); // wrong size
 
+  // Fix - dynamic
+  setups_ok   .push_back(PriorAsStruct({"P","fix",vector2,vector0,true,vector2}));
+  setups_ok   .push_back(PriorAsStruct({"P","fix",vector3,vector0,true,vector3}));
+  setups_death.push_back(PriorAsStruct({"P","fix",vector4,vector0,true,vector4})); // wrong state size
+  setups_death.push_back(PriorAsStruct({"P","fix",vector3,vector0,true,vector4})); // inconsistent size
+  
   // Factor - not dynamic
-  setups_ok   .push_back(PriorAsStruct({'P',"factor",VectorXd::Random(2),VectorXd::Random(2),false}));
-  setups_ok   .push_back(PriorAsStruct({'P',"factor",VectorXd::Random(3),VectorXd::Random(3),false}));
-  setups_death.push_back(PriorAsStruct({'P',"factor",VectorXd::Random(4),VectorXd::Random(4),false})); // wrong size
-  setups_death.push_back(PriorAsStruct({'P',"factor",VectorXd::Random(2),VectorXd::Random(3),false})); // inconsistent size
+  setups_ok   .push_back(PriorAsStruct({"P","factor",vector2,vector2,false}));
+  setups_ok   .push_back(PriorAsStruct({"P","factor",vector3,vector3,false}));
+  setups_death.push_back(PriorAsStruct({"P","factor",vector4,vector4,false})); // wrong state size
+  setups_death.push_back(PriorAsStruct({"P","factor",vector2,vector3,false})); // inconsistent size
 
-  // SHOULD FAIL: Factor - dynamic (not implemented)
-  setups_death.push_back(PriorAsStruct({'P',"factor",VectorXd::Random(2),VectorXd::Random(2),true}));
-  setups_death.push_back(PriorAsStruct({'P',"factor",VectorXd::Random(3),VectorXd::Random(3),true}));
+  // Factor - dynamic
+  setups_ok   .push_back(PriorAsStruct({"P","factor",vector2,vector2,true,vector2}));
+  setups_ok   .push_back(PriorAsStruct({"P","factor",vector3,vector3,true,vector3}));
+  setups_death.push_back(PriorAsStruct({"P","factor",vector4,vector4,true,vector4})); // wrong state size
+  setups_death.push_back(PriorAsStruct({"P","factor",vector3,vector3,true,vector4})); // inconsistent size
+  setups_death.push_back(PriorAsStruct({"P","factor",vector3,vector4,true,vector3})); // inconsistent size
 
   // TEST SETUPS
   testPriors(setups_ok, true);
@@ -135,103 +156,336 @@ TEST(Prior, O)
 {
   std::vector<PriorAsStruct> setups_ok, setups_death;
 
-  // SHOULD FAIL: Nothing - not dynamic (not allowed for P and O)
-  setups_death.push_back(PriorAsStruct({'O',"nothing",VectorXd::Random(1),VectorXd::Random(0),false}));
-  setups_death.push_back(PriorAsStruct({'O',"nothing",VectorXd::Random(4).normalized(),VectorXd::Random(0),false}));
+  // 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
 
-  // SHOULD FAIL: Nothing - dynamic (not implemented)
-  setups_death.push_back(PriorAsStruct({'O',"nothing",VectorXd::Random(1),VectorXd::Random(0),true}));
-  setups_death.push_back(PriorAsStruct({'O',"nothing",VectorXd::Random(4).normalized(),VectorXd::Random(0),true}));
+  // 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",VectorXd::Random(1),VectorXd::Random(0),false}));
-  setups_ok   .push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(4).normalized(),VectorXd::Random(0),false}));
-  setups_death.push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),false})); // wrong size
-  setups_death.push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(4),VectorXd::Random(0),false})); // not normalized
-
-  // SHOULD FAIL: Initial guess - dynamic (not implemented)
-  setups_death.push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(1),VectorXd::Random(0),true}));
-  setups_death.push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(4).normalized(),VectorXd::Random(0),true}));
+  setups_ok   .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,false}));
+  setups_death.push_back(PriorAsStruct({"O","initial_guess",vector4,vector0,false})); // not normalized
+  setups_death.push_back(PriorAsStruct({"O","initial_guess",vector3,vector0,false})); // wrong size
+
+  // Initial guess - dynamic
+  setups_ok   .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,true,vector1}));
+  setups_ok   .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,true,vector3}));
+  setups_death.push_back(PriorAsStruct({"O","initial_guess",vector4,vector0,true,vector3})); // not normalized
+  setups_death.push_back(PriorAsStruct({"O","initial_guess",vector3,vector0,true,vector3})); // wrong size
+  setups_death.push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,true,vector4})); // inconsistent size
   
   // Fix - not dynamic
-  setups_ok   .push_back(PriorAsStruct({'O',"fix",VectorXd::Random(1),VectorXd::Random(0),false}));
-  setups_ok   .push_back(PriorAsStruct({'O',"fix",VectorXd::Random(4).normalized(),VectorXd::Random(0),false}));
-  setups_death.push_back(PriorAsStruct({'O',"fix",VectorXd::Random(3),VectorXd::Random(0),false})); // wrong size
-  setups_death.push_back(PriorAsStruct({'O',"fix",VectorXd::Random(4),VectorXd::Random(0),false})); // not normalized
+  setups_ok   .push_back(PriorAsStruct({"O","fix",vector1,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"O","fix",vectorq,vector0,false}));
+  setups_death.push_back(PriorAsStruct({"O","fix",vector3,vector0,false})); // wrong size
+  setups_death.push_back(PriorAsStruct({"O","fix",vector4,vector0,false})); // not normalized
 
-  // SHOULD FAIL: Fix - dynamic (not implemented)
-  setups_death.push_back(PriorAsStruct({'O',"fix",VectorXd::Random(1),VectorXd::Random(0),true}));
-  setups_death.push_back(PriorAsStruct({'O',"fix",VectorXd::Random(4).normalized(),VectorXd::Random(0),true}));
+  // Fix - dynamic
+  setups_ok   .push_back(PriorAsStruct({"O","fix",vector1,vector0,true,vector1}));
+  setups_ok   .push_back(PriorAsStruct({"O","fix",vectorq,vector0,true,vector3}));
+  setups_death.push_back(PriorAsStruct({"O","fix",vector4,vector0,true,vector3})); // not normalized
+  setups_death.push_back(PriorAsStruct({"O","fix",vector3,vector0,true,vector3})); // wrong size
+  setups_death.push_back(PriorAsStruct({"O","fix",vectorq,vector0,true,vector4})); // inconsistent size
 
   // Factor - not dynamic
-  setups_ok   .push_back(PriorAsStruct({'O',"factor",VectorXd::Random(1),VectorXd::Random(1),false}));
-  setups_ok   .push_back(PriorAsStruct({'O',"factor",VectorXd::Random(4).normalized(),VectorXd::Random(3),false}));
-  setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(3).normalized(),VectorXd::Random(3),false})); // wrong state size
-  setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(4).normalized(),VectorXd::Random(4),false})); // wrong sigma size
-  setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(4),VectorXd::Random(3),false})); // not normalized
+  setups_ok   .push_back(PriorAsStruct({"O","factor",vector1,vector1,false}));
+  setups_ok   .push_back(PriorAsStruct({"O","factor",vectorq,vector3,false}));
+  setups_death.push_back(PriorAsStruct({"O","factor",vector4,vector3,false})); // not normalized
+  setups_death.push_back(PriorAsStruct({"O","factor",vector3,vector3,false})); // wrong size
+  setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector4,false})); // inconsistent size
 
-  // SHOULD FAIL: Factor - dynamic (not implemented)
-  setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(1),VectorXd::Random(2),true}));
-  setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(4).normalized(),VectorXd::Random(3),true}));
+  // Factor - dynamic
+  setups_ok   .push_back(PriorAsStruct({"O","factor",vector1,vector1,true,vector1}));
+  setups_ok   .push_back(PriorAsStruct({"O","factor",vectorq,vector3,true,vector3}));
+  setups_death.push_back(PriorAsStruct({"O","factor",vector4,vector3,true,vector3})); // not normalized
+  setups_death.push_back(PriorAsStruct({"O","factor",vector3,vector3,true,vector3})); // wrong size
+  setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector3,true,vector4})); // inconsistent size
+  setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector4,true,vector3})); // inconsistent size
 
   // TEST SETUPS
   testPriors(setups_ok, true);
   testPriors(setups_death, false);
 }
 
-TEST(Prior, K)
+TEST(Prior, StateBlock)
 {
   std::vector<PriorAsStruct> setups_ok, setups_death;
 
-  // Nothing - not dynamic
-  setups_ok   .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(1),VectorXd::Random(0),false}));
-  setups_ok   .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(3),VectorXd::Random(0),false}));
-  setups_ok   .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(6),VectorXd::Random(0),false}));
+  // 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({'K',"nothing",VectorXd::Random(1),VectorXd::Random(0),true,VectorXd::Random(1)}));
-  setups_ok   .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(3)}));
-  setups_death.push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(2)})); // wrong size
+  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
   
   // Initial guess - not dynamic
-  setups_ok   .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(1),VectorXd::Random(0),false}));
-  setups_ok   .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),false}));
-  setups_ok   .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(6),VectorXd::Random(0),false}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector2,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector3,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector4,vector0,false}));
 
   // Initial guess - dynamic
-  setups_ok   .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(1),VectorXd::Random(0),true,VectorXd::Random(1)}));
-  setups_ok   .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(3)}));
-  setups_death.push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(2)})); // wrong size
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector2,vector0,true,vector2}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector3,vector0,true,vector3}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","initial_guess",vector4,vector0,true,vector4}));
+  setups_death.push_back(PriorAsStruct({"StateBlock","initial_guess",vector3,vector0,true,vector4})); // inconsistent size
   
   // Fix - not dynamic
-  setups_ok   .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(1),VectorXd::Random(0),false}));
-  setups_ok   .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(3),VectorXd::Random(0),false}));
-  setups_ok   .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(6),VectorXd::Random(0),false}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","fix",vector2,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","fix",vector3,vector0,false}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","fix",vector4,vector0,false})); // wrong size
 
   // Fix - dynamic
-  setups_ok   .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(1),VectorXd::Random(0),true,VectorXd::Random(1)}));
-  setups_ok   .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(3)}));
-  setups_death.push_back(PriorAsStruct({'K',"fix",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(2)})); // wrong size
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","fix",vector2,vector0,true,vector2}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","fix",vector3,vector0,true,vector3}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","fix",vector4,vector0,true,vector4}));
+  setups_death.push_back(PriorAsStruct({"StateBlock","fix",vector3,vector0,true,vector4})); // inconsistent size
+  
+  // Factor - not dynamic
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","factor",vector2,vector2,false}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","factor",vector3,vector3,false}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","factor",vector4,vector4,false}));
+  setups_death.push_back(PriorAsStruct({"StateBlock","factor",vector2,vector3,false})); // inconsistent size
+
+  // Factor - dynamic
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","factor",vector2,vector2,true,vector2}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","factor",vector3,vector3,true,vector3}));
+  setups_ok   .push_back(PriorAsStruct({"StateBlock","factor",vector4,vector4,true,vector4}));
+  setups_death.push_back(PriorAsStruct({"StateBlock","factor",vector3,vector3,true,vector4})); // inconsistent size
+  setups_death.push_back(PriorAsStruct({"StateBlock","factor",vector3,vector4,true,vector3})); // inconsistent size
+
+  // TEST SETUPS
+  testPriors(setups_ok, true);
+  testPriors(setups_death, false);
+}
+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
+
+  // Initial guess - dynamic
+  setups_ok   .push_back(PriorAsStruct({"StateAngle","initial_guess",vector1,vector0,true,vector1}));
+  setups_death.push_back(PriorAsStruct({"StateAngle","initial_guess",vector3,vector0,true,vector3})); // wrong size
+  setups_death.push_back(PriorAsStruct({"StateAngle","initial_guess",vector1,vector0,true,vector4})); // inconsistent size
+  
+  // Fix - not dynamic
+  setups_ok   .push_back(PriorAsStruct({"StateAngle","fix",vector1,vector0,false}));
+  setups_death.push_back(PriorAsStruct({"StateAngle","fix",vector3,vector0,false})); // wrong size
+
+  // Fix - dynamic
+  setups_ok   .push_back(PriorAsStruct({"StateAngle","fix",vector1,vector0,true,vector1}));
+  setups_death.push_back(PriorAsStruct({"StateAngle","fix",vector3,vector0,true,vector3})); // wrong size
+  setups_death.push_back(PriorAsStruct({"StateAngle","fix",vector1,vector0,true,vector4})); // inconsistent size
 
   // Factor - not dynamic
-  setups_ok   .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(1),VectorXd::Random(1),false}));
-  setups_ok   .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(3),VectorXd::Random(3),false}));
-  setups_ok   .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(6),VectorXd::Random(6),false}));
-  setups_death.push_back(PriorAsStruct({'K',"factor",VectorXd::Random(4),VectorXd::Random(3),false})); // wrong sigma size
+  setups_ok   .push_back(PriorAsStruct({"StateAngle","factor",vector1,vector1,false}));
+  setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector3,vector3,false})); // wrong size
+  setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector1,vector4,false})); // inconsistent size
 
   // Factor - dynamic
-  setups_ok   .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(1),VectorXd::Random(1),true,VectorXd::Random(1)}));
-  setups_ok   .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(3),VectorXd::Random(3),true,VectorXd::Random(3)}));
-  setups_death.push_back(PriorAsStruct({'K',"factor",VectorXd::Random(3),VectorXd::Random(3),true,VectorXd::Random(2)})); // wrong sigma_drift size
-  setups_death.push_back(PriorAsStruct({'K',"factor",VectorXd::Random(3),VectorXd::Random(2),true,VectorXd::Random(3)})); // wrong sigma size
+  setups_ok   .push_back(PriorAsStruct({"StateAngle","factor",vector1,vector1,true,vector1}));
+  setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector3,vector3,true,vector3})); // wrong size
+  setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector1,vector3,true,vector1})); // inconsistent size
+  setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector1,vector1,true,vector3})); // inconsistent size
 
   // TEST SETUPS
   testPriors(setups_ok, true);
   testPriors(setups_death, false);
 }
 
-// TODO: param_server constructor!
+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
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vector3,vector0,false})); // wrong size
+
+  // Initial guess - dynamic
+  setups_ok   .push_back(PriorAsStruct({"StateQuaternion","initial_guess",vectorq,vector0,true,vector3}));
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vector4,vector0,true,vector3})); // not normalized
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vector3,vector0,true,vector3})); // wrong size
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vectorq,vector0,true,vector4})); // inconsistent size
+  
+  // Fix - not dynamic
+  setups_ok   .push_back(PriorAsStruct({"StateQuaternion","fix",vectorq,vector0,false}));
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","fix",vector3,vector0,false})); // wrong size
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","fix",vector4,vector0,false})); // not normalized
+
+  // Fix - dynamic
+  setups_ok   .push_back(PriorAsStruct({"StateQuaternion","fix",vectorq,vector0,true,vector3}));
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","fix",vector4,vector0,true,vector3})); // not normalized
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","fix",vector3,vector0,true,vector3})); // wrong size
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","fix",vectorq,vector0,true,vector4})); // inconsistent size
+
+  // Factor - not dynamic
+  setups_ok   .push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector3,false}));
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vector4,vector3,false})); // not normalized
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vector3,vector3,false})); // wrong size
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector4,false})); // inconsistent size
 
+  // Factor - dynamic
+  setups_ok   .push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector3,true,vector3}));
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vector4,vector3,true,vector3})); // not normalized
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vector3,vector3,true,vector3})); // wrong size
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector3,true,vector4})); // inconsistent size
+  setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector4,true,vector3})); // inconsistent size
+
+  // TEST SETUPS
+  testPriors(setups_ok, true);
+  testPriors(setups_death, false);
+}
+
+TEST(Prior, ParamsServer)
+{
+  ParserYaml parser   = ParserYaml("test/yaml/params_prior.yaml", wolf_root);
+  ParamsServer server = ParamsServer(parser.getParams());
+
+  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<bool> dynamics({false,true});
+
+  VectorXd p_state(4), o_state(4), po_sigma(4);
+  p_state << 1, 2, 3, 4;
+  o_state << 1, 0, 0, 0;
+  po_sigma << 0.1, 0.2, 0.3, 0.4;
+
+  // P & O
+  for (auto key : keys)
+    for (auto dim : dims)
+      for (auto mode : modes)
+        for (auto dynamic : dynamics)
+        {
+          std::string prefix = key + to_string(dim) + "_" + mode + (dynamic ? "_dynamic" : "");
+
+          WOLF_INFO("Creating prior from prefix ", prefix);
+          auto P = Prior(prefix, key, server);
+
+          // Checks
+          ASSERT_EQ(P.getMode(), mode);
+          ASSERT_EQ(P.isDynamic(), dynamic);
+
+          VectorXd state = p_state.head(dim);
+          VectorXd sigma = po_sigma.head(dim);
+          if (key == "O")
+          {
+            state = o_state.head(dim == 2 ? 1 : 4);
+            sigma = po_sigma.head(dim == 2 ? 1 : 3);
+          }
+
+          if (mode != "nothing")
+          {
+            ASSERT_MATRIX_APPROX(P.getState(),state,wolf::Constants::EPS);
+          }
+          if (dynamic)
+          {
+            ASSERT_MATRIX_APPROX(P.getSigmaDrift(),sigma,wolf::Constants::EPS);
+          }
+          if (mode == "factor")
+          {
+            ASSERT_MATRIX_APPROX(P.getSigma(),sigma,wolf::Constants::EPS);
+          }
+        }
+
+  // I
+  for (auto mode : modes)
+    for (auto dynamic : dynamics)
+    {
+      std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : "");
+      WOLF_INFO("Creating prior from prefix ", prefix);
+      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);
+      }
+      if (dynamic)
+      {
+        ASSERT_MATRIX_APPROX(P.getSigmaDrift(),po_sigma,wolf::Constants::EPS);
+      }
+      if (mode == "factor")
+      {
+        ASSERT_MATRIX_APPROX(P.getSigma(),po_sigma,wolf::Constants::EPS);
+      }
+    }
+}
+
+TEST(Prior, ParamsServerWrong)
+{
+  ParserYaml parser   = ParserYaml("test/yaml/params_prior_wrong.yaml", wolf_root);
+  ParamsServer server = ParamsServer(parser.getParams());
+
+  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<bool> dynamics({false,true});
+
+  VectorXd p_state(4), o_state(4), po_sigma(4);
+  p_state << 1, 2, 3, 4;
+  o_state << 1, 0, 0, 0;
+  po_sigma << 0.1, 0.2, 0.3, 0.4;
+
+  // P & O
+  for (auto key : keys)
+    for (auto dim : dims)
+      for (auto mode : modes)
+        for (auto dynamic : dynamics)
+        {
+          std::string prefix = key + to_string(dim) + "_" + mode + (dynamic ? "_dynamic" : "");
+
+          WOLF_INFO("Creating prior from prefix ", prefix);
+          ASSERT_THROW(auto P = Prior(prefix, key, server),std::runtime_error);
+        }
+
+  // I
+  for (auto mode : modes)
+    for (auto dynamic : dynamics)
+    {
+      std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : "");
+      WOLF_INFO("Creating prior from prefix ", prefix);
+      ASSERT_THROW(auto P = Prior(prefix, "StateBlock", server),std::runtime_error);
+    }
+}
 
 int main(int argc, char **argv)
 {
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 5f2e785f147ed275ce48c73ac6d922d028fd2f73..0a367cd3047d706bbf46ce4697a4ea9c67fbd388 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -43,8 +43,8 @@ TEST(SensorBase, POfix)
   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","fix", Eigen::Vector2d::Zero())},
+                                                {'O',Prior("O","fix", Eigen::Vector1d::Zero())}}),
                                         params);
 
   ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
diff --git a/test/yaml/params_prior.yaml b/test/yaml/params_prior.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1b1955cdd541043243419ea56ed0e4be17201404
--- /dev/null
+++ b/test/yaml/params_prior.yaml
@@ -0,0 +1,228 @@
+config:
+  problem:
+    dim: 2
+  
+  # P in 2D
+  P2_nothing:
+    mode: nothing
+    dynamic: false
+  
+  P2_initial_guess:
+    mode: initial_guess
+    state: [1, 2]
+    dynamic: false
+  
+  P2_fix:
+    mode: fix
+    state: [1, 2]
+    dynamic: false
+
+  P2_factor:
+    mode: factor
+    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
+    state: [1, 2]
+    dynamic: true
+    sigma_drift: [0.1, 0.2]
+  
+  P2_fix_dynamic:
+    mode: fix
+    state: [1, 2]
+    dynamic: true
+    sigma_drift: [0.1, 0.2]
+
+  P2_factor_dynamic:
+    mode: factor
+    state: [1, 2]
+    sigma: [0.1, 0.2]
+    dynamic: true
+    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]
+    dynamic: false
+  
+  P3_fix:
+    mode: fix
+    state: [1, 2, 3]
+    dynamic: false
+
+  P3_factor:
+    mode: factor
+    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
+    state: [1, 2, 3]
+    dynamic: true
+    sigma_drift: [0.1, 0.2, 0.3]
+  
+  P3_fix_dynamic:
+    mode: fix
+    state: [1, 2, 3]
+    dynamic: true
+    sigma_drift: [0.1, 0.2, 0.3]
+
+  P3_factor_dynamic:
+    mode: factor
+    state: [1, 2, 3]
+    sigma: [0.1, 0.2, 0.3]
+    dynamic: true
+    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]
+    dynamic: false
+  
+  O2_fix:
+    mode: fix
+    state: [1]
+    dynamic: false
+
+  O2_factor:
+    mode: factor
+    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
+    state: [1]
+    dynamic: true
+    sigma_drift: [0.1]
+  
+  O2_fix_dynamic:
+    mode: fix
+    state: [1]
+    dynamic: true
+    sigma_drift: [0.1]
+
+  O2_factor_dynamic:
+    mode: factor
+    state: [1]
+    sigma: [0.1]
+    dynamic: true
+    sigma_drift: [0.1]
+
+  # O in 3D
+  O3_nothing:
+    mode: nothing
+    dynamic: false
+  
+  O3_initial_guess:
+    mode: initial_guess
+    state: [1, 0, 0, 0]
+    dynamic: false
+  
+  O3_fix:
+    mode: fix
+    state: [1, 0, 0, 0]
+    dynamic: false
+
+  O3_factor:
+    mode: factor
+    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
+    state: [1, 0, 0, 0]
+    dynamic: true
+    sigma_drift: [0.1, 0.2, 0.3]
+  
+  O3_fix_dynamic:
+    mode: fix
+    state: [1, 0, 0, 0]
+    dynamic: true
+    sigma_drift: [0.1, 0.2, 0.3]
+
+  O3_factor_dynamic:
+    mode: factor
+    state: [1, 0, 0, 0]
+    sigma: [0.1, 0.2, 0.3]
+    dynamic: true
+    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]
+    dynamic: false
+  
+  I_fix:
+    mode: fix
+    state: [1, 2, 3, 4]
+    dynamic: false
+
+  I_factor:
+    mode: factor
+    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
+    state: [1, 2, 3, 4]
+    dynamic: true
+    sigma_drift: [0.1, 0.2, 0.3, 0.4]
+  
+  I_fix_dynamic:
+    mode: fix
+    state: [1, 2, 3, 4]
+    dynamic: true
+    sigma_drift: [0.1, 0.2, 0.3, 0.4]
+
+  I_factor_dynamic:
+    mode: factor
+    state: [1, 2, 3, 4]
+    sigma: [0.1, 0.2, 0.3, 0.4]
+    dynamic: true
+    sigma_drift: [0.1, 0.2, 0.3, 0.4]
\ No newline at end of file
diff --git a/test/yaml/params_prior_wrong.yaml b/test/yaml/params_prior_wrong.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5a40d4743123e8e777016d86cbcb7aa178a0ed66
--- /dev/null
+++ b/test/yaml/params_prior_wrong.yaml
@@ -0,0 +1,228 @@
+config:
+  problem:
+    dim: 2
+  
+  # P in 2D
+  P2_nothing:
+    mode: no  # wrong mode
+    dynamic: false
+  
+  P2_initial_guess:
+    mode: initial_guess
+    #state: [1, 2] # missing
+    dynamic: false
+  
+  P2_fix:
+    mode: fix
+    #state: [1, 2] # missing
+    dynamic: false
+
+  P2_factor:
+    mode: factor
+    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
+    state: [1, 2]
+    dynamic: true
+    #sigma_drift: [0.1, 0.2] #missing
+  
+  P2_fix_dynamic:
+    mode: fix
+    state: [1, 2]
+    dynamic: true
+    sigma_drift: [0.1, 0.2, 0.3] # wrong size
+
+  P2_factor_dynamic:
+    mode: factor
+    state: [1, 2]
+    #sigma: [0.1, 0.2] # missing
+    dynamic: true
+    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]
+    dynamic: false
+  
+  P3_fix:
+    mode: fix
+    state: [1, 2, 3, 4] # wrong size
+    dynamic: false
+
+  P3_factor:
+    mode: factor
+    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
+    state: [1, 2, 3]
+    dynamic: true
+    sigma_drift: [0.1, 0.2] # wrong size
+  
+  P3_fix_dynamic:
+    mode: fix
+    state: [1, 2, 3]
+    dynamic: true
+    #sigma_drift: [0.1, 0.2] # missing
+
+  P3_factor_dynamic:
+    mode: factor
+    state: [1, 2, 3]
+    sigma: [0.1, 0.2] # wrong size
+    dynamic: true
+    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
+    dynamic: false
+  
+  O2_fix:
+    mode: fix
+    state: [1]
+    #dynamic: false # missing
+
+  O2_factor:
+    mode: factor
+    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
+    state: [1]
+    dynamic: true
+    #sigma_drift: [0.1] # missing
+  
+  O2_fix_dynamic:
+    mode: fix
+    state: [1 2] # wrong size
+    dynamic: true
+    sigma_drift: [0.1]
+
+  O2_factor_dynamic:
+    mode: factor
+    state: [1]
+    sigma: [0.1 0.2] # wrong size
+    dynamic: true
+    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
+    dynamic: false
+  
+  O3_fix:
+    mode: fix
+    state: [1, 0, 0, 0]
+    #dynamic: false # missing
+
+  O3_factor:
+    mode: factor
+    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
+    state: [1, 0, 0] # wrong size
+    dynamic: true
+    sigma_drift: [0.1, 0.2, 0.3]
+  
+  O3_fix_dynamic:
+    mode: fix
+    state: [1, 0, 0, 0]
+    dynamic: true
+    #sigma_drift: [0.1, 0.2, 0.3] # missing
+
+  O3_factor_dynamic:
+    mode: factor
+    state: [1, 0, 0, 0]
+    sigma: [0.1, 0.2, 0.3]
+    dynamic: true
+    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]
+    dynamic: false
+
+  I_factor:
+    mode: factor
+    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
+    state: [1, 2, 3, 4]
+    dynamic: true
+    sigma_drift: [0.1, 0.2, 0.3] # wrong size
+  
+  I_fix_dynamic:
+    mode: fix
+    state: [1, 2, 3] # wrong size
+    dynamic: true
+    sigma_drift: [0.1, 0.2, 0.3, 0.4]
+
+  I_factor_dynamic:
+    mode: factor
+    state: [1, 2, 3, 4]
+    sigma: [0.1, 0.2, 0.3]  # wrong size
+    dynamic: true
+    sigma_drift: [0.1, 0.2, 0.3, 0.4]
\ No newline at end of file