diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index c793cd3e17257c5c207b37fd86d0675b9a0926a5..8144a15ad78b814f18637667402f018ddf85300a 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -446,39 +446,40 @@ TimeStamp Problem::getTimeStamp ( ) const
 VectorComposite Problem::getState(const StateStructure& _structure) const
 {
     StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+
     VectorComposite state;
 
-    if (not processor_is_motion_list_.empty() ) // Compose from different processor motion
+    // compose the states of all processor motions into one only state
+    for (const auto& prc : processor_is_motion_list_)
     {
-        // compose the states of all processor motions into one only state
-        for (const auto& prc : processor_is_motion_list_)
+        const auto& prc_state = prc->getState();
+        for (const auto& pair_key_vec : prc_state)
         {
-            const auto& prc_state = prc->getState();
-            for (const auto& pair_key_vec : prc_state)
-            {
-                if (state.count(pair_key_vec.first) == 0) // only add those keys that do not exist yet
-                    state.insert(pair_key_vec);
-            }
+            if (state.count(pair_key_vec.first) == 0) // only add those keys that do not exist yet
+                state.insert(pair_key_vec);
         }
+    }
 
-        // check for empty blocks and fill them with zeros
-        for (const auto& ckey : frame_structure_)
+    // check for empty blocks and fill them with the last KF, or with zeros in the worst case
+    VectorComposite state_last;
+    const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
+    if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure);
+
+    for (const auto& ckey : structure)
+    {
+        const auto& key = string(1,ckey);
+        if (state.count(key) == 0)
         {
-            const auto& key = string(1,ckey);
-            if (state.count(key) == 0)
+            auto state_last_it = state_last.find(key);
+
+            if (state_last_it != state_last.end())
+                state.emplace(key, state_last_it->second);
+
+            else
                 state.emplace(key, stateZero(key).at(key));
         }
     }
 
-    else // Use last estimated frame's state
-    {
-        auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
-        if (last_kf_or_aux)
-            state = last_kf_or_aux->getState(structure);
-        else
-            state = stateZero(structure);
-    }
-
     return state;
 }
 
@@ -488,42 +489,37 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
 
     VectorComposite state;
 
-    if (not processor_is_motion_list_.empty() )
+    for (const auto& prc : processor_is_motion_list_)
     {
-        for (const auto& prc : processor_is_motion_list_)
-        {
-            const auto& prc_state = prc->getState(_ts);
+        const auto& prc_state = prc->getState(_ts);
 
-            // transfer processor vector blocks to problem state
-            for (const auto& pair_key_vec : prc_state)
-            {
-                const auto& key = pair_key_vec.first;
-
-                if (state.count(key) == 0) // Only write once. This gives priority to processors upfront in the list
-                    state.insert(pair_key_vec);
-            }
-        }
-
-        // check for empty blocks and fill them with zeros
-        for (const auto& ckey : frame_structure_)
+        // transfer processor vector blocks to problem state
+        for (const auto& pair_key_vec : prc_state)
         {
-            const auto& key = string(1,ckey);
-            if (state.count(key) == 0)
-                state.emplace(key, stateZero(key).at(key));
-        }
+            const auto& key = pair_key_vec.first;
 
+            if (state.count(key) == 0) // Only write once. This gives priority to processors upfront in the list
+                state.insert(pair_key_vec);
+        }
     }
 
-    else // Use last estimated frame's state
+    // check for empty blocks and fill them with the closest KF to ts, or with zeros in the worst case
+    VectorComposite state_last;
+    const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+    if (last_kf_or_aux) state_last = last_kf_or_aux->getState(structure);
+
+    for (const auto& ckey : structure)
     {
-        const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
-        if (last_kf_or_aux)
-        {
-            state = last_kf_or_aux->getState(structure);
-        }
-        else
+        const auto& key = string(1,ckey);
+        if (state.count(key) == 0)
         {
-            state = stateZero(structure);
+            auto state_last_it = state_last.find(key);
+
+            if (state_last_it != state_last.end())
+                state.emplace(key, state_last_it->second);
+
+            else
+                state.emplace(key, stateZero(key).at(key));
         }
     }