diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque_preint.h index 1c6133e282734c512b0260fd21781353669a6b0b..a7041f4d85191beb9a4a6051ea52d0356e154027 100644 --- a/include/bodydynamics/processor/processor_force_torque_preint.h +++ b/include/bodydynamics/processor/processor_force_torque_preint.h @@ -132,7 +132,7 @@ inline void ProcessorForceTorquePreint::configure(SensorBasePtr _sensor) inline Eigen::VectorXd ProcessorForceTorquePreint::deltaZero() const { - return (Eigen::VectorXd(13) << 0,0,0, 0,0,0, 0,0,0, 0,0,0,1 ).finished(); // p, q, v + return (Eigen::VectorXd(13) << 0,0,0, 0,0,0, 0,0,0, 0,0,0,1 ).finished(); // com, com vel, ang momentum, orientation } } // namespace wolf diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp index e24f86d5ef48a1957c291acac94dcad3766bfb9e..cb39f2a8bcb77e0cc98b997f65c792bcfade636b 100644 --- a/src/processor/processor_force_torque_preint.cpp +++ b/src/processor/processor_force_torque_preint.cpp @@ -46,11 +46,7 @@ ProcessorForceTorquePreint::ProcessorForceTorquePreint(ParamsProcessorForceTorqu dimc_(_params_motion_force_torque_preint->dimc) { - // Set constant parts of Jacobians - jacobian_delta_preint_.setIdentity(12,12); // delta composition / CURRENT delta - jacobian_delta_.setIdentity(12,12); // delta composition / PREVIOUS delta - jacobian_calib_.setZero(12,6); // delta preint / biases - unmeasured_perturbation_cov_ = pow(params_motion_force_torque_preint_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix<double, 12, 12>::Identity(); + // } ProcessorForceTorquePreint::~ProcessorForceTorquePreint()