diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp index 8ae3141e495616e33772eccd1f710dea612f762f..707cb405da41324703e49c8eaf834e3c1c288e8f 100644 --- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp +++ b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp @@ -273,7 +273,8 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen * * J_delta_data = J_delta_tangent * J_tangent_data * J_delta_model = J_delta_model + J_delta_tangent * J_tangent_model <-- remember all Jacobians are PARTIAL - * derivatives. J_delta_bias = J_delta_tangent * J_tangent_bias + * derivatives. + * J_delta_bias = J_delta_tangent * J_tangent_bias * * Finally we can assemble the Jacobian `J_delta_calib` as the concatenation of bias and model: * @@ -354,9 +355,6 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen // 3. Compose jacobians Matrix<double, 18, 6> J_delta_bias = J_delta_tangent * J_tangent_bias; - // !!!!!! 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; _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;