Skip to content
Snippets Groups Projects
Commit 62558489 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Improve code clarity in getDeltaCorrected()

parent 5e7679f0
No related branches found
No related tags found
1 merge request!145Imu improvements
......@@ -55,22 +55,23 @@ CaptureMotion::~CaptureMotion()
Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current)
{
VectorXs calib_preint = getCalibrationPreint();
VectorXs delta_preint = getBuffer().get().back().delta_integr_;
MatrixXs jac_calib = getBuffer().get().back().jacobian_calib_;
VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
VectorXs delta = correctDelta(delta_preint, delta_error);
return delta;
VectorXs calib_preint = getCalibrationPreint();
VectorXs delta_preint = getBuffer().get().back().delta_integr_;
MatrixXs jac_calib = getBuffer().get().back().jacobian_calib_;
VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
VectorXs delta_corrected = correctDelta(delta_preint, delta_error);
return delta_corrected;
}
Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts)
{
VectorXs calib_preint = getCalibrationPreint();
VectorXs delta_preint = getBuffer().getMotion(_ts).delta_integr_;
MatrixXs jac_calib = getBuffer().getMotion(_ts).jacobian_calib_;
VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
VectorXs delta = correctDelta(delta_preint, delta_error);
return delta;
VectorXs calib_preint = getBuffer().getCalibrationPreint();
Motion motion = getBuffer().getMotion(_ts);
VectorXs delta_preint = motion.delta_integr_;
MatrixXs jac_calib = motion.jacobian_calib_;
VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
VectorXs delta_corrected = correctDelta(delta_preint, delta_error);
return delta_corrected;
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment