diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 1936420a1ad0b160cde194c5e67b2f8f6b159565..6d3c459bb2f01e1f9f5acaa8914042fbc1304526 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -367,30 +367,44 @@ void Problem::getCurrentState(Eigen::VectorXd& _state) const
 
 void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) const
 {   
+    // Prior if not set yet
     if (!isPriorSet() and prior_options_->mode != "nothing")
     {
         _state = prior_options_->state;
         _ts = 0;
+        return;
     }
-    else if (!processor_is_motion_list_.empty())
+
+    // retrieve the minimum of the most recent ts in all processor is motion then call getSate(ts, state)
+    if (!processor_is_motion_list_.empty())
     {
-        // retrieve the minimum of the most recent ts in all processor is motion then call getSate(ts, state)
         std::list<TimeStamp> proc_is_motion_current_ts;
-        for (auto proc: processor_is_motion_list_){
-            proc_is_motion_current_ts.push_back(proc->getCurrentTimeStamp());
+        for (auto proc: processor_is_motion_list_)
+        {
+            auto ts = proc->getCurrentTimeStamp();
+            if (ts != 0) // a not initialized PM provides 0 timestamp FIXME: change by isMotion::ready()?
+                proc_is_motion_current_ts.push_back(ts);
+        }
+        if (!proc_is_motion_current_ts.empty())
+        {
+            auto min_it = std::min_element(proc_is_motion_current_ts.begin(), proc_is_motion_current_ts.end());
+            getState(*min_it, _state);
+            _ts = *min_it;
+            return;
         }
-        auto min_it = std::min_element(proc_is_motion_current_ts.begin(), proc_is_motion_current_ts.end());
-        getState(*min_it, _state);
-        _ts = *min_it;
     }
-    else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
+
+    // Last KF state
+    if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
     {
         // kind of redundant with getState(_ts, _state)
         trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(_ts);
         trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
+        return;
     }
-    else
-        _state = zeroState();
+
+    // zeroState otherwise
+    _state = zeroState();
 }