diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
index b407b3123447b3f7fd38bafd32c5365e81ef0d8f..81156be7c680a4fdc991dd450c18bc50932791e1 100644
--- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
@@ -22,7 +22,7 @@
 /*
  * processor_preintegrator_force_torque_inertial_dynamics.cpp
  *
- *  Created on: Aug 19, 2021
+ *  Created on: Aug 1, 2022
  *      Author: asanjuan
  */
 
@@ -217,9 +217,12 @@ bool ProcessorForceTorqueInertialPreintDynamics::voteForKeyFrame() const
         return true;
     }
     // dist_traveled
-    double dist = getBuffer().back().delta_integr_.head<3>().norm();
-    if (dist > params_force_torque_inertial_preint_dynamics_->dist_traveled) 
-    //li fem cas a la delta_integr que dona la imu? o fem, per exemple, una mitjana amb la delta_integr que ve de la dinàmica?
+    VectorComposite X0 = getOrigin()->getFrame()->getState();
+    VectorComposite X1;
+    double dt = getBuffer().back().ts_ - getOrigin()->getFrame()->getTimeStamp();
+    statePlusDelta(X0, getBuffer().back().delta_integr_, dt, X1);
+    double dist = (X1.at('P') - X0.at('P')).norm();
+    if (dist > params_force_torque_inertial_preint_dynamics_->dist_traveled)
     {
         WOLF_DEBUG( "PM: vote: distance traveled" );
         return true;
diff --git a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp b/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
index 6fc1430582d776c268bc356bf159b87fce2ccd87..f18a4143a1ef87e7525bf8b842c67b163108df0e 100644
--- a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
+++ b/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
@@ -198,9 +198,62 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, 1m_x_moving_test)
 
 }
 
+//Test to see if the voteForKeyFrame works (distance traveled)
+TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_dist)
+{
+    VectorXd data      = VectorXd::Zero(12);  // [ a, w, f, t ]
+    data.segment<3>(0) = -gravity();
+    data(0)            = 2;
+    data.segment<3>(6) = - 1.952*gravity();
+    data(6)            = 1.952*2;
+    MatrixXd         data_cov = MatrixXd::Identity(12, 12);
+
+    p->setMaxTimeSpan(999);
+    p->setDistTraveled(0.995);
+
+    CaptureMotionPtr C0_0       = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+    CaptureMotionPtr C1_0       = std::make_shared<CaptureMotion>("CaptureMotion", 0.3, S, data, data_cov, nullptr);
+    CaptureMotionPtr C2_0       = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
+    CaptureMotionPtr C3_0       = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+    C0_0->process();
+    C1_0->process();
+    C2_0->process();
+    C3_0->process();
+
+    P->print(4,1,1,1);
+
+}
+
+//Test to see if the voteForKeyFrame works (buffer)
+TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_buffer)
+{
+    VectorXd data      = VectorXd::Zero(12);  // [ a, w, f, t ]
+    data.segment<3>(0) = -gravity();
+    data(0)            = 2;
+    data.segment<3>(6) = - 1.952*gravity();
+    data(6)            = 1.952*2;
+    MatrixXd         data_cov = MatrixXd::Identity(12, 12);
+
+    p->setMaxTimeSpan(999);
+    p->setMaxBuffLength(3);
+
+    CaptureMotionPtr C0_0       = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
+    CaptureMotionPtr C1_0       = std::make_shared<CaptureMotion>("CaptureMotion", 0.3, S, data, data_cov, nullptr);
+    CaptureMotionPtr C2_0       = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
+    CaptureMotionPtr C3_0       = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+    C0_0->process();
+    C1_0->process();
+    C2_0->process();
+    C3_0->process();
+
+    P->print(4,1,1,1);
+
+}
+
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
-    //::testing::GTEST_FLAG(filter) = "Test_ProcessorForceTorqueInertialPreintDynamics_yaml.residual";
+    ::testing::GTEST_FLAG(filter) = "Test_ProcessorForceTorqueInertialPreintDynamics_yaml.VoteForKeyFrame_buffer";
     return RUN_ALL_TESTS();
 }
\ No newline at end of file