diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index 224fb93ec44260c0a824f81e400168f627dd5ab7..b81d9f8cfaf90396ffdf904ebf3c12d9bb6249d1 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -116,6 +116,10 @@ class SolverCeres : public SolverManager
 
         const Eigen::SparseMatrixd computeHessian() const;
 
+        virtual int numStateBlocksDerived() const override;
+
+        virtual int numFactorsDerived() const override;
+
     protected:
 
         bool checkDerived(std::string prefix="") const override;
@@ -139,6 +143,13 @@ class SolverCeres : public SolverManager
         bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
 
         bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override;
+
+        bool isStateBlockFixedDerived(const StateBlockPtr& st) override;
+
+        bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+                                                const LocalParametrizationBasePtr& local_param) override;
+
+        bool hasLocalParametrizationDerived(const StateBlockPtr& st)  const override;
 };
 
 inline ceres::Solver::Summary SolverCeres::getSummary()
@@ -168,6 +179,34 @@ inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& stat
             && ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
 }
 
+inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st)
+{
+    return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
+};
+
+inline bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+                                                            const LocalParametrizationBasePtr& local_param)
+{
+    return state_blocks_local_param_.count(st) == 1 and
+           state_blocks_local_param_.at(st)->getLocalParametrization() == local_param and
+           ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
+};
+
+inline bool SolverCeres::hasLocalParametrizationDerived(const StateBlockPtr& st) const
+{
+    return state_blocks_local_param_.count(st) == 1;
+};
+
+inline int SolverCeres::numStateBlocksDerived() const
+{
+    return ceres_problem_->NumParameterBlocks();
+}
+
+inline int SolverCeres::numFactorsDerived() const
+{
+    return ceres_problem_->NumResidualBlocks();
+};
+
 } // namespace wolf
 
 #endif
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 378f92cb9c3a48b2410567262562b6e74e3e16fe..bdd8a37aaa817b36609cc672402161f60a5fdba2 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -121,7 +121,22 @@ class SolverManager
 
         virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
 
+        virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final;
+
         virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final;
+        virtual bool isStateBlockFixed(const StateBlockPtr& st) final;
+
+        virtual bool hasThisLocalParametrization(const StateBlockPtr& st,
+                                                 const LocalParametrizationBasePtr& local_param) final;
+
+        virtual bool hasLocalParametrization(const StateBlockPtr& st) const final;
+
+        virtual int numFactors() const final;
+        virtual int numStateBlocks() const final;
+        virtual int numStateBlocksFloating() const final;
+
+        virtual int numFactorsDerived() const = 0;
+        virtual int numStateBlocksDerived() const = 0;
 
         virtual bool check(std::string prefix="") const final;
 
@@ -134,7 +149,7 @@ class SolverManager
 
         virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
         const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
-        virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
+        double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
 
     private:
         // SolverManager functions
@@ -158,6 +173,10 @@ class SolverManager
         virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0;
         virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
         virtual bool checkDerived(std::string prefix="") const = 0;
+        virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0;
+        virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+                                                        const LocalParametrizationBasePtr& local_param) = 0;
+        virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
 };
 
 // Params (here becaure it needs of declaration of SolverManager::ReportVerbosity)
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 2f682d050f42e4490468252674a06dd4cf07b1df..9649f9e063fab5b8ead117332ca8f070836b8b86 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -353,7 +353,12 @@ SolverManager::ReportVerbosity SolverManager::getVerbosity() const
 
 bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr) const
 {
-    return floating_state_blocks_.count(state_ptr) == 1 or (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr));
+    return isStateBlockFloating(state_ptr) or (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr));
+}
+
+bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const
+{
+    return floating_state_blocks_.count(state_ptr) == 1;
 }
 
 bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
@@ -361,6 +366,45 @@ bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
     return factors_.count(fac_ptr) and isFactorRegisteredDerived(fac_ptr);
 }
 
+bool SolverManager::isStateBlockFixed(const StateBlockPtr& st)
+{
+    if (isStateBlockFloating(st))
+        return st->isFixed();
+    else
+        return isStateBlockFixedDerived(st);
+}
+
+bool SolverManager::hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
+{
+    if (isStateBlockFloating(st))
+        return st->getLocalParametrization() == local_param;
+    else
+        return hasThisLocalParametrizationDerived(st, local_param);
+};
+
+bool SolverManager::hasLocalParametrization(const StateBlockPtr& st) const
+{
+    if (isStateBlockFloating(st))
+        return st->hasLocalParametrization();
+    else
+        return hasLocalParametrizationDerived(st);
+};
+
+int SolverManager::numFactors() const
+{
+    return factors_.size();
+}
+
+int SolverManager::numStateBlocks() const
+{
+    return state_blocks_.size() + floating_state_blocks_.size();
+}
+
+int SolverManager::numStateBlocksFloating() const
+{
+    return floating_state_blocks_.size();
+}
+
 double SolverManager::getPeriod() const
 {
     return params_->period;
@@ -431,9 +475,38 @@ bool SolverManager::check(std::string prefix) const
         }
     }
 
-    // checkDerived
+    // CHECK DERIVED ----------------------
     ok &= checkDerived(prefix);
 
+    // CHECK IF DERIVED IS UP TO DATE ----------------------
+    // state blocks registered in derived solver
+    for (auto sb : state_blocks_)
+        if (!isStateBlockRegisteredDerived(sb.first))
+        {
+            WOLF_ERROR("SolverManager::checkAgainstDerived: state block ", sb.first, " is in state_blocks_ but not registered in derived solver - in ", prefix);
+            ok = false;
+        }
+
+    // factors registered in derived solver
+    for (auto fac : factors_)
+        if (!isFactorRegisteredDerived(fac))
+        {
+            WOLF_ERROR("SolverManager::checkAgainstDerived: factor ", fac->id(), " is in factors_ but not registered in derived solver - in ", prefix);
+            ok = false;
+        }
+
+    // numbers
+    if (numStateBlocksDerived() != state_blocks_.size())
+    {
+        WOLF_ERROR("SolverManager::checkAgainstDerived: numStateBlocksDerived() = ", numStateBlocksDerived(), " DIFFERENT THAN state_blocks_.size() = ", state_blocks_.size(), " - in ", prefix);
+        ok = false;
+    }
+    if (numFactorsDerived() != numFactors())
+    {
+        WOLF_ERROR("SolverManager::checkAgainstDerived: numFactorsDerived() = ", numFactorsDerived(), " DIFFERENT THAN numFactors() = ", numFactors(), " - in ", prefix);
+        ok = false;
+    }
+
     return ok;
 }
 
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index 43b902abd0e5dd8648730303f5721ee55da9472f..89febb761317bc9bf81d10642e5ee6a981ab356b 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_manager_dummy.h
@@ -17,7 +17,7 @@ WOLF_PTR_TYPEDEFS(SolverManagerDummy);
 class SolverManagerDummy : public SolverManager
 {
     public:
-        std::set<FactorBasePtr> factors_;
+        std::set<FactorBasePtr> factors_derived_;
         std::map<StateBlockPtr,bool> state_block_fixed_;
         std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
 
@@ -26,28 +26,29 @@ class SolverManagerDummy : public SolverManager
         {
         };
 
-        bool isStateBlockFixed(const StateBlockPtr& st) const
+        bool isStateBlockFixedDerived(const StateBlockPtr& st) override
         {
-            if (floating_state_blocks_.count(st))
-                return st->isFixed();
-            else
-                return state_block_fixed_.at(st);
+            return state_block_fixed_.at(st);
         };
 
-        bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
+        bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) override
         {
-            if (floating_state_blocks_.count(st))
-                return st->getLocalParametrization() == local_param;
-            else
-                return state_block_local_param_.count(st) == 1 and  state_block_local_param_.at(st) == local_param;
+            return state_block_local_param_.count(st) == 1 and  state_block_local_param_.at(st) == local_param;
         };
 
-        bool hasLocalParametrization(const StateBlockPtr& st) const
+        bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override
         {
-            if (floating_state_blocks_.count(st))
-                return st->hasLocalParametrization();
-            else
-                return state_block_local_param_.count(st) == 1;
+            return state_block_local_param_.count(st) == 1;
+        };
+
+        virtual int numStateBlocksDerived() const override
+        {
+            return state_block_fixed_.size();
+        }
+
+        virtual int numFactorsDerived() const override
+        {
+            return factors_derived_.size();
         };
 
         void computeCovariances(const CovarianceBlocksToBeComputed blocks) override {};
@@ -68,11 +69,11 @@ class SolverManagerDummy : public SolverManager
         std::string solveDerived(const ReportVerbosity report_level) override { return std::string("");};
         void addFactorDerived(const FactorBasePtr& fac_ptr) override
         {
-            factors_.insert(fac_ptr);
+            factors_derived_.insert(fac_ptr);
         };
         void removeFactorDerived(const FactorBasePtr& fac_ptr) override
         {
-            factors_.erase(fac_ptr);
+            factors_derived_.erase(fac_ptr);
         };
         void addStateBlockDerived(const StateBlockPtr& state_ptr) override
         {
@@ -102,7 +103,7 @@ class SolverManagerDummy : public SolverManager
 
         bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override
         {
-            return factors_.count(fac_ptr) == 1;
+            return factors_derived_.count(fac_ptr) == 1;
         };
 };
 
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
index a9f397c1a3ce250253bd7031c624beca81e10319..3dd5a30b546edc5755b130ea204c60c5592a4c23 100644
--- a/test/gtest_solver_ceres.cpp
+++ b/test/gtest_solver_ceres.cpp
@@ -36,14 +36,6 @@ class SolverCeresWrapper : public SolverCeres
         {
         };
 
-        bool isStateBlockFixed(const StateBlockPtr& st)
-        {
-            if (floating_state_blocks_.count(st))
-                return st->isFixed();
-            else
-                return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
-        };
-
         int numStateBlocks()
         {
             return ceres_problem_->NumParameterBlocks();
@@ -54,24 +46,6 @@ class SolverCeresWrapper : public SolverCeres
             return ceres_problem_->NumResidualBlocks();
         };
 
-        bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
-        {
-            if (floating_state_blocks_.count(st))
-                return st->getLocalParametrization() == local_param;
-            else
-                return state_blocks_local_param_.count(st) == 1 and
-                       state_blocks_local_param_.at(st)->getLocalParametrization() == local_param and
-                       ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
-        };
-
-        bool hasLocalParametrization(const StateBlockPtr& st) const
-        {
-            if (floating_state_blocks_.count(st))
-                return st->hasLocalParametrization();
-            else
-                return state_blocks_local_param_.count(st) == 1;
-        };
-
 };
 
 TEST(SolverCeres, Create)
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index cbe3824e537aa3bd050670c0c9f854a40857bb6d..de2586dfa38d431719c8441863d099058a1d4e65 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -13,6 +13,7 @@
 #include "core/state_block/state_block.h"
 #include "core/capture/capture_void.h"
 #include "core/factor/factor_pose_2d.h"
+#include "core/factor/factor_block_absolute.h"
 #include "core/state_block/local_parametrization_base.h"
 #include "core/state_block/local_parametrization_angle.h"
 #include "dummy/solver_manager_dummy.h"
@@ -23,25 +24,43 @@
 using namespace wolf;
 using namespace Eigen;
 
+StateBlockPtr createStateBlock()
+{
+    Vector2d state; state << 1, 2;
+    return std::make_shared<StateBlock>(state);
+}
+
+FactorBasePtr createFactor(StateBlockPtr sb_ptr)
+{
+    return std::make_shared<FactorBlockAbsolute>(std::make_shared<FeatureBase>("Dummy",
+                                                                               VectorXd::Zero(sb_ptr->getSize()),
+                                                                               MatrixXd::Identity(sb_ptr->getSize(),sb_ptr->getSize())),
+                                                 sb_ptr,
+                                                 nullptr,
+                                                 false);
+}
+
 TEST(SolverManager, Create)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // check double pointers to branches
-    ASSERT_EQ(P, solver_manager_ptr->getProblem());
+    EXPECT_EQ(P, solver_manager_ptr->getProblem());
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
-TEST(SolverManager, AddStateBlock)
+////////////////////////////////////////////////////////
+// FLOATING STATE BLOCKS
+TEST(SolverManager, FloatingStateBlock_Add)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
     // update solver
@@ -49,24 +68,25 @@ TEST(SolverManager, AddStateBlock)
 
     // check stateblock
     EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
-TEST(SolverManager, DoubleAddStateBlock)
+TEST(SolverManager, FloatingStateBlock_DoubleAdd)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
     // update solver
     solver_manager_ptr->update();
 
-    // add stateblock again
+    // notify stateblock again
     P->notifyStateBlock(sb_ptr,ADD);
 
     // update solver
@@ -74,110 +94,115 @@ TEST(SolverManager, DoubleAddStateBlock)
 
     // check stateblock
     EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
-TEST(SolverManager, UpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddUpdate)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // update solver
     solver_manager_ptr->update();
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // check stateblock unfixed
-    ASSERT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
 
     // Fix frame
     sb_ptr->fix();
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_TRUE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // update solver manager
     solver_manager_ptr->update();
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // check stateblock fixed
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
-TEST(SolverManager, AddUpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddFixed)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // Fix state block
     sb_ptr->fix();
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_TRUE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // update solver manager
     solver_manager_ptr->update();
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // check stateblock fixed
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
-TEST(SolverManager, AddUpdateLocalParamStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // Local param
     LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
@@ -187,81 +212,84 @@ TEST(SolverManager, AddUpdateLocalParamStateBlock)
     sb_ptr->fix();
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_TRUE(sb_ptr->fixUpdated());
-    ASSERT_TRUE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
 
     // update solver manager
     solver_manager_ptr->update();
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // check stateblock fixed
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
-    ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
-    ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
-TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
     // Local param
     LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
     sb_ptr->setLocalParametrization(local_ptr);
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_TRUE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
     // update solver manager
     solver_manager_ptr->update();
 
     // check stateblock localparam
-    ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+    EXPECT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
 
     // Remove local param
     sb_ptr->removeLocalParametrization();
 
     // check flags
-    ASSERT_FALSE(sb_ptr->stateUpdated());
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_TRUE(sb_ptr->localParamUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
 
     // update solver manager
     solver_manager_ptr->update();
 
     // check stateblock localparam
-    ASSERT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
+    EXPECT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
-TEST(SolverManager, RemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_Remove)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
     // update solver
@@ -274,19 +302,19 @@ TEST(SolverManager, RemoveStateBlock)
     solver_manager_ptr->update();
 
     // check stateblock
-    ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
-TEST(SolverManager, AddRemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddRemove)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
     // remove state_block
@@ -296,38 +324,19 @@ TEST(SolverManager, AddRemoveStateBlock)
     solver_manager_ptr->update();
 
     // check state block
-    ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
-}
-
-TEST(SolverManager, RemoveUpdateStateBlock)
-{
-    ProblemPtr P = Problem::create("PO", 2);
-    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
-
-    // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
-
-    // add state_block
-    P->notifyStateBlock(sb_ptr,ADD);
-
-    // remove state_block
-    P->notifyStateBlock(sb_ptr,REMOVE);
-
-    // update solver
-    solver_manager_ptr->update();
+    EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
-TEST(SolverManager, DoubleRemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_DoubleRemove)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // add stateblock
+    // notify stateblock
     P->notifyStateBlock(sb_ptr,ADD);
 
     // remove state_block
@@ -341,216 +350,734 @@ TEST(SolverManager, DoubleRemoveStateBlock)
 
     // update solver manager
     solver_manager_ptr->update();
+
+    // checks
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
-TEST(SolverManager, AddUpdatedStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddUpdated)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
     // Fix
     sb_ptr->fix();
 
     // Set State
-    Vector2d state_2 = 2*state;
-    sb_ptr->setState(state_2);
+    sb_ptr->setState(2*sb_ptr->getState());
 
     // Check flags have been set true
-    ASSERT_TRUE(sb_ptr->fixUpdated());
-    ASSERT_TRUE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->stateUpdated());
 
     // == When an ADD is notified: a ADD notification should be stored ==
 
-    // add state_block
+    // notify state block
     P->notifyStateBlock(sb_ptr,ADD);
 
     Notification notif;
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
-    ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
-    ASSERT_EQ(notif, ADD);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(notif, ADD);
 
     // == Insert OTHER notifications ==
 
     // Set State --> FLAG
-    state_2 = 2*state;
-    sb_ptr->setState(state_2);
+    sb_ptr->setState(2*sb_ptr->getState());
 
     // Fix --> FLAG
     sb_ptr->unfix();
     // Check flag has been set true
-    ASSERT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
 
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
 
     // == consume empties the notification map ==
     solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
     // Check flags have been reset
-    ASSERT_FALSE(sb_ptr->fixUpdated());
-    ASSERT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
 
     // == When an REMOVE is notified: a REMOVE notification should be stored ==
 
     // remove state_block
     P->notifyStateBlock(sb_ptr,REMOVE);
 
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
-    ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
-    ASSERT_EQ(notif, REMOVE);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(notif, REMOVE);
 
     // == REMOVE + ADD: notification map should be empty ==
     P->notifyStateBlock(sb_ptr,ADD);
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
 
     // == UPDATES should clear the list of notifications ==
-    // add state_block
+    // notify state block
     P->notifyStateBlock(sb_ptr,ADD);
 
     // update solver
     solver_manager_ptr->update();
 
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
 
     // == ADD + REMOVE: notification map should be empty ==
     P->notifyStateBlock(sb_ptr,ADD);
     P->notifyStateBlock(sb_ptr,REMOVE);
-    ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
 }
 
-TEST(SolverManager, AddFactor)
+////////////////////////////////////////////////////////
+// STATE BLOCKS AND FACTOR
+TEST(SolverManager, StateBlockAndFactor_Add)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
 
-    // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceKeyFrame(0, P->stateZero() );
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
 
-    // notification
-    Notification notif;
-    ASSERT_TRUE(P->getFactorNotification(c,notif));
-    ASSERT_EQ(notif, ADD);
+    // notify stateblock (floating for the moment)
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // checks
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+
+    // notify factor (state block now not floating)
+    P->notifyFactor(fac_ptr,ADD);
 
     // update solver
     solver_manager_ptr->update();
 
-    // check factor
-    ASSERT_TRUE(solver_manager_ptr->isFactorRegistered(c));
+    // checks
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
-TEST(SolverManager, RemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
 
-    // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceKeyFrame(0, P->stateZero() );
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+    // update solver
+    solver_manager_ptr->update();
+
+    // notify stateblock again
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
 
     // update solver
     solver_manager_ptr->update();
 
-    // add factor
-    P->notifyFactor(c,REMOVE);
+    // check stateblock
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_Fix)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
 
-    // notification
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock unfixed
+    EXPECT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
+
+    // Fix frame
+    sb_ptr->fix();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // update solver manager
+    solver_manager_ptr->update();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock fixed
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddFixed)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // Fix state block
+    sb_ptr->fix();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // update solver manager
+    solver_manager_ptr->update();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock fixed
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // Local param
+    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+    sb_ptr->setLocalParametrization(local_ptr);
+
+    // Fix state block
+    sb_ptr->fix();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+    // update solver manager
+    solver_manager_ptr->update();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // check stateblock fixed
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // Local param
+    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+    sb_ptr->setLocalParametrization(local_ptr);
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // update solver manager
+    solver_manager_ptr->update();
+
+    // check stateblock localparam
+    EXPECT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+    // Remove local param
+    sb_ptr->removeLocalParametrization();
+
+    // check flags
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->localParamUpdated());
+
+    // update solver manager
+    solver_manager_ptr->update();
+
+    // check stateblock localparam
+    EXPECT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE); // there is afactor involved, removal posponed (REMOVE in notification list)
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // checks
     Notification notif;
-    ASSERT_TRUE(P->getFactorNotification(c,notif));
-    ASSERT_EQ(notif, REMOVE);
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(notif, REMOVE);
+    EXPECT_TRUE(solver_manager_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // update solver
+    solver_manager_ptr->update();
+
+    // remove state_block
+    P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
 
     // update solver
     solver_manager_ptr->update();
 
-    // check factor
-    ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+    // checks
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
-TEST(SolverManager, AddRemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
 
-    // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceKeyFrame(0, P->stateZero() );
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
 
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
 
-    // notification
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
+
+    // update solver
+    solver_manager_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
+
+    // check state block
     Notification notif;
-    ASSERT_TRUE(P->getFactorNotification(c,notif));
-    ASSERT_EQ(notif, ADD);
+    EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(notif, ADD);
+    EXPECT_TRUE(solver_manager_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
 
-    // add factor
-    P->notifyFactor(c,REMOVE);
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
 
-    // notification
-    ASSERT_EQ(P->getFactorNotificationMapSize(), 0); // ADD+REMOVE cancels out
-    ASSERT_FALSE(P->getFactorNotification(c, notif)); // ADD+REMOVE cancels out
+    // remove state_block
+    P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
 
     // update solver
     solver_manager_ptr->update();
 
-    // check factor
-    ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+    // checks
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
-TEST(SolverManager, DoubleRemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
 {
     ProblemPtr P = Problem::create("PO", 2);
     SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
 
     // Create State block
-    Vector2d state; state << 1, 2;
-    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
 
-    // Create (and add) factor point 2d
-    FrameBasePtr        F = P->emplaceKeyFrame(TimeStamp(0), P->stateZero() );
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
 
-    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
-    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
-    auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
 
     // update solver
+    solver_manager_ptr->update(); // factor ADD is posponed
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE); // should be ignored
+
+    // update solver manager
     solver_manager_ptr->update();
 
-    // remove factor
-    P->notifyFactor(c,REMOVE);
+    // check state block
+    Notification notif;
+    EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(notif, ADD);
+    EXPECT_TRUE(solver_manager_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify stateblock
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    // remove state_block
+    P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
+
+    // update solver
+    solver_manager_ptr->update(); // state block should be automatically stored as floating
+
+    // remove state_block
+    P->notifyFactor(fac_ptr,REMOVE); // should be ignored
 
     // update solver
     solver_manager_ptr->update();
 
-    // remove factor
-    P->notifyFactor(c,REMOVE);
+    // checks
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(solver_manager_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // Fix
+    sb_ptr->fix();
+
+    // Change State
+    sb_ptr->setState(2*sb_ptr->getState());
+
+    // Check flags have been set true
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+    EXPECT_TRUE(sb_ptr->stateUpdated());
+
+    // == When an ADD is notified: a ADD notification should be stored ==
+
+    // notify state block
+    P->notifyStateBlock(sb_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,ADD);
+
+    Notification notif;
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(notif, ADD);
+
+    // == Insert OTHER notifications ==
+
+    // Set State --> FLAG
+    sb_ptr->setState(2*sb_ptr->getState());
+
+    // Fix --> FLAG
+    sb_ptr->unfix();
+    // Check flag has been set true
+    EXPECT_TRUE(sb_ptr->fixUpdated());
+
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+    // == consume empties the notification map ==
+    solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+    // Check flags have been reset
+    EXPECT_FALSE(sb_ptr->fixUpdated());
+    EXPECT_FALSE(sb_ptr->stateUpdated());
+
+    // == When an REMOVE is notified: a REMOVE notification should be stored ==
+
+    // remove state_block
+    P->notifyStateBlock(sb_ptr,REMOVE);
+
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+    EXPECT_EQ(notif, REMOVE);
+
+    // == REMOVE + ADD: notification map should be empty ==
+    P->notifyStateBlock(sb_ptr,ADD);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+
+    // == UPDATES should clear the list of notifications ==
+    // notify state block
+    P->notifyStateBlock(sb_ptr,ADD);
 
     // update solver
     solver_manager_ptr->update();
 
-    // check factor
-    ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+
+    // == ADD + REMOVE: notification map should be empty ==
+    P->notifyStateBlock(sb_ptr,ADD);
+    P->notifyStateBlock(sb_ptr,REMOVE);
+    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+}
+
+////////////////////////////////////////////////////////
+// ONLY FACTOR
+TEST(SolverManager, OnlyFactor_Add)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify factor (state block has not been notified)
+    P->notifyFactor(fac_ptr,ADD);
+
+    // update solver
+    solver_manager_ptr->update(); // factor should be put in the notification list again
+
+    // checks
+    Notification notif;
+    EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_EQ(notif, ADD);
+    EXPECT_TRUE(solver_manager_ptr->check());
 }
 
+TEST(SolverManager, OnlyFactor_Remove)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify factor (state block has not been notified)
+    P->notifyFactor(fac_ptr,ADD);
+
+    // update solver
+    solver_manager_ptr->update(); // factor should be put in the notification list again
+
+    // notify factor
+    P->notifyFactor(fac_ptr,REMOVE);
+
+    // update solver
+    solver_manager_ptr->update(); // factor ADD notification should be cancelled
+
+    // checks
+    Notification notif;
+    EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_FALSE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_TRUE(solver_manager_ptr->check());
+}
+
+TEST(SolverManager, OnlyFactor_AddRemove)
+{
+    ProblemPtr P = Problem::create("PO", 2);
+    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+    // Create State block
+    auto sb_ptr = createStateBlock();
+
+    // Create Factor
+    auto fac_ptr = createFactor(sb_ptr);
+
+    // notify factor (state block has not been notified)
+    P->notifyFactor(fac_ptr,ADD);
+
+    // notify factor
+    P->notifyFactor(fac_ptr,REMOVE);
+
+    // update solver
+    solver_manager_ptr->update(); // factor ADD notification should be cancelled
+
+    // checks
+    Notification notif;
+    EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+    EXPECT_FALSE(P->getFactorNotification(fac_ptr, notif));
+    EXPECT_TRUE(solver_manager_ptr->check());
+}
+
+////////////////////////////////////////////////////////
+// MULTITHREADING
+
 /*TEST(SolverManager, MultiThreadingTruncatedNotifications)
 {
     double Dt = 5.0;