From 867e195dcc5bb73d992ff4a52fc4db69bce4f3e0 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Wed, 14 Mar 2018 13:55:11 +0100
Subject: [PATCH] Fix getState() that failed when querying non-buffered time
stamps
---
src/processor_motion.cpp | 12 ++++++++++--
1 file changed, 10 insertions(+), 2 deletions(-)
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 731cb7f2f..dd2104f61 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
--
GitLab