Get the gyro covariance from the motion data

include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h

@@ -72,7 +72,6 @@ class ProcessorForceTorqueInertialDynamics : public ProcessorMotion
     ParamsProcessorForceTorqueInertialDynamicsPtr params_force_torque_inertial_dynamics_;
     SensorForceTorqueInertialPtr                  sensor_fti_;
     Matrix6d imu_drift_cov_;
-    Matrix3d gyro_noise_cov_;
 
   public:
     /** \brief convert raw CaptureMotion data to the delta-state format.

src/processor/processor_force_torque_inertial_dynamics.cpp

@@ -100,7 +100,7 @@ void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBase
     // - factor relates w_measured, IMU_bias(last capture), L(last capture) and i(sensor)
 
     auto measurement_gyro = motion.data_.segment<3>(3);
-    auto gyro_cov         = sensor_fti_->getNoiseCov().block<3, 3>(3, 3);
+    auto gyro_cov         = motion.data_cov_.block<3, 3>(3, 3);
 
     auto ftr_base = FeatureBase::emplace<FeatureBase>(_capture_own, "FeatureBase", measurement_gyro, gyro_cov);
 
@@ -137,7 +137,7 @@ void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBase
                                                        0,                   // take all of first second block
                                                        -1,                  // take all of first second block
                                                        shared_from_this(),  // this processor
-                                                       false);              // loss function
+                                                       params_->apply_loss_function); // loss function
         }
     }
 }
@@ -154,14 +154,12 @@ void ProcessorForceTorqueInertialDynamics::configure(SensorBasePtr _sensor)
 {
     sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
 
-    auto gyro_noise_std = sensor_fti_->getParamsSensorForceTorqueInertial()->gyro_noise_std;
     auto acc_drift_std  = sensor_fti_->getParamsSensorForceTorqueInertial()->acc_drift_std;
     auto gyro_drift_std = sensor_fti_->getParamsSensorForceTorqueInertial()->gyro_drift_std;
 
     ArrayXd imu_drift_sigmas(6);
     imu_drift_sigmas << acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std;
     imu_drift_cov_  = imu_drift_sigmas.square().matrix().asDiagonal();
-    gyro_noise_cov_ = Array3d(gyro_noise_std, gyro_noise_std, gyro_noise_std).square().matrix().asDiagonal();
 }
 
 void ProcessorForceTorqueInertialDynamics::data2tangent(const Eigen::VectorXd& _data,