diff --git a/src/problem.cpp b/src/problem.cpp
index 46cdb2eb85d804a215a6d74dd3456142201f40d1..3989d2695e61c95b5b3b1068131f40461c9621ab 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -822,6 +822,12 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
                     }
                     else
                         cout << " -> S-";
+                    if (C->isMotion())
+                    {
+                        auto CM = std::static_pointer_cast<CaptureMotion>(C);
+                        if (CM->getOriginFramePtr())
+                            cout << " -> OF" << CM->getOriginFramePtr()->id();
+                    }
 
                     cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
                     if (constr_by)
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 731cb7f2f635dc405c0e6ff50cb1e06fc1f8d451..cd0a2615486677178ad32b03c3e1152901a034a7 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -267,12 +267,23 @@ void ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
         // We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
         capture_motion = getCaptureMotionContainingTimeStamp(_ts);
 
-    if (capture_motion)
+    if (capture_motion)  // We found a CaptureMotion whose buffer contains the time stamp
     {
-        // We found a CaptureMotion whose buffer contains the time stamp
-        VectorXs state_0 = capture_motion->getOriginFramePtr()->getState();
-        VectorXs delta = capture_motion->getDeltaCorrected(origin_ptr_->getCalibration(), _ts);
-        Scalar dt = _ts - capture_motion->getBuffer().get().front().ts_;
+        // Get origin state and calibration
+        VectorXs state_0          = capture_motion->getOriginFramePtr()->getState();
+        CaptureBasePtr cap_orig   = capture_motion->getOriginFramePtr()->getCaptureOf(getSensorPtr());
+        VectorXs calib            = cap_orig->getCalibration();
+
+        // Get delta and correct it with new bias
+        VectorXs calib_preint     = capture_motion->getBuffer().getCalibrationPreint();
+        Motion   motion           = capture_motion->getBuffer().getMotion(_ts);
+        
+        VectorXs delta_step       = motion.jacobian_calib_ * (calib - calib_preint);
+        VectorXs delta            = capture_motion->correctDelta( motion.delta_integr_, delta_step);
+
+        // Compose on top of origin state using the buffered time stamp, not the query time stamp
+        // TODO Interpolate the delta to produce a state at the query time stamp _ts
+        Scalar dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_; // = _ts - capture_motion->getOriginPtr()->getTimeStamp();
         statePlusDelta(state_0, delta, dt, _x);
     }
     else