new gtest testing noise

src/sensor/sensor_force_torque_inertial.cpp

@@ -48,6 +48,14 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
     setStateBlockDynamic('C', false);
     setStateBlockDynamic('i', false);
     setStateBlockDynamic('m', false);
+
+    VectorXd noise_std_vector(12);
+    Vector3d acc_noise_std_vector = Vector3d::Ones() * params_fti_->acc_noise_std; 
+    Vector3d gyro_noise_std_vector = Vector3d::Ones() * params_fti_->gyro_noise_std; 
+    Vector3d force_noise_std_vector = Vector3d::Ones() * params_fti_->force_noise_std;
+    Vector3d torque_noise_std_vector =Vector3d::Ones() * params_fti_->torque_noise_std;
+    noise_std_vector << acc_noise_std_vector, gyro_noise_std_vector, force_noise_std_vector, torque_noise_std_vector;
+    setNoiseStd(noise_std_vector);
 }
 
 // TODO: Adapt this API to that of MR !448

test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml

@@ -19,7 +19,7 @@ config:
       type: "TreeManagerSlidingWindow"
       plugin: "core"
       n_frames: 3
-      n_fix_first_frames: 0
+      n_fix_first_frames: 1
       viral_remove_empty_parent: true
   map:
     type: "MapBase"
@@ -33,12 +33,14 @@ config:
       pose: [0,0,0, 0,0,0,1]
     
     # IMU
-    force_noise_std:              0.1         # std dev of force noise in N 
-    torque_noise_std:             0.1         # std dev of torque noise in N/m
-    acc_noise_std:                0.01        # std dev of acc noise in m/s2
-    gyro_noise_std:               0.01        # std dev of gyro noise in rad/s
-    acc_drift_std:                0.00001     # std dev of acc drift m/s2/sqrt(s)
-    gyro_drift_std:               0.00001     # std dev of gyro drift rad/s/sqrt(s)
+    acc_noise_std:                0.001      # std dev of acc noise in m/s2
+    gyro_noise_std:               0.001      # std dev of gyro noise in rad/s
+    acc_drift_std:                0.0001     # std dev of acc drift m/s2/sqrt(s)
+    gyro_drift_std:               0.0001     # std dev of gyro drift rad/s/sqrt(s)
+    
+    #FT
+    force_noise_std:              0.001         # std dev of force noise in N 
+    torque_noise_std:             0.0001       # std dev of torque noise in N/m
     
     # Dynamics
     com:                          [0,0,0.0341]                      # center of mass [m]