From 27929ca13124d1eed5d04003a61ac9a0820c6b17 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 6 Aug 2022 21:53:52 +0200
Subject: [PATCH] Add unmeasured covariance to the features
---
.../processor_force_torque_inertial_preint_dynamics.cpp | 6 ++++--
1 file changed, 4 insertions(+), 2 deletions(-)
diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
index 992a184..a0d8065 100644
--- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
@@ -75,13 +75,15 @@ CaptureMotionPtr ProcessorForceTorqueInertialPreintDynamics::emplaceCapture(cons
return capture;
}
+
FeatureBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFeature(CaptureMotionPtr _capture_own)
{
+
auto motion = _capture_own->getBuffer().back();
FeatureBasePtr ftr = FeatureBase::emplace<FeatureMotion>(_capture_own,
"FeatureMotion",
motion.delta_integr_,
- motion.delta_integr_cov_,
+ motion.delta_integr_cov_ + unmeasured_perturbation_cov_,
_capture_own->getCalibrationPreint(),
motion.jacobian_calib_);
return ftr;
@@ -344,7 +346,7 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
// !!!!!! Now J_delta_model seems to be calculated in function tangent2delta as J_delta_tangent * J_tangent_model
// !!!!!!!!!!!
// TODO: Check the calculation od the J_delta_model in fti::tangent2delta()
- // J_delta_model += J_delta_tangent * J_tangent_model;
+ J_delta_model += J_delta_tangent * J_tangent_model;
_jacobian_calib << J_delta_bias, J_delta_model; // J_delta_calib = [J_delta_bias ; J_delta_model]
const auto& J_delta_data = J_delta_tangent * J_tangent_data;
--
GitLab