diff --git a/src/problem.cpp b/src/problem.cpp
index 2b752a3697103e30f371b531e283326062989685..8eba70e3f585adb283d479742eb985c61bbcdde9 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -379,8 +379,7 @@ StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
     _state_ptr->addNotification(StateBlock::Notification::ADD);
 
     // Push all notifications from SB to problem
-    for (auto notif : _state_ptr->getNotifications())
-        notifyStateBlock(_state_ptr, notif);
+    notifyStateBlock(_state_ptr);
 
     return _state_ptr;
 }
@@ -394,10 +393,10 @@ void Problem::removeStateBlockPtr(StateBlockPtr _state_ptr)
 
     // Add remove notification
     _state_ptr->addNotification(StateBlock::Notification::REMOVE);
-    notifyStateBlock(_state_ptr, StateBlock::Notification::REMOVE);
+    notifyStateBlock(_state_ptr);
 }
 
-void Problem::notifyStateBlock(StateBlockPtr _state_ptr, const StateBlock::Notification _type)
+void Problem::notifyStateBlock(StateBlockPtr _state_ptr)
 {
     notified_state_block_list_.push_back(_state_ptr);
     notified_state_block_list_.sort();
diff --git a/src/problem.h b/src/problem.h
index d0e71b864b8c06262e43623517721637c418dc56..a8a182a04ba2b7d824663cc8d8320c66db4bddaa 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -273,7 +273,7 @@ class Problem : public std::enable_shared_from_this<Problem>
 
         /** \brief Notify State Block change
          */
-        void notifyStateBlock(StateBlockPtr _state_ptr, const StateBlock::Notification _type);
+        void notifyStateBlock(StateBlockPtr _state_ptr);
 
         /** \brief Gets a list of state blocks which state has been changed to be handled by the solver
          */
diff --git a/src/processor_tracker_landmark_polyline.cpp b/src/processor_tracker_landmark_polyline.cpp
index e95769247f10e5e52219afcbb8692b7aa9c9de51..0475ab31adbb4cb0d5600e52f327b68c926f4c61 100644
--- a/src/processor_tracker_landmark_polyline.cpp
+++ b/src/processor_tracker_landmark_polyline.cpp
@@ -956,8 +956,6 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBaseList& _lmk_
                 // Unfix origin
                 polyline_ptr->getPPtr()->unfix();
                 polyline_ptr->getOPtr()->unfix();
-                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 +987,6 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBaseList& _lmk_
                 for (auto id = polyline_ptr->getFirstId(); id <= polyline_ptr->getLastId(); id++)
                 {
                     polyline_ptr->getPointStateBlockPtr(id)->fix();
-                    getProblem()->updateFixStateBlockPtr(polyline_ptr->getPointStateBlockPtr(id));
                 }
             }
         }
diff --git a/src/state_block.cpp b/src/state_block.cpp
index 2fee2da0e53adf92810f1ef421428410419a70f6..3e53245de9c78b5720d56822c22d4e45447832f0 100644
--- a/src/state_block.cpp
+++ b/src/state_block.cpp
@@ -16,7 +16,7 @@ void StateBlock::setState(const Eigen::VectorXs& _state, const bool _notify)
     {
         addNotification(StateBlock::Notification::UPDATE_STATE);
         if (getProblem() != nullptr)
-            getProblem()->notifyStateBlock(shared_from_this(), StateBlock::Notification::UPDATE_STATE);
+            getProblem()->notifyStateBlock(shared_from_this());
     }
 }
 
@@ -26,7 +26,7 @@ void StateBlock::setFixed(bool _fixed)
     // Notify
     addNotification(StateBlock::Notification::UPDATE_FIX);
     if (getProblem() != nullptr)
-        getProblem()->notifyStateBlock(shared_from_this(), StateBlock::Notification::UPDATE_FIX);
+        getProblem()->notifyStateBlock(shared_from_this());
 }
 
 void StateBlock::addNotification(const StateBlock::Notification _new_notification)
@@ -67,31 +67,9 @@ StateBlock::Notifications StateBlock::consumeNotifications() const
 
 StateBlock::Notifications StateBlock::getNotifications() const
 {
+    std::lock_guard<std::mutex> lock(notifictions_mut_);
     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 08d5af6591ab88de62ae4b0a7e2616a65f0b5351..d2d7c3159d5c0e75d0b7778cbccff4a5601348ac 100644
--- a/src/state_block.h
+++ b/src/state_block.h
@@ -143,10 +143,6 @@ public:
          **/
         StateBlock::Notifications getNotifications() const;
 
-        /** \brief Print list of notifications
-         **/
-        void printNotifications() const;
-
 };
 
 } // namespace wolf