diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 99e6d44b4271a7975e2570a3dc889d2c4d89989b..7bbac43e6aadc040886f8d4ffdcb9b5a023c3e19 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -310,6 +310,7 @@ SET(SRCS_BASE
     processor_tracker_feature.cpp
     processor_tracker_landmark.cpp
     sensor_base.cpp
+    state_block.cpp
     track_matrix.cpp
     time_stamp.cpp
     trajectory_base.cpp
diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index 71a94387deaf352d9f1230de56513de7a470e1dc..e860b5757b882d5382ba0752b9dbe87d42aa89bd 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -182,11 +182,7 @@ void CaptureBase::fix()
     {
         auto sbp = getStateBlockPtr(i);
         if (sbp != nullptr)
-        {
             sbp->fix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
     }
     updateCalibSize();
 }
@@ -197,11 +193,7 @@ void CaptureBase::unfix()
     {
         auto sbp = getStateBlockPtr(i);
         if (sbp != nullptr)
-        {
             sbp->unfix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
     }
     updateCalibSize();
 }
@@ -212,11 +204,7 @@ void CaptureBase::fixExtrinsics()
     {
         auto sbp = getStateBlockPtr(i);
         if (sbp != nullptr)
-        {
             sbp->fix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
     }
     updateCalibSize();
 }
@@ -227,11 +215,7 @@ void CaptureBase::unfixExtrinsics()
     {
         auto sbp = getStateBlockPtr(i);
         if (sbp != nullptr)
-        {
             sbp->unfix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
     }
     updateCalibSize();
 }
@@ -242,11 +226,7 @@ void CaptureBase::fixIntrinsics()
     {
         auto sbp = getStateBlockPtr(i);
         if (sbp != nullptr)
-        {
             sbp->fix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
     }
     updateCalibSize();
 }
@@ -257,11 +237,7 @@ void CaptureBase::unfixIntrinsics()
     {
         auto sbp = getStateBlockPtr(i);
         if (sbp != nullptr)
-        {
             sbp->unfix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
     }
     updateCalibSize();
 }
diff --git a/src/capture_base.h b/src/capture_base.h
index 1d01e42841c91fa84b86ef7d1b21ccbc9a784ff8..3c14dc5e1cd96908f91e45d81763e2191eae5bf1 100644
--- a/src/capture_base.h
+++ b/src/capture_base.h
@@ -59,6 +59,8 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
         void setFramePtr(const FrameBasePtr _frm_ptr);
         void unlinkFromFrame(){frame_ptr_.reset();}
 
+        virtual void setProblem(ProblemPtr _problem) final;
+
         FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
         FeatureBaseList& getFeatureList();
         void addFeatureList(FeatureBaseList& _new_ft_list);
@@ -109,6 +111,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
 #include "sensor_base.h"
 #include "frame_base.h"
 #include "feature_base.h"
+#include "state_block.h"
 
 namespace wolf{
 
@@ -117,6 +120,15 @@ inline SizeEigen CaptureBase::getCalibSize() const
     return calib_size_;
 }
 
+inline void CaptureBase::setProblem(ProblemPtr _problem)
+{
+    NodeBase::setProblem(_problem);
+    for (auto ft : feature_list_)
+        ft->setProblem(_problem);
+    for (auto sb : state_block_vec_)
+        if (sb) sb->setProblem(_problem);
+}
+
 inline void CaptureBase::updateCalibSize()
 {
     calib_size_ = computeCalibSize();
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index e11b7369d8eb3176d992ddbe71ef1403838486ff..36caa290b20d8be1bbe741dab144590c61791031 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -42,7 +42,7 @@ std::string QRManager::solve(const unsigned int& _report_level)
 
     // update state blocks
     for (auto sb_pair : sb_2_col_)
-        sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize()));
+        sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize()), false);
 
     if (_report_level == 1)
         return std::string("Success!\n");
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index 2a027b1a0ef384b2e30da68b84ea16eac62b03cc..d57a9a88f2a5c74a1c38e81fc9a16a59b11d6f3b 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -112,6 +112,13 @@ void FeatureBase::setMeasurementInformation(const Eigen::MatrixXs & _meas_info)
     measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info);
 }
 
+void FeatureBase::setProblem(ProblemPtr _problem)
+{
+    NodeBase::setProblem(_problem);
+    for (auto ctr : constraint_list_)
+        ctr->setProblem(_problem);
+}
+
 Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) const
 {
     // impose symmetry
diff --git a/src/feature_base.h b/src/feature_base.h
index 01cd2fc2e65c7d5f495e0eccb20cf82f17acdbbf..3bd60033a40929c7e6a18705acbd46a2a56a94e3 100644
--- a/src/feature_base.h
+++ b/src/feature_base.h
@@ -48,6 +48,8 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
         virtual ~FeatureBase();
         void remove();
 
+        virtual void setProblem(ProblemPtr _problem) final;
+
         // properties
         unsigned int id();
         unsigned int trackId(){return track_id_;}
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 123c4aac5ada62dd2377924ea136f0e45ead62bd..088e9effe597908d556b1e8a8f8ea48226fe4822 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -139,22 +139,14 @@ void FrameBase::fix()
 {
     for (auto sbp : state_block_vec_)
         if (sbp != nullptr)
-        {
             sbp->fix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
 }
 
 void FrameBase::unfix()
 {
     for (auto sbp : state_block_vec_)
         if (sbp != nullptr)
-        {
             sbp->unfix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
 }
 
 bool FrameBase::isFixed() const
@@ -186,7 +178,7 @@ void FrameBase::setState(const Eigen::VectorXs& _state)
     for (StateBlockPtr sb : state_block_vec_)
         if (sb && (index < _st_size))
         {
-            sb->setState(_state.segment(index, sb->getSize()));
+            sb->setState(_state.segment(index, sb->getSize()), isKey());
             index += sb->getSize();
         }
 }
diff --git a/src/frame_base.h b/src/frame_base.h
index 758f2eef7f05b0015d6e5dd0eb80dc2a6447923d..8f49e9b70a9275b590466fe5edcc728cb731605b 100644
--- a/src/frame_base.h
+++ b/src/frame_base.h
@@ -122,6 +122,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
 
         // Wolf tree access ---------------------------------------------------
     public:
+        virtual void setProblem(ProblemPtr _problem) final;
+
         TrajectoryBasePtr getTrajectoryPtr() const;
         void setTrajectoryPtr(TrajectoryBasePtr _trj_ptr);
 
@@ -258,6 +260,15 @@ inline unsigned int FrameBase::getHits() const
     return constrained_by_list_.size();
 }
 
+inline void FrameBase::setProblem(ProblemPtr _problem)
+{
+    NodeBase::setProblem(_problem);
+    for (auto cap : capture_list_)
+        cap->setProblem(_problem);
+    for (auto sb : state_block_vec_)
+        if (sb) sb->setProblem(_problem);
+}
+
 inline ConstraintBaseList& FrameBase::getConstrainedByList()
 {
     return constrained_by_list_;
diff --git a/src/landmark_base.cpp b/src/landmark_base.cpp
index 8ff4ae919211cf0ec4bc7328f44bd9abcc3b1fe9..b84c078162635f2320abe46abd6e4c8a3c93cc61 100644
--- a/src/landmark_base.cpp
+++ b/src/landmark_base.cpp
@@ -18,6 +18,7 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
 {
     state_block_vec_[0] = _p_ptr;
     state_block_vec_[1] = _o_ptr;
+
 //    std::cout << "constructed  +L" << id() << std::endl;
 }
                 
@@ -54,22 +55,14 @@ void LandmarkBase::fix()
 {
     for (auto sbp : state_block_vec_)
         if (sbp != nullptr)
-        {
             sbp->fix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
 }
 
 void LandmarkBase::unfix()
 {
     for (auto sbp : state_block_vec_)
         if (sbp != nullptr)
-        {
             sbp->unfix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
 }
 
 bool LandmarkBase::isFixed() const
diff --git a/src/landmark_base.h b/src/landmark_base.h
index 50a5608f97adcb35208f0dd82e79848a8c3bfe91..5bc32c90083aacb06edf3ce160c54bdc90334c76 100644
--- a/src/landmark_base.h
+++ b/src/landmark_base.h
@@ -88,6 +88,8 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
 
 
         // Navigate wolf tree
+        virtual void setProblem(ProblemPtr _problem) final;
+
         ConstraintBasePtr addConstrainedBy(ConstraintBasePtr _ctr_ptr);
         unsigned int getHits() const;
         ConstraintBaseList& getConstrainedByList();
@@ -105,6 +107,13 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
 
 namespace wolf{
 
+inline void LandmarkBase::setProblem(ProblemPtr _problem)
+{
+    NodeBase::setProblem(_problem);
+    for (auto sb : state_block_vec_)
+        if (sb) sb->setProblem(_problem);
+}
+
 inline MapBasePtr LandmarkBase::getMapPtr()
 {
     return map_ptr_.lock();
diff --git a/src/node_base.h b/src/node_base.h
index 6fc308280b803ffab21949f587ad50c8504dc78b..7ccb3bff2f53e13700b51fe2cfdbc9377e601527 100644
--- a/src/node_base.h
+++ b/src/node_base.h
@@ -81,7 +81,7 @@ class NodeBase
         void setName(const std::string& _name);
 
         ProblemPtr getProblem() const;
-        void setProblem(ProblemPtr _prob_ptr);
+        virtual void setProblem(ProblemPtr _prob_ptr);
 };
 
 } // namespace wolf
@@ -132,12 +132,12 @@ inline void NodeBase::setName(const std::string& _name)
 
 inline ProblemPtr NodeBase::getProblem() const
 {
-  return problem_ptr_.lock();
+    return problem_ptr_.lock();
 }
 
 inline void NodeBase::setProblem(ProblemPtr _prob_ptr)
 {
-  problem_ptr_ = _prob_ptr;
+    problem_ptr_ = _prob_ptr;
 }
 
 } // namespace wolf
diff --git a/src/problem.cpp b/src/problem.cpp
index 5e646aef19ee5e8f2e91c9fd47343c142f81f043..2b752a3697103e30f371b531e283326062989685 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -369,27 +369,20 @@ StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
 {
     //std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
 
+    // set problem
+    _state_ptr->setProblem(shared_from_this());
+
     // add the state unit to the list
     state_block_list_.push_back(_state_ptr);
 
-    // queue for solver manager
+    // Add add notification
     _state_ptr->addNotification(StateBlock::Notification::ADD);
-    notified_state_block_list_.push_back(_state_ptr);
-    notified_state_block_list_.sort();
-    notified_state_block_list_.unique();
 
-    return _state_ptr;
-}
+    // Push all notifications from SB to problem
+    for (auto notif : _state_ptr->getNotifications())
+        notifyStateBlock(_state_ptr, notif);
 
-void Problem::updateStateBlockPtr(StateBlockPtr _state_ptr)
-{
-    //std::cout << "Problem::updateStateBlockPtr " << _state_ptr.get() << std::endl;
-
-    // queue for solver manager
-    _state_ptr->addNotification(StateBlock::Notification::FIX_UPDATE);
-    notified_state_block_list_.push_back(_state_ptr);
-    notified_state_block_list_.sort();
-    notified_state_block_list_.unique();
+    return _state_ptr;
 }
 
 void Problem::removeStateBlockPtr(StateBlockPtr _state_ptr)
@@ -401,6 +394,11 @@ void Problem::removeStateBlockPtr(StateBlockPtr _state_ptr)
 
     // Add remove notification
     _state_ptr->addNotification(StateBlock::Notification::REMOVE);
+    notifyStateBlock(_state_ptr, StateBlock::Notification::REMOVE);
+}
+
+void Problem::notifyStateBlock(StateBlockPtr _state_ptr, const StateBlock::Notification _type)
+{
     notified_state_block_list_.push_back(_state_ptr);
     notified_state_block_list_.sort();
     notified_state_block_list_.unique();
@@ -1367,4 +1365,6 @@ void Problem::print(const std::string& depth, bool constr_by, bool metric, bool
         print(0, constr_by, metric, state_blocks);
 }
 
+
+
 } // namespace wolf
diff --git a/src/problem.h b/src/problem.h
index 87ae8f4bdad357543d057f17d76a7829e74e9c10..d0e71b864b8c06262e43623517721637c418dc56 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -16,6 +16,7 @@ struct ProcessorParamsBase;
 //wolf includes
 #include "wolf.h"
 #include "frame_base.h"
+#include "state_block.h"
 
 // std includes
 
@@ -266,14 +267,14 @@ class Problem : public std::enable_shared_from_this<Problem>
          */
         StateBlockPtr addStateBlock(StateBlockPtr _state_ptr);
 
-        /** \brief Adds a new state block to be updated to solver manager
-         */
-        void updateStateBlockPtr(StateBlockPtr _state_ptr);
-
         /** \brief Adds a state block to be removed to solver manager
          */
         void removeStateBlockPtr(StateBlockPtr _state_ptr);
 
+        /** \brief Notify State Block change
+         */
+        void notifyStateBlock(StateBlockPtr _state_ptr, const StateBlock::Notification _type);
+
         /** \brief Gets a list of state blocks which state has been changed to be handled by the solver
          */
         StateBlockList& getNotifiedStateBlockList();
diff --git a/src/processor_tracker_landmark_polyline.cpp b/src/processor_tracker_landmark_polyline.cpp
index 00a4699ecc7c391a9b1c4b5aad2ad32f630df69e..e95769247f10e5e52219afcbb8692b7aa9c9de51 100644
--- a/src/processor_tracker_landmark_polyline.cpp
+++ b/src/processor_tracker_landmark_polyline.cpp
@@ -956,8 +956,8 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBaseList& _lmk_
                 // Unfix origin
                 polyline_ptr->getPPtr()->unfix();
                 polyline_ptr->getOPtr()->unfix();
-                getProblem()->updateStateBlockPtr(polyline_ptr->getPPtr());
-                getProblem()->updateStateBlockPtr(polyline_ptr->getOPtr());
+                getProblem()->updateFixStateBlockPtr(polyline_ptr->getPPtr());
+                getProblem()->updateFixStateBlockPtr(polyline_ptr->getOPtr());
 
                 // Move origin to B
                 polyline_ptr->getPPtr()->setState(polyline_ptr->getPointVector((configuration ? B_id : A_id)));
@@ -989,7 +989,7 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBaseList& _lmk_
                 for (auto id = polyline_ptr->getFirstId(); id <= polyline_ptr->getLastId(); id++)
                 {
                     polyline_ptr->getPointStateBlockPtr(id)->fix();
-                    getProblem()->updateStateBlockPtr(polyline_ptr->getPointStateBlockPtr(id));
+                    getProblem()->updateFixStateBlockPtr(polyline_ptr->getPointStateBlockPtr(id));
                 }
             }
         }
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 801922ff6acc3d43d44617a99563907a8dd6aabe..fbd05692eb2b7664f505f7bcccd8ef041952fe4e 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -111,11 +111,7 @@ void SensorBase::fix()
 {
     for( auto sbp : state_block_vec_)
         if (sbp != nullptr)
-        {
             sbp->fix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
     updateCalibSize();
 }
 
@@ -123,11 +119,7 @@ void SensorBase::unfix()
 {
     for( auto sbp : state_block_vec_)
         if (sbp != nullptr)
-        {
             sbp->unfix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
     updateCalibSize();
 }
 
@@ -137,11 +129,7 @@ void SensorBase::fixExtrinsics()
     {
         auto sbp = state_block_vec_[i];
         if (sbp != nullptr)
-        {
             sbp->fix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
     }
     updateCalibSize();
 }
@@ -152,11 +140,7 @@ void SensorBase::unfixExtrinsics()
     {
         auto sbp = state_block_vec_[i];
         if (sbp != nullptr)
-        {
             sbp->unfix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
     }
     updateCalibSize();
 }
@@ -167,11 +151,7 @@ void SensorBase::fixIntrinsics()
     {
         auto sbp = state_block_vec_[i];
         if (sbp != nullptr)
-        {
             sbp->fix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
     }
     updateCalibSize();
 }
@@ -182,11 +162,7 @@ void SensorBase::unfixIntrinsics()
     {
         auto sbp = state_block_vec_[i];
         if (sbp != nullptr)
-        {
             sbp->unfix();
-            if (getProblem() != nullptr)
-                getProblem()->updateStateBlockPtr(sbp);
-        }
     }
     updateCalibSize();
 }
@@ -374,4 +350,13 @@ StateBlockPtr SensorBase::getStateBlockPtrDynamic(unsigned int _i, const TimeSta
         return getStateBlockPtrStatic(_i);
 }
 
+void SensorBase::setProblem(ProblemPtr _problem)
+{
+    NodeBase::setProblem(_problem);
+    for (auto prc : processor_list_)
+        prc->setProblem(_problem);
+    for (auto sb : state_block_vec_)
+        if (sb) sb->setProblem(_problem);
+}
+
 } // namespace wolf
diff --git a/src/sensor_base.h b/src/sensor_base.h
index f10b547f321e62d123f12ab6520d379e3df69dc8..3c740bc88233645d9d3d6767e65c048267761acc 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -95,6 +95,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
 
         unsigned int id();
 
+        virtual void setProblem(ProblemPtr _problem) final;
+
         HardwareBasePtr getHardwarePtr();
         void setHardwarePtr(const HardwareBasePtr _hw_ptr);
 
@@ -163,8 +165,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
 
 #include "problem.h"
 #include "hardware_base.h"
-#include "processor_base.h"
 #include "capture_base.h"
+#include "processor_base.h"
 
 namespace wolf{
 
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 2914a4fc7216974f2701736d2ecb223dcfde3b1a..aec47485db1277e9ec150ddeeed5b33f2773419c 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -40,8 +40,6 @@ void SolverManager::update()
       {
         const bool registered = state_blocks_.find(state)!=state_blocks_.end();
 
-        //        const auto p = state_blocks_.emplace(state, state->getState());
-
         // call addStateBlock only if first time added.
         if (!registered)
         {
@@ -53,20 +51,21 @@ void SolverManager::update()
 
         break;
       }
-//      case StateBlock::Notification::STATE_UPDATE:
-//      {
-//        WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
-//                        "Updating the state of an unregistered StateBlock !");
-//
-//        assert(state_blocks_.find(state)!=state_blocks_.end() &&
-//            "Updating the state of an unregistered StateBlock !");
-//
-//        Eigen::VectorXs new_state = state->getState();
-//        std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state));
-//
-//        break;
-//      }
-      case StateBlock::Notification::FIX_UPDATE:
+      case StateBlock::Notification::UPDATE_STATE:
+      {
+        WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
+                        "Updating the state of an unregistered StateBlock !");
+
+        assert(state_blocks_.find(state)!=state_blocks_.end() &&
+            "Updating the state of an unregistered StateBlock !");
+
+        Eigen::VectorXs new_state = state->getState();
+        // We assume the same size for the states in both WOLF and the solver.
+        std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state));
+
+        break;
+      }
+      case StateBlock::Notification::UPDATE_FIX:
       {
         WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(),
                         "Updating the fix state of an unregistered StateBlock !");
@@ -156,7 +155,7 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
   {
     // Avoid usuless copies
     if (!it->first->isFixed())
-      it->first->setState(it->second);
+      it->first->setState(it->second, false);
   }
 
   return report;
diff --git a/src/state_block.cpp b/src/state_block.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2fee2da0e53adf92810f1ef421428410419a70f6
--- /dev/null
+++ b/src/state_block.cpp
@@ -0,0 +1,97 @@
+#include "state_block.h"
+namespace wolf
+{
+
+void StateBlock::setState(const Eigen::VectorXs& _state, const bool _notify)
+{
+    assert(_state.size() == state_.size());
+    {
+        std::lock_guard<std::mutex> lock(mut_state_);
+        state_ = _state;
+        state_size_ = state_.size();
+    }
+
+    // Notify
+    if (_notify)
+    {
+        addNotification(StateBlock::Notification::UPDATE_STATE);
+        if (getProblem() != nullptr)
+            getProblem()->notifyStateBlock(shared_from_this(), StateBlock::Notification::UPDATE_STATE);
+    }
+}
+
+void StateBlock::setFixed(bool _fixed)
+{
+    fixed_.store(_fixed);
+    // Notify
+    addNotification(StateBlock::Notification::UPDATE_FIX);
+    if (getProblem() != nullptr)
+        getProblem()->notifyStateBlock(shared_from_this(), StateBlock::Notification::UPDATE_FIX);
+}
+
+void StateBlock::addNotification(const StateBlock::Notification _new_notification)
+{
+    std::lock_guard<std::mutex> lock(notifictions_mut_);
+    if (_new_notification == Notification::ADD)
+    {
+        // When an ADD arrives, the state is already the newest,
+        // thus old instructions can be cleared
+        if (shared_from_this()->notifications_.size() > 0)
+            notifications_.clear();
+
+        // Push ADD notification to the front
+        notifications_.emplace_front(Notification::ADD);
+    }
+    else if (_new_notification == Notification::REMOVE)
+    {
+        // If we want to remove a block that still has an ADD instruction
+        // we can just clear all notifications and just keep the remove
+        if (!notifications_.empty() && notifications_.front() == Notification::ADD)
+            notifications_.clear();
+        else
+        {
+            notifications_.clear();
+            notifications_.emplace_back(Notification::REMOVE);
+        }
+    }
+    else
+        // UPDATE_FIX; UPDATE_STATE
+        notifications_.emplace_back(_new_notification);
+}
+
+StateBlock::Notifications StateBlock::consumeNotifications() const
+{
+    std::lock_guard<std::mutex> lock(notifictions_mut_);
+    return std::move(notifications_);
+}
+
+StateBlock::Notifications StateBlock::getNotifications() const
+{
+    return notifications_;
+}
+
+void StateBlock::printNotifications() const
+{
+    WOLF_TRACE("SB Notifications for: ", shared_from_this())
+    for (auto notif : notifications_)
+    {
+        switch (notif)
+        {
+            case Notification::ADD:
+                WOLF_TRACE("   ADD")
+                break;
+            case Notification::REMOVE:
+                WOLF_TRACE("   REMOVE")
+                break;
+            case Notification::UPDATE_FIX:
+                WOLF_TRACE("   UPDATE_FIX")
+                break;
+            case Notification::UPDATE_STATE:
+                WOLF_TRACE("   UPDATE_STATE")
+                break;
+        }
+    }
+
+}
+
+}
diff --git a/src/state_block.h b/src/state_block.h
index 6f99144e66cb784d9bb587163c779487ba4173e3..08d5af6591ab88de62ae4b0a7e2616a65f0b5351 100644
--- a/src/state_block.h
+++ b/src/state_block.h
@@ -28,16 +28,16 @@ namespace wolf {
  *     - Non-fixed state blocks are estimated.
  *  - A local parametrization useful for optimizing in the tangent space to the manifold.
  */
-class StateBlock
+class StateBlock : public std::enable_shared_from_this<StateBlock>
 {
 public:
 
   enum class Notification : std::size_t
   {
-    ADD = 0,
+    ADD = 1,
     REMOVE,
-    STATE_UPDATE,
-    FIX_UPDATE
+    UPDATE_STATE,
+    UPDATE_FIX
   };
 
   using Notifications = std::list<Notification>;
@@ -47,7 +47,7 @@ public:
         mutable Notifications notifications_;
         mutable std::mutex notifictions_mut_;
 
-        NodeBaseWPtr node_ptr_; //< pointer to the wolf Node owning this StateBlock
+        ProblemWPtr  problem_ptr_; ///< pointer to the wolf problem
 
         std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not
 
@@ -89,7 +89,15 @@ public:
 
         /** \brief Sets the state vector
          **/
-        void setState(const Eigen::VectorXs& _state);
+        void setState(const Eigen::VectorXs& _state, const bool _notify = true);
+
+        /** \brief Set the pointer to Problem
+         */
+        void setProblem(const ProblemPtr _problem);
+
+        /** \brief Return the problem pointer
+         */
+        ProblemPtr getProblem();
 
         /** \brief Returns the state size
          **/
@@ -127,19 +135,31 @@ public:
 
         StateBlock::Notifications consumeNotifications() const;
 
+        /** \brief Check if exist any notification
+         **/
         bool hasNotifications() const;
+
+        /** \brief Return list of notifications
+         **/
+        StateBlock::Notifications getNotifications() const;
+
+        /** \brief Print list of notifications
+         **/
+        void printNotifications() const;
+
 };
 
 } // namespace wolf
 
 // IMPLEMENTATION
+#include "problem.h"
 #include "local_parametrization_base.h"
+#include "node_base.h"
 
 namespace wolf {
 
 inline StateBlock::StateBlock(const Eigen::VectorXs& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
 //        notifications_{Notification::ADD},
-        node_ptr_(), // nullptr
         fixed_(_fixed),
         state_size_(_state.size()),
         state_(_state),
@@ -150,7 +170,6 @@ inline StateBlock::StateBlock(const Eigen::VectorXs& _state, bool _fixed, LocalP
 
 inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
 //        notifications_{Notification::ADD},
-        node_ptr_(), // nullptr
         fixed_(_fixed),
         state_size_(_size),
         state_(Eigen::VectorXs::Zero(_size)),
@@ -171,19 +190,6 @@ inline Eigen::VectorXs StateBlock::getState() const
     return state_;
 }
 
-inline void StateBlock::setState(const Eigen::VectorXs& _state)
-{
-    assert(_state.size() == state_.size());
-
-    {
-      std::lock_guard<std::mutex> lock(mut_state_);
-      state_ = _state;
-      state_size_ = state_.size();
-    }
-
-    //addNotification(Notification::STATE_UPDATE);
-}
-
 inline SizeEigen StateBlock::getSize() const
 {
     return state_size_.load();
@@ -211,12 +217,6 @@ inline void StateBlock::unfix()
     setFixed(false);
 }
 
-inline void StateBlock::setFixed(bool _fixed)
-{
-    fixed_.store(_fixed);
-    addNotification(Notification::FIX_UPDATE);
-}
-
 inline bool StateBlock::hasLocalParametrization() const
 {
     return (local_param_ptr_ != nullptr);
@@ -239,16 +239,14 @@ inline void StateBlock::setLocalParametrizationPtr(LocalParametrizationBasePtr _
     local_param_ptr_ = _local_param;
 }
 
-inline void StateBlock::addNotification(const StateBlock::Notification _new_notification)
+inline void StateBlock::setProblem(const ProblemPtr _problem)
 {
-  std::lock_guard<std::mutex> lock(notifictions_mut_);
-  notifications_.emplace_back(_new_notification);
+    problem_ptr_ = _problem;
 }
 
-inline StateBlock::Notifications StateBlock::consumeNotifications() const
+inline ProblemPtr StateBlock::getProblem()
 {
-  std::lock_guard<std::mutex> lock(notifictions_mut_);
-  return std::move(notifications_);
+    return problem_ptr_.lock();
 }
 
 inline bool StateBlock::hasNotifications() const
diff --git a/src/test/gtest_ceres_manager.cpp b/src/test/gtest_ceres_manager.cpp
index 8f1466cb45d4d6844b91dd8d974c91846d563390..07ef3ab49101491c3396921f1907cb7dbe17dfb0 100644
--- a/src/test/gtest_ceres_manager.cpp
+++ b/src/test/gtest_ceres_manager.cpp
@@ -135,9 +135,6 @@ TEST(CeresManager, UpdateStateBlock)
     // Fix frame
     sb_ptr->fix();
 
-    // update stateblock
-    P->updateStateBlockPtr(sb_ptr);
-
     // update solver manager
     ceres_manager_ptr->update();
 
@@ -160,9 +157,6 @@ TEST(CeresManager, AddUpdateStateBlock)
     // Fix state block
     sb_ptr->fix();
 
-    // update stateblock
-    P->updateStateBlockPtr(sb_ptr);
-
     // update solver manager
     ceres_manager_ptr->update();
 
@@ -244,17 +238,6 @@ TEST(CeresManager, RemoveUpdateStateBlock)
 
     // update solver
     ceres_manager_ptr->update();
-
-    // Fix state block
-    sb_ptr->fix();
-
-    ASSERT_DEATH({
-        // update stateblock
-        P->updateStateBlockPtr(sb_ptr);
-
-        // update solver manager
-        ceres_manager_ptr->update();
-    },"");
 }
 
 TEST(CeresManager, DoubleRemoveStateBlock)
diff --git a/src/test/gtest_solver_manager.cpp b/src/test/gtest_solver_manager.cpp
index 7543ea18155e7fc80a79e079d3ac35d343335634..f2aee334e543ab02507c302d44a010555a70e4f1 100644
--- a/src/test/gtest_solver_manager.cpp
+++ b/src/test/gtest_solver_manager.cpp
@@ -80,7 +80,7 @@ TEST(SolverManager, Create)
     ProblemPtr P = Problem::create("PO 2D");
     SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
 
-    // check double ointers to branches
+    // check double pointers to branches
     ASSERT_EQ(P, solver_manager_ptr->getProblemPtr());
 }
 
@@ -149,9 +149,6 @@ TEST(SolverManager, UpdateStateBlock)
     // Fix frame
     sb_ptr->fix();
 
-    // update stateblock
-    P->updateStateBlockPtr(sb_ptr);
-
     // update solver manager
     solver_manager_ptr->update();
 
@@ -174,9 +171,6 @@ TEST(SolverManager, AddUpdateStateBlock)
     // Fix state block
     sb_ptr->fix();
 
-    // update stateblock
-    P->updateStateBlockPtr(sb_ptr);
-
     // update solver manager
     solver_manager_ptr->update();
 
@@ -228,7 +222,7 @@ TEST(SolverManager, AddRemoveStateBlock)
     // update solver
     solver_manager_ptr->update();
 
-    // check stateblock
+    // check state block
     ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
 }
 
@@ -241,7 +235,7 @@ TEST(SolverManager, RemoveUpdateStateBlock)
     Vector2s state; state << 1, 2;
     StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
 
-    // add stateblock
+    // add state_block
     P->addStateBlock(sb_ptr);
 
     // remove state_block
@@ -249,17 +243,6 @@ TEST(SolverManager, RemoveUpdateStateBlock)
 
     // update solver
     solver_manager_ptr->update();
-
-    // Fix state block
-    sb_ptr->fix();
-
-    ASSERT_DEATH({
-        // update stateblock
-        P->updateStateBlockPtr(sb_ptr);
-
-        // update solver manager
-        solver_manager_ptr->update();
-    },"");
 }
 
 TEST(SolverManager, DoubleRemoveStateBlock)
@@ -287,6 +270,96 @@ TEST(SolverManager, DoubleRemoveStateBlock)
     solver_manager_ptr->update();
 }
 
+TEST(SolverManager, AddUpdatedStateBlock)
+{
+    ProblemPtr P = Problem::create("PO 2D");
+    SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
+
+    // Create State block
+    Vector2s state; state << 1, 2;
+    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+
+    // Fix
+    sb_ptr->fix();
+
+    // Set State
+    Vector2s state_2 = 2*state;
+    sb_ptr->setState(state_2);
+
+    // == First add 2 UPDATES (FIX and STATE) ==
+
+    // The expected order is:
+    // ADD in front of the list
+    // Others in historical order
+    ASSERT_EQ(sb_ptr->getNotifications().size(),2); // UPDATE_FIX, UPDATE_STATE
+    StateBlock::Notifications sb_notif = sb_ptr->getNotifications();
+    StateBlock::Notifications::iterator iter = sb_notif.begin();
+    ASSERT_EQ(*iter,StateBlock::Notification::UPDATE_FIX);
+    iter++;
+    ASSERT_EQ(*iter,StateBlock::Notification::UPDATE_STATE);
+
+    // == When an ADD is notified, all previous notifications should be cleared (if not consumption of notifs) ==
+
+    // add state_block
+    P->addStateBlock(sb_ptr);
+
+    // Get list of SB notifications
+    StateBlockList P_notif_sb = P->getNotifiedStateBlockList();
+
+    ASSERT_EQ(P_notif_sb.size(),1);
+
+    sb_notif = P_notif_sb.front()->getNotifications();
+
+    ASSERT_EQ(sb_notif.size(),1);
+    ASSERT_EQ(sb_notif.front(),StateBlock::Notification::ADD);
+
+    // == Insert OTHER notifications ==
+
+    // Set State
+    state_2 = 2*state;
+    sb_ptr->setState(state_2);
+
+    // Fix
+    sb_ptr->unfix();
+
+    sb_notif = P_notif_sb.front()->getNotifications();
+    ASSERT_EQ(sb_notif.size(),3); // ADD, UPDATE_STATE, UPDATE_FIX
+
+    iter = sb_notif.begin();
+    ASSERT_EQ(*iter,StateBlock::Notification::ADD);
+    iter++;
+    ASSERT_EQ(*iter,StateBlock::Notification::UPDATE_STATE);
+    iter++;
+    ASSERT_EQ(*iter,StateBlock::Notification::UPDATE_FIX);
+
+    // == REMOVE should clear all previous notifications in the stack because there's an ADD ==
+
+    // remove state_block
+    P->removeStateBlockPtr(sb_ptr);
+
+    P_notif_sb = P->getNotifiedStateBlockList();
+    ASSERT_EQ(P_notif_sb.size(),1);
+
+    sb_notif = P_notif_sb.front()->getNotifications();
+    ASSERT_EQ(sb_notif.size(),0); // ADD + REMOVE = EMPTY
+
+    // == UPDATES + REMOVE should clear the list of notifications ==
+
+    // Fix
+    sb_ptr->fix();
+
+    // Set State
+    state_2 = 2*state;
+    sb_ptr->setState(state_2);
+
+    // remove state_block
+    P->removeStateBlockPtr(sb_ptr);
+
+    sb_notif = P_notif_sb.front()->getNotifications();
+    ASSERT_EQ(sb_notif.size(),1);
+    ASSERT_EQ(sb_notif.front(),StateBlock::Notification::REMOVE);
+}
+
 TEST(SolverManager, AddConstraint)
 {
     ProblemPtr P = Problem::create("PO 2D");
diff --git a/src/test/gtest_trajectory.cpp b/src/test/gtest_trajectory.cpp
index 69a23a5bd184bc8b5f69e7885c2a69bb3b28a318..17e0667475716d16b694ee8c2b3d8139e91035af 100644
--- a/src/test/gtest_trajectory.cpp
+++ b/src/test/gtest_trajectory.cpp
@@ -46,11 +46,11 @@ struct DummyNotificationProcessor
           {
             break;
           }
-          case StateBlock::Notification::STATE_UPDATE:
+          case StateBlock::Notification::UPDATE_STATE:
           {
             break;
           }
-          case StateBlock::Notification::FIX_UPDATE:
+          case StateBlock::Notification::UPDATE_FIX:
           {
             break;
           }