diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
index db76c4925689d94b39a0a17c22790894e4f4b09d..11f248448329c3fc696bdf2748980117497270dd 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
+++ b/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
@@ -71,6 +71,8 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
   protected:
     ParamsProcessorForceTorqueInertialPreintDynamicsPtr params_force_torque_inertial_preint_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_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
index a0d8065ae5ce58c4f7add9241540d0b4c728ed74..8ae3141e495616e33772eccd1f710dea612f762f 100644
--- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
@@ -111,6 +111,17 @@ void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBas
 void ProcessorForceTorqueInertialPreintDynamics::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;
+
+    gyro_noise_cov_ = Eigen::Array3d(gyro_noise_std, gyro_noise_std, gyro_noise_std).square().matrix().asDiagonal();
+    imu_drift_cov_ =
+        Eigen::Array<double,6,1>(acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std)
+            .square()
+            .matrix()
+            .asDiagonal();
 }
 
 void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::VectorXd& _data,