diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 711906457a384d0548abb3a722108a841c0557c6..5e0d5c73033114a60d77a37b1674cd6e25dfccc1 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -663,7 +663,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
     Vector3d inertia_guess (0.01, 0.01, 0.02);
     S->getStateBlock('i')->setState(inertia_guess);
     double mass_guess = 2.0;
-    S->getStateBlock('m')->setState(Vector1d(2.0));
+    S->getStateBlock('m')->setState(Vector1d(mass_guess));
 
     Vector3d    acc_true = -gravity();
     Vector3d    ang_vel_true (0, 0, 0);
@@ -710,11 +710,11 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
     // initialiation of all the variables needed in the following iteration process
     
     Vector3d    dL;
-    double      dt = 0.5;  // time increment
+    double      dt = 0.1;  // time increment
     std::string report;
-    unsigned int num_kfs = P->getTrajectory()->getFrameMap().size();
+    unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
 
-    for (TimeStamp t = dt; t <= 20.0; t += dt)
+    for (TimeStamp t = dt; t <= 8.0; t += dt)
     {
         // SIMULATOR
 
@@ -751,9 +751,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
         C->process();
 
         // check if new KF
-        if (P->getTrajectory()->getFrameMap().size() > num_kfs)
+        if (last_kf_id != p->getOrigin()->getFrame()->id())
         {
-            num_kfs = P->getTrajectory()->getFrameMap().size();
+            last_kf_id = p->getOrigin()->getFrame()->id();       
             
             // fix measured position and orientation
             p->getOrigin()->getFrame()->unfix();
@@ -761,15 +761,16 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, rotation_test_unfixing
             p->getOrigin()->getFrame()->getStateBlock('P')->setState(position_data);
             p->getOrigin()->getFrame()->getStateBlock('O')->fix();
             p->getOrigin()->getFrame()->getStateBlock('O')->setState(orient_data);
-        }
 
-        report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
-        WOLF_INFO("Total angle turned: ", angle_true.transpose(), " rad.");
-        WOLF_INFO("Estimated IMU bias: ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
-        WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
-        WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
-        WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState()(0), " Kg.");
-        WOLF_INFO("-----------------------------");
+            report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+
+            WOLF_INFO("Total angle turned: ", angle_true.transpose(), " rad.");
+            WOLF_INFO("Estimated IMU bias: ", S->getStateBlock('I')->getState().transpose(), " m/s2 | rad/s.");
+            WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+            WOLF_INFO("Estimated inertia: ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+            WOLF_INFO("Estimated mass: ", S->getStateBlock('m')->getState()(0), " Kg.");
+            WOLF_INFO("-----------------------------");
+        }
     }
 
     WOLF_INFO("True IMU bias     : ", bias_true.transpose(), " m/s2 | rad/s.");