diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp index 731cb7f2f635dc405c0e6ff50cb1e06fc1f8d451..dd2104f61922253e81e2a3bcd58cf30789996a29 100644 --- a/src/processor_motion.cpp +++ b/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