diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp index 992a18499ace494bed94928ac693ca69ba9c9e44..a0d8065ae5ce58c4f7add9241540d0b4c728ed74 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;