Fix getState() that failed when querying non-buffered time stamps

src/processor_motion.cpp

@@ -271,8 +271,16 @@ void ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
     {
         // 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 delta and correct it with new bias
+        VectorXs calib_preint    = capture_motion->getBuffer().getCalibrationPreint();
+        Motion   motion          = capture_motion->getBuffer().getMotion(_ts);
+        VectorXs delta_error     = motion.jacobian_calib_ * (origin_ptr_->getCalibration() - calib_preint);
+        VectorXs delta           = capture_motion->correctDelta( motion.delta_integr_, delta_error);
+
+        // Compose on top of 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