diff --git a/src/processor/processor_force_torque_inertial_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp
index 395b253961f01ed16d87eb42abded8692424aa7a..15c72e2f7a5dda65029222ad07e00b37c144dafe 100644
--- a/src/processor/processor_force_torque_inertial_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_dynamics.cpp
@@ -404,7 +404,7 @@ void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::Vect
     Matrix<double, 18, 12> J_delta_tangent;
     Matrix<double, 18, 7>  J_delta_model;
     fti::tangent2delta(
-        tangent, model, _dt, _delta, J_delta_tangent, J_delta_model);  // Aquí ja farà bé ell sol el J_delta_model?
+        tangent, model, _dt, _delta, J_delta_tangent, J_delta_model);  
 
     // 3. Compose jacobians
     Matrix<double, 18, 6> J_delta_bias = J_delta_tangent * J_tangent_bias;
diff --git a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
index ad44a282cb1c2b914848ab2aefbbf4c40dbb2ca6..ad54fa8e05b66fa310efe27fc09f3344d6720198 100644
--- a/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_simulation_problem_force_torque_inertial_dynamics.cpp
@@ -52,7 +52,7 @@ using namespace bodydynamics::fti;
 
 WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
 
-class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
+class Test_SimulationProblemForceTorqueInertialDynamics_yaml : public testing::Test
 {
   public:
     ProblemPtr                              P;
@@ -65,21 +65,21 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
     double                                  mass_true;
     std::vector<double>                     time_stamp;
     double                                  dt;
-    Vector3d                                position_i, vlin_i, vang_i, force_i, torque_i, a_meas_i;
     std::vector<Vector3d>                   position, vlin, vang, force, torque, a_meas;
     std::vector<Quaterniond>                quaternion;
 
   protected:
     void ExtractAndCompleteData()
     {
+        Vector3d      position_i, vlin_i, vang_i, force_i, torque_i, a_meas_i;
         std::ifstream data_simulation;
-        string data_file = "/home/asanjuan/dev/wolf_lib/bodydynamics/test/dades_simulacio_pep.csv";
+        string        data_file = "/home/asanjuan/dev/wolf_lib/bodydynamics/test/dades_simulacio_pep.csv";
         data_simulation.open(data_file, std::ios::in);
-        string        line_data;
-        char          delimiter = ',';
+        string line_data;
+        char   delimiter = ',';
         std::getline(data_simulation, line_data);
-        //this acceleration is just to fill a gap, it is not going to be used and it is not true
-        a_meas.push_back(Vector3d(0,0,0));
+        // this acceleration is just to fill a gap, it is not going to be used and it is not true
+        a_meas.push_back(Vector3d(0, 0, 0));
 
         int counter = 0;
 
@@ -91,7 +91,6 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
             string string_time_stamp;
             std::getline(data, string_time_stamp, delimiter);
             time_stamp.push_back(std::stod(string_time_stamp));
-            WOLF_INFO(time_stamp[counter]);
 
             // position
             string string_pos_x;
@@ -189,13 +188,13 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
             if (counter != 0)
             {
                 dt       = time_stamp[counter] - time_stamp[counter - 1];
-                a_meas_i = quaternion[counter].conjugate() * ((vlin[counter] - vlin[counter - 1]) / dt + gravity());
+                a_meas_i = quaternion[counter].conjugate() * ((vlin[counter] - vlin[counter - 1]) / dt - gravity());
+                // a_meas_i = quaternion[counter] * ((vlin[counter] - vlin[counter - 1]) / dt - gravity());
                 a_meas.push_back(a_meas_i);
-                //We have to be careful with the index 
+                // We have to be careful with the index
             }
             counter++;
         }
-
     }
 
     void SetUp() override
@@ -204,7 +203,7 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
 
         std::string wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
         ParserYaml  parser =
-            ParserYaml("problem_force_torque_inertial_dynamics_solve_test.yaml", wolf_root + "/test/yaml/");
+            ParserYaml("problem_force_torque_inertial_dynamics_simulation_test.yaml", wolf_root + "/test/yaml/");
         ParamsServer server = ParamsServer(parser.getParams());
         P                   = Problem::autoSetup(server);
         // CERES WRAPPER
@@ -217,44 +216,79 @@ class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
         S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
         p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
 
+        //need to change with the real values, waiting for Pep's answer
         bias_true    = Vector6d::Zero();
         cdm_true     = {0, 0, 0.0341};
         inertia_true = {0.0176, 0.0180, 0.0296};  // rounded {0.017598, 0.017957, 0.029599}
         mass_true    = 1.952;
-
     }
 };
 
-TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, force_registraion)
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, force_registraion)
 {
     FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
 }
 
-TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, simulation)
+TEST_F(Test_SimulationProblemForceTorqueInertialDynamics_yaml, simulation)
 {
     // Calibration params
+    Vector6d bias_guess(Vector6d::Zero());
+    S->getStateBlock('I')->setState(bias_guess);
     Vector3d cdm_guess(0.005, 0.005, 0.1);
     S->getStateBlock('C')->setState(cdm_guess);
     Vector3d inertia_guess(0.01, 0.01, 0.02);
     S->getStateBlock('i')->setState(inertia_guess);
-    double mass_guess = 2.0;
+    double mass_guess = 1.5;
     S->getStateBlock('m')->setState(Vector1d(mass_guess));
 
     S->getStateBlock('P')->fix();
     S->getStateBlock('O')->fix();
-    S->getStateBlock('I')->unfix();
+    S->getStateBlock('I')->fix();
     S->getStateBlock('C')->unfix();
     S->getStateBlock('i')->unfix();
     S->getStateBlock('m')->unfix();
     S->setStateBlockDynamic('I', false);
-    
+
+    // add regularization 
+    S->addPriorParameter('C',                   // cdm
+                         cdm_guess,             // cdm 
+                         Matrix3d::Identity(),  // cov 
+                         0,                     // start: X coordinate
+                         3);                    // size
+
+    S->addPriorParameter('i',                   // inertia
+                         inertia_guess,         // inertia
+                         .00001*Matrix3d::Identity(),  // cov
+                         0,                     // start: X coordinate
+                         3);
+
+    S->addPriorParameter('m',                   // mass
+                         Vector1d(mass_guess),  // mass
+                         Matrix1d::Identity(),  // cov
+                         0,                     // start
+                         1);
 
     std::string report;
 
-    for (int i = 1; i < time_stamp.size(); i++)
+    // Force processor to make a KF at t=0
+    CaptureMotionPtr C0 =
+        std::make_shared<CaptureForceTorqueInertial>(0, S, VectorXd::Zero(12), MatrixXd::Identity(12, 12), nullptr);
+    C0->process();
+    // fix measured position and orientation
+    p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+    p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[0]);
+    p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+    p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[0].coeffs());
+
+
+    P->print(4, 1, 1, 1);
+
+    unsigned int last_kf_id = p->getOrigin()->getFrame()->id();
+
+    for (int i = 1; i < time_stamp.size(); i++)  // start with second data
     {
-        //SIMULATOR
-        
+        // SIMULATOR
+
         TimeStamp t = time_stamp[i];
 
         // Data
@@ -263,27 +297,44 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, simulation)
         data.segment<3>(3) = vang[i];
         data.segment<3>(6) = force[i];
         data.segment<3>(9) = torque[i];
-        MatrixXd data_cov  = MatrixXd::Identity(12, 12);
-
+        MatrixXd data_cov  = S->getNoiseCov();
 
         // ESTIMATOR
 
         CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
         C->process();
 
-        p->getOrigin()->getFrame()->unfix();
-        p->getOrigin()->getFrame()->getStateBlock('P')->fix();
-        p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i]);
-        p->getOrigin()->getFrame()->getStateBlock('O')->fix();
-        p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i].coeffs());
-
-        report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
-        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("-----------------------------");
+        // check if new KF
+        if (last_kf_id != p->getOrigin()->getFrame()->id())
+        {
+            last_kf_id = p->getOrigin()->getFrame()->id();
+
+            // fix measured position and orientation
+            p->getOrigin()->getFrame()->getStateBlock('P')->fix();
+            p->getOrigin()->getFrame()->getStateBlock('P')->setState(position[i]);
+            p->getOrigin()->getFrame()->getStateBlock('O')->fix();
+            p->getOrigin()->getFrame()->getStateBlock('O')->setState(quaternion[i].coeffs());
+
+
+
+            // solve!
+            report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+
+            WOLF_INFO(report);
+            P->print(1, 0, 1, 1);
+
+            // results of this iteration
+            WOLF_INFO("Torque                  : ", torque[i].transpose(),"  N m .");
+            WOLF_INFO("Angular velocity        : ", vang[i].transpose(),"  rad/s .");
+            WOLF_INFO("Angular momentum        : ", p->getOrigin()->getFrame()->getStateBlock('L')->getState().transpose(), "  kg m²/s .");
+            WOLF_INFO("Estimated inertia       : ", S->getStateBlock('i')->getState().transpose(), " m^2 Kg.");
+            WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
+            WOLF_INFO("Estimated mass          : ", S->getStateBlock('m')->getState()(0), " Kg.");
+            WOLF_INFO("-----------------------------");
+        }
     }
 
+    // FINAL RESULTS
     WOLF_INFO("True center of mass     : ", cdm_true.transpose(), " m.");
     WOLF_INFO("Guess center of mass    : ", cdm_guess.transpose(), " m.");
     WOLF_INFO("Estimated center of mass: ", S->getStateBlock('C')->getState().transpose(), " m.");
@@ -294,13 +345,10 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, simulation)
     WOLF_INFO("Guess mass              : ", mass_guess, " Kg.");
     WOLF_INFO("Estimated mass          : ", S->getStateBlock('m')->getState()(0), " Kg.");
     WOLF_INFO("-----------------------------");
-
-    WOLF_INFO("Acceleration     : ", a_meas[1].transpose());
 }
 
 int main(int argc, char **argv)
 {
-
     testing::InitGoogleTest(&argc, argv);
     // ::testing::GTEST_FLAG(filter) =
     //     "Test_SolveProblemForceTorqueInertialDynamics_yaml.simulation";
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 100292809f8a91411143c8ec3823c87a81afd34c..9508e6cee38df8090ac3748e98a58d70a1ae1197 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -208,7 +208,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
     WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
     WOLF_INFO("-----------------------------");
 
-    double dt = 0.2;
+    double dt        = 0.2;
     double time_last = 5.0;
 
     for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
@@ -291,7 +291,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
     // This is due to these first factors relying on the linearization Jacobian to correct the
     // poorly preintegrated delta.
 
-    double dt = 0.2;
+    double dt        = 0.2;
     double time_last = 5.0;
 
     for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
@@ -369,7 +369,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_and_ang_mom_hoveri
     WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
     WOLF_INFO("-----------------------------");
 
-    double dt = 0.2;
+    double dt        = 0.2;
     double time_last = 10.0;
 
     for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
@@ -455,7 +455,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_and_ang_m
     // This is due to these first factors relying on the linearization Jacobian to correct the
     // poorly preintegrated delta.
 
-    double dt = 0.2;
+    double dt        = 0.2;
     double time_last = 10.0;
 
     for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
@@ -562,7 +562,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, hovering_test_without_
     // This is due to these first factors relying on the linearization Jacobian to correct the
     // poorly preintegrated delta.
 
-    double dt = 0.2;
+    double dt        = 0.2;
     double time_last = 10.0;
 
     for (TimeStamp t = p->getLast()->getTimeStamp() + dt; t <= time_last; t += dt)
@@ -1092,6 +1092,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml,
         data.segment<3>(3) = ang_vel_true + gyro_bias_true + gyro_noise;
         data.segment<3>(6) = force_true + force_noise;
         data.segment<3>(9) = torque_true + torque_noise;
+        data_cov  = S->getNoiseCov();
 
         // Pose measurements (simulate motion capture)
         position_data = position_true;
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 cb6915f7f862657f2a18ce16f297f91e027ee73c..c76464f63119b98989d70e69fbc3cc03cd2527b7 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -61,7 +61,7 @@ config:
     n_propellers: 3
     keyframe_vote:
       voting_active:    true
-      max_time_span:    0.995
+      max_time_span:    0.0995
       max_buff_length:  999   # motion deltas
       dist_traveled:    999   # meters
       angle_turned:     999   # radians (1 rad approx 57 deg, approx 60 deg)