Consider prior state if nothing else is found in getState()

src/problem/problem.cpp

@@ -461,9 +461,16 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
     }
 
     // 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);
+
+    if (last_kf_or_aux)
+        state_last = last_kf_or_aux->getState(structure);
+
+    else if (prior_options_ and prior_options_->mode != "nothing")
+        state_last = prior_options_->state;
 
     for (const auto& ckey : structure)
     {
@@ -504,9 +511,16 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
     }
 
     // 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);
+
+    if (last_kf_or_aux)
+        state_last = last_kf_or_aux->getState(structure);
+
+    else if (prior_options_ and prior_options_->mode != "nothing")
+        state_last = prior_options_->state;
 
     for (const auto& ckey : structure)
     {