Add unmeasured covariance to the features

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;