diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 5cb8b42909b9fe93be361983a4f664fef17ce608..366d9e4aa1f9fa59880dc3f175a146577786cc6d 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -96,15 +96,6 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
 
     preProcess(); // Derived class operations
 
-    // integrate data
-    if (last_ptr_ != nullptr)
-    {
-        integrateOneStep();
-    }
-    else {
-        last_nullptr_ = true;
-    }
-
     PackKeyFramePtr pack = computeProcessingStep();
     if (pack)
         buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
@@ -139,6 +130,18 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         case RUNNING_WITH_KF_ON_ORIGIN :
             break;
 
+        default :
+            break;
+    }
+
+
+    // integrate data
+    // Done at this place because setPrior() needs 
+    integrateOneStep();
+
+
+    switch(processing_step_)
+    {
         case RUNNING_WITH_KF_BEFORE_ORIGIN :
         {