diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h index dd9eef9ee86e6fd6daab00cdd142fc6713217d59..91a58b87620b66168be69be70ab84586917e605c 100644 --- a/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h +++ b/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. diff --git a/src/processor/processor_force_torque_inertial_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp index 994246ffc302b4f9c51c9238293d8971d0a251ae..7709b08aa9c23ed1bc8876be1c72d2d318f27c8a 100644 --- a/src/processor/processor_force_torque_inertial_dynamics.cpp +++ b/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,