diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 940adf9489f1ddacb280954d657081b0d3f5ef6a..ad158eddd0b1b2813680c0691d49838b8406f7db 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -564,16 +564,16 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
     // Data
     VectorXd data      = VectorXd::Zero(12);  // [ a, w, f, t ]
     data.segment<3>(0) = -gravity();
-    Vector3d initial_ang_vel(0, 0, 0);
-    data.segment<3>(3) = initial_ang_vel;
+    Vector3d ang_vel_measure_initial(0, 0, 0);
+    data.segment<3>(3) = ang_vel_measure_initial;
     data.segment<3>(6) = -mass_true * gravity();
-    Vector3d initial_torque(0, 0, 1);
-    data.segment<3>(9) = initial_torque;
+    Vector3d torque_measure_constant(0, 0, 1);
+    data.segment<3>(9) = torque_measure_constant;
     MatrixXd data_cov  = MatrixXd::Identity(12, 12);
 
     S->getStateBlock('P')->fix();
     S->getStateBlock('O')->fix();
-    S->getStateBlock('I')->unfix();
+    S->getStateBlock('I')->fix();
     S->getStateBlock('C')->fix();
     S->getStateBlock('i')->unfix();
     S->getStateBlock('m')->fix();
@@ -585,32 +585,34 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
 
     C_KF0->getFrame()->unfix();
     C_KF0->getFrame()->getStateBlock('P')->fix();
-    C_KF0->getFrame()->getStateBlock('O')->fix();
-
-    WOLF_INFO("======== SOLVE PROBLEM =======")
-    std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
-    WOLF_INFO(report);  // should show a very low iteration number (possibly 1)
-
-    WOLF_INFO("True inertia     : ", inertia_true, " m^2 kg.");
-    WOLF_INFO("Guess inertia    : ", inertia_guess, " m^2 kg.");
-    WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 kg.");
-    WOLF_INFO("-----------------------------");
+    C_KF0->getFrame()->getStateBlock('P')->setState(Vector3d(0, 0, 0));
 
-    Vector3d dL = data.segment<3>(9); //dL = torque*dt
-    Vector3d ang_mom = C_KF0->getFrame()->getStateBlock('L')->getState();
-    ang_mom = ang_mom + dL; //L = L + dt
-
-    Vector3d ang_vel;
-    Vector3d actual_inertia;
+    // initialiation of all the variables needed in the following iteration process
+    Vector3d         dL;
+    Vector3d         L_time_actual;
+    Vector3d         ang_vel_actual;
+    Vector3d         inertia_actual;
+    double           dt            = 1;  // time increment
+    CaptureMotionPtr C_time_before = C_KF0;
+    std::string      report;
 
     for (TimeStamp t = 1.0; t <= 50.0; t += 1.0)
     {
-        // w = L*i_inv  we actualize the data 
-        actual_inertia = p->getSensor()->getStateBlock('i')->getState();
-        ang_vel(0) = ang_mom(0) / actual_inertia(0);
-        ang_vel(1) = ang_mom(1) / actual_inertia(1);
-        ang_vel(2) = ang_mom(2) / actual_inertia(2);
-        data.segment<3>(3) = ang_vel;
+        // SIMULATOR
+
+        // calculate dL = torque*dt
+        dL = data.segment<3>(9) * dt;
+        // actualize L = L + dt
+        L_time_actual = C_time_before->getFrame()->getStateBlock('L')->getState();
+        L_time_actual = L_time_actual + dL;  // L = L + dt
+        // w = L*i_inv  we actualize the data
+        inertia_actual     = p->getSensor()->getStateBlock('i')->getState();
+        ang_vel_actual(0)  = L_time_actual(0) / inertia_actual(0);
+        ang_vel_actual(1)  = L_time_actual(1) / inertia_actual(1);
+        ang_vel_actual(2)  = L_time_actual(2) / inertia_actual(2);
+        data.segment<3>(3) = ang_vel_actual;
+
+        // ESTIMATOR
 
         CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
         C->setTimeStamp(t);
@@ -618,18 +620,19 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_around_z_axis
 
         p->getOrigin()->getFrame()->unfix();
         p->getOrigin()->getStateBlock('P')->fix();
-        p->getOrigin()->getStateBlock('O')->fix();
+        p->getOrigin()->getStateBlock('P')->setState(Vector3d(0, 0, 0));
 
         report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
         WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
         WOLF_INFO("-----------------------------");
 
-        //L increment
-        //dL = torque*dt 
-        //dL in this specific problem is always the same because torque is not changing
-        ang_mom = p->getOrigin()->getFrame()->getStateBlock('L')->getState();
-        ang_mom = ang_mom + dL;
+        WOLF_INFO("True inertia     : ", inertia_true, " m^2 Kg.");
+        WOLF_INFO("Guess inertia    : ", inertia_guess, " m^2 Kg.");
+        WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+        WOLF_INFO("-----------------------------");
 
+        // saving of the actual capture to use it in the next iteration
+        C_time_before = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
     }
 
     auto inertia_estimated = S->getStateBlock('i')->getState();
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
index 8eb8204126f94bc008bcb01587ae2fa5482fec6b..12fd236bfb138431bb04965ce1de309e70285df0 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -43,7 +43,7 @@ config:
     # Dynamics
     com:                          [0,0,0.0341]                      # center of mass [m]
     inertia:                      [0.017598,0.017957,0.029599]      # moments of inertia i_xx, i_yy, i_zz [kg m2]
-    mass:                         1.9                               # mass [kg]
+    mass:                         1.952                             # mass [kg]
 
   processors:
    -