Fix test: wrong feature being checked

test/gtest_processor_force_torque_inertial_dynamics.cpp

@@ -129,7 +129,7 @@ TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, not_moving_test)
 
     betw_states << dpi, dvi, dpd, dvd, dL, dq.coeffs(); 
     
-    VectorXd delta_preintegrated = C_KF1->getFeatureList().back()->getMeasurement();
+    VectorXd delta_preintegrated = C_KF1->getFeatureList().front()->getMeasurement();
 
     //Cheking that the preintegrated delta calculated by the processor is the same as the one that is correct for sure
     ASSERT_MATRIX_APPROX(betw_states, delta_preintegrated, 1e-8);
@@ -197,7 +197,7 @@ TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, 1m_x_moving_test__com_zer
 
     betw_states << dpi, dvi, dpd, dvd, dL, dq.coeffs(); 
     
-    VectorXd delta_preintegrated = C_KF1->getFeatureList().back()->getMeasurement();
+    VectorXd delta_preintegrated = C_KF1->getFeatureList().front()->getMeasurement();
 
     //Cheking that the preintegrated delta calculated by the processor is the same as the one that is correct for sure
     ASSERT_MATRIX_APPROX(betw_states, delta_preintegrated, 1e-8);