diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 1e0006d70660ae35a665bb921ca35eeba366b2e3..172ccff4c9986851ca7760afde526b04bbc1e3f7 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -90,8 +90,8 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
         void removeStateBlocks();
         virtual void registerNewStateBlocks() const;
 
-        void fix();
-        void unfix();
+        virtual void fix() override;
+        virtual void unfix() override;
 
         bool hasCalibration() {return calib_size_ > 0;}
         SizeEigen getCalibSize() const;
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index b5cfcc42fab8f1da75b18f26fbacb8dff4968055..eb4e6cd3a038a9f9d3ee934aa4cc9fd3d61ad3f5 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -105,7 +105,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
 
         // States
     public:
-        void setState(const Eigen::VectorXd& _state);
         bool getCovariance(Eigen::MatrixXd& _cov) const;
 
         // Wolf tree access ---------------------------------------------------
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 43f82f10036a364061c681e79e38c598e88dceb4..ab90bb26d654e76905e47a9b57effd6be4b69e54 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -173,8 +173,10 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         bool process(const CaptureBasePtr capture_ptr);
 
         // State blocks
-        void addStateBlock(const std::string& _key, const StateBlockPtr _sb_ptr);
-        void addStateBlock(const char _key, const StateBlockPtr _sb_ptr) { addStateBlock(std::string(1, _key), _sb_ptr); }
+        virtual StateBlockPtr addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr) override;
+        virtual StateBlockPtr addStateBlock(const char _key, const StateBlockPtr& _sb_ptr) override { return this->addStateBlock(std::string(1, _key), _sb_ptr); }
+        StateBlockPtr addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic);
+        StateBlockPtr addStateBlock(const char _key, const StateBlockPtr& _sb_ptr, const bool _dynamic) { return this->addStateBlock(std::string(1, _key), _sb_ptr, _dynamic); }
         StateBlockPtr getStateBlockDynamic(const std::string& _key) const;
         StateBlockPtr getStateBlockDynamic(const std::string& _key, const TimeStamp& _ts) const;
         StateBlockPtr getStateBlockDynamic(const char _key) const { return getStateBlockDynamic(std::string(1,_key)); }
@@ -277,19 +279,31 @@ inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
     return processor_list_;
 }
 
-inline void SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr _sb_ptr)
+inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr)
 {
     assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
     HasStateBlocks::addStateBlock(_key, _sb_ptr);
+    setStateBlockDynamic(_key, false);
+    return _sb_ptr;
+}
+
+inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
+{
+    assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
+    HasStateBlocks::addStateBlock(_key, _sb_ptr);
+    setStateBlockDynamic(_key, _dynamic);
+    return _sb_ptr;
 }
 
 inline void SensorBase::setStateBlockDynamic(const std::string& _key, bool _dynamic)
 {
+    assert(hasStateBlock(_key) and "setting dynamic an state block of a key that doesn't exist in the sensor. It is required anyway.");
     state_block_dynamic_[_key] = _dynamic;
 }
 
 inline bool SensorBase::isStateBlockDynamic(const std::string& _key) const
 {
+    assert(state_block_dynamic_.count(_key) != 0);
     return state_block_dynamic_.at(_key);
 }
 
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index f2c7cf227c3ca2b308731a5c2d2a257cb2de6910..c5938cef8777b47daea6f3205fde75554915288b 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -22,33 +22,28 @@ class HasStateBlocks
         HasStateBlocks(const std::string& _structure) : structure_(_structure), state_block_map_() {}
         virtual ~HasStateBlocks();
 
-    public:
         const std::string& getStructure() const { return structure_; }
         void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; }
         bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
 
-    public:
         const std::map<std::string, StateBlockPtr>& getStateBlockMap() const;
         std::vector<StateBlockPtr> getStateBlockVec() const;
 
-    public:
         // Some typical shortcuts -- not all should be coded here, see notes below.
         StateBlockPtr getP() const;
         StateBlockPtr getO() const;
         void setP(const StateBlockPtr _p_ptr);
         void setO(const StateBlockPtr _o_ptr);
 
-    public:
         // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
-        void fix();
-        void unfix();
-        bool isFixed() const;
-
-    public:
-        StateBlockPtr   addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb);
-        StateBlockPtr   addStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return addStateBlock(std::string(1,_sb_type), _sb); }
-        unsigned int    removeStateBlock(const std::string& _sb_type);
-        unsigned int    removeStateBlock(const char _sb_type);
+        virtual void fix();
+        virtual void unfix();
+        virtual bool isFixed() const;
+
+        virtual StateBlockPtr   addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb);
+        virtual StateBlockPtr   addStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return addStateBlock(std::string(1,_sb_type), _sb); }
+        virtual unsigned int    removeStateBlock(const std::string& _sb_type);
+        virtual unsigned int    removeStateBlock(const char _sb_type);
         bool            hasStateBlock(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
         bool            hasStateBlock(const char _sb_type) const { return hasStateBlock(std::string(1, _sb_type)); }
         StateBlockPtr   getStateBlock(const std::string& _sb_type) const;
@@ -65,8 +60,7 @@ class HasStateBlocks
         inline StateBlockPtr emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
 
         // States
-    public:
-        void setState(const Eigen::VectorXd& _state, const bool _notify = true);
+        virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true);
         Eigen::VectorXd getState() const;
         void getState(Eigen::VectorXd& _state) const;
         unsigned int getSize() const;
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index e971b00ab87f5dbd2aa31efae66c434e61d6dbde..dd4e848601004573fabd94b328be05d728826cd9 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -23,31 +23,29 @@ CaptureBase::CaptureBase(const std::string& _type,
 {
     if (_sensor_ptr)
     {
-
-        if (_sensor_ptr->isStateBlockDynamic("P"))
+        if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic("P"))
         {
             assert(_p_ptr && "Pointer to dynamic position params is null!");
             addStateBlock("P", _p_ptr);
         }
         else
-            assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static");
-
+            assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static or doesn't exist");
 
-        if (_sensor_ptr->isStateBlockDynamic("O"))
+        if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic("O"))
         {
             assert(_o_ptr && "Pointer to dynamic orientation params is null!");
             addStateBlock("O", _o_ptr);
         }
         else
-            assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static");
+            assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist");
 
-        if (_sensor_ptr->isStateBlockDynamic("I"))
+        if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic("I"))
         {
             assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
             addStateBlock("I", _intr_ptr);
         }
         else
-            assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static");
+            assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist");
 
     }
     else if (_p_ptr || _o_ptr || _intr_ptr)
@@ -144,7 +142,7 @@ StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
 {
     if (getSensor())
     {
-        if (getSensor()->isStateBlockDynamic(_key))
+        if (getSensor()->getO() and getSensor()->isStateBlockDynamic(_key))
             return HasStateBlocks::getStateBlock(_key);
         else
             return getSensor()->getStateBlock(_key);
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index d293a8727cda2b24875b41bacadde96112f722c6..8fab1f81f8cebc9832bad1ad1d5987a34747ee4c 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -205,11 +205,6 @@ void FrameBase::setAux()
     }
 }
 
-void FrameBase::setState(const Eigen::VectorXd& _state)
-{
-    HasStateBlocks::setState(_state, isKeyOrAux());
-}
-
 bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const
 {
     return getProblem()->getFrameCovariance(shared_from_this(), _cov);
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 05755041df8b92c7e53895b8ed2732d0739b37ce..3848f77c96d2fdbd3db5197dd35195342dd95474 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -25,26 +25,21 @@ SensorBase::SensorBase(const std::string& _type,
         noise_std_(_noise_size),
         noise_cov_(_noise_size, _noise_size)
 {
+    assert((_p_ptr or not _p_dyn) and "Trying to set dynamic position state block without providing a position state block. It is required anyway.");
+    assert((_o_ptr or not _o_dyn) and "Trying to set dynamic orientation state block without providing an orientation state block. It is required anyway.");
+    assert((_intr_ptr or not _intr_dyn) and "Trying to set dynamic intrinsics state block without providing an intrinsics state block. It is required anyway.");
+
     noise_std_.setZero();
     noise_cov_.setZero();
 
     if (_p_ptr)
-    {
-        addStateBlock("P", _p_ptr);
-    }
-    setStateBlockDynamic("P", _p_dyn);
+        addStateBlock("P", _p_ptr, _p_dyn);
 
     if (_o_ptr)
-    {
-        addStateBlock("O", _o_ptr);
-    }
-    setStateBlockDynamic("O", _o_dyn);
+        addStateBlock("O", _o_ptr, _o_dyn);
 
     if (_intr_ptr)
-    {
-        addStateBlock("I", _intr_ptr);
-    }
-    setStateBlockDynamic("I", _intr_dyn);
+        addStateBlock("I", _intr_ptr, _intr_dyn);
 
     updateCalibSize();
 }
@@ -68,22 +63,13 @@ SensorBase::SensorBase(const std::string& _type,
     setNoiseStd(_noise_std);
 
     if (_p_ptr)
-    {
-        addStateBlock("P", _p_ptr);
-    }
-    setStateBlockDynamic("P", _p_dyn);
+        addStateBlock("P", _p_ptr, _p_dyn);
 
     if (_o_ptr)
-    {
-        addStateBlock("O", _o_ptr);
-    }
-    setStateBlockDynamic("O", _o_dyn);
+        addStateBlock("O", _o_ptr, _o_dyn);
 
     if (_intr_ptr)
-    {
-        addStateBlock("I", _intr_ptr);
-    }
-    setStateBlockDynamic("I", _intr_dyn);
+        addStateBlock("I", _intr_ptr, _intr_dyn);
 
     updateCalibSize();
 }
@@ -403,7 +389,7 @@ StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key, const Ti
 
 bool SensorBase::isStateBlockInCapture(const std::string& _key, CaptureBasePtr& _cap) const
 {
-    if (isStateBlockDynamic(_key))
+    if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
     {
         _cap = lastCapture();
         //        cap = lastKeyCapture();
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 45d9967706d9eca9d04d9b7f15b806f8fa0cf4bd..f0c1cbfcb2cee9f676700846dbb93fe0cd9e3521 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -59,7 +59,7 @@ TEST(CaptureBase, Dynamic_sensor_params)
     StateBlockPtr p(std::make_shared<StateBlock>(2));
     StateBlockPtr o(std::make_shared<StateAngle>() );
     StateBlockPtr i(std::make_shared<StateBlock>(2));
-    SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2, true, true, true)); // last 3 'true' mark dynamic
+    SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", std::make_shared<StateBlock>(2), std::make_shared<StateAngle>(), std::make_shared<StateBlock>(2), 2, true, true, true)); // last 3 'true' mark dynamic
     CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
 
     // query sensor blocks -- need KFs to find some Capture with the params