Follow core 434

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

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()