diff --git a/CMakeLists.txt b/CMakeLists.txt
index eba12ca4379a314de77f457308cb5e2ff20876d8..84505a3442495401472555f5de562c45f8743e6c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -103,30 +103,29 @@ include/${PROJECT_NAME}/math/force_torque_delta_tools.h
 include/${PROJECT_NAME}/math/force_torque_inertial_delta_tools.h
   )
 SET(HDRS_CAPTURE
-include/${PROJECT_NAME}/capture/capture_force_torque_preint.h
+include/${PROJECT_NAME}/capture/capture_force_torque.h
+include/${PROJECT_NAME}/capture/capture_force_torque_inertial.h
 include/${PROJECT_NAME}/capture/capture_inertial_kinematics.h
 include/${PROJECT_NAME}/capture/capture_leg_odom.h
 include/${PROJECT_NAME}/capture/capture_point_feet_nomove.h
   )
 SET(HDRS_FACTOR
 include/${PROJECT_NAME}/factor/factor_angular_momentum.h
-include/${PROJECT_NAME}/factor/factor_force_torque.h
 include/${PROJECT_NAME}/factor/factor_force_torque_inertial.h
 include/${PROJECT_NAME}/factor/factor_force_torque_inertial_dynamics.h
-include/${PROJECT_NAME}/factor/factor_force_torque_preint.h
+include/${PROJECT_NAME}/factor/factor_force_torque.h
 include/${PROJECT_NAME}/factor/factor_inertial_kinematics.h
 include/${PROJECT_NAME}/factor/factor_point_feet_nomove.h
 include/${PROJECT_NAME}/factor/factor_point_feet_altitude.h
   )
 SET(HDRS_FEATURE
 include/${PROJECT_NAME}/feature/feature_force_torque.h
-include/${PROJECT_NAME}/feature/feature_force_torque_preint.h
 include/${PROJECT_NAME}/feature/feature_inertial_kinematics.h
   )
 SET(HDRS_PROCESSOR
-include/${PROJECT_NAME}/processor/processor_force_torque_inertial_preint.h
-include/${PROJECT_NAME}/processor/processor_force_torque_inertial_preint_dynamics.h
-include/${PROJECT_NAME}/processor/processor_force_torque_preint.h
+include/${PROJECT_NAME}/processor/processor_force_torque_inertial.h
+include/${PROJECT_NAME}/processor/processor_force_torque_inertial_dynamics.h
+include/${PROJECT_NAME}/processor/processor_force_torque.h
 include/${PROJECT_NAME}/processor/processor_inertial_kinematics.h
 include/${PROJECT_NAME}/processor/processor_point_feet_nomove.h
   )
@@ -139,20 +138,20 @@ include/${PROJECT_NAME}/sensor/sensor_point_feet_nomove.h
 
 # ============ SOURCES ============ 
 SET(SRCS_CAPTURE
-src/capture/capture_force_torque_preint.cpp
+src/capture/capture_force_torque.cpp
+src/capture/capture_force_torque_inertial.cpp
 src/capture/capture_inertial_kinematics.cpp
 src/capture/capture_leg_odom.cpp
 src/capture/capture_point_feet_nomove.cpp
 )
 SET(SRCS_FEATURE
 src/feature/feature_force_torque.cpp
-src/feature/feature_force_torque_preint.cpp
 src/feature/feature_inertial_kinematics.cpp
 )
 SET(SRCS_PROCESSOR
-src/processor/processor_force_torque_inertial_preint.cpp
-src/processor/processor_force_torque_inertial_preint_dynamics.cpp
-src/processor/processor_force_torque_preint.cpp
+src/processor/processor_force_torque_inertial.cpp
+src/processor/processor_force_torque_inertial_dynamics.cpp
+src/processor/processor_force_torque.cpp
 src/processor/processor_inertial_kinematics.cpp
 src/processor/processor_point_feet_nomove.cpp
 )
diff --git a/demos/processor_ft_preint.yaml b/demos/processor_ft_preint.yaml
index d9b49d70b860fe2e3029a4cc9bd62ffcc2fdc6c8..e9d8c3b37000e40a58ac253eaec04a96f0ae4232 100644
--- a/demos/processor_ft_preint.yaml
+++ b/demos/processor_ft_preint.yaml
@@ -1,5 +1,5 @@
-type: "ProcessorForceTorquePreint"         # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-name: "PFTPreint"    # This is ignored. The name provided to the SensorFactory prevails
+type: "ProcessorForceTorque"         # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+name: "PFT"    # This is ignored. The name provided to the SensorFactory prevails
 time_tolerance: 0.0005         # Time tolerance for joining KFs
 unmeasured_perturbation_std: 0.0000000
 sensor_ikin_name: "SenIK"
diff --git a/demos/solo_imu_kine.cpp b/demos/solo_imu_kine.cpp
index a9f52035066348852f9a296504e414520669656e..02a07f6c138bab362d7a2357aa23dc63d2b6826e 100644
--- a/demos/solo_imu_kine.cpp
+++ b/demos/solo_imu_kine.cpp
@@ -68,12 +68,12 @@
 #include "bodydynamics/sensor/sensor_inertial_kinematics.h"
 #include "bodydynamics/sensor/sensor_force_torque.h"
 #include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_force_torque_.h"
 #include "bodydynamics/capture/capture_leg_odom.h"
 #include "bodydynamics/processor/processor_inertial_kinematics.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/processor/processor_force_torque_.h"
 #include "bodydynamics/factor/factor_inertial_kinematics.h"
-#include "bodydynamics/factor/factor_force_torque_preint.h"
+#include "bodydynamics/factor/factor_force_torque_.h"
 
 #include "bodydynamics/sensor/sensor_point_feet_nomove.h"
 #include "bodydynamics/processor/processor_point_feet_nomove.h"
diff --git a/demos/solo_imu_kine_mocap.cpp b/demos/solo_imu_kine_mocap.cpp
index 82d2df1c42da66a66e6894fe49e037b4a04259cd..8ae580418ec8691b984c8a5c3a11e1c3ea81aab5 100644
--- a/demos/solo_imu_kine_mocap.cpp
+++ b/demos/solo_imu_kine_mocap.cpp
@@ -68,12 +68,12 @@
 #include "bodydynamics/sensor/sensor_inertial_kinematics.h"
 #include "bodydynamics/sensor/sensor_force_torque.h"
 #include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_force_torque_.h"
 #include "bodydynamics/capture/capture_leg_odom.h"
 #include "bodydynamics/processor/processor_inertial_kinematics.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/processor/processor_force_torque_.h"
 #include "bodydynamics/factor/factor_inertial_kinematics.h"
-#include "bodydynamics/factor/factor_force_torque_preint.h"
+#include "bodydynamics/factor/factor_force_torque_.h"
 
 #include "bodydynamics/capture/capture_point_feet_nomove.h"
 #include "bodydynamics/sensor/sensor_point_feet_nomove.h"
diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp
index 9953db41444d0d1b1da732f5460c3bd398cd3a63..181fc3d44d83f1d89755f34abb5a24f4a806e4f3 100644
--- a/demos/solo_real_povcdl_estimation.cpp
+++ b/demos/solo_real_povcdl_estimation.cpp
@@ -58,12 +58,12 @@
 #include "bodydynamics/sensor/sensor_inertial_kinematics.h"
 #include "bodydynamics/sensor/sensor_force_torque.h"
 #include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_force_torque_.h"
 #include "bodydynamics/capture/capture_leg_odom.h"
 #include "bodydynamics/processor/processor_inertial_kinematics.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/processor/processor_force_torque_.h"
 #include "bodydynamics/factor/factor_inertial_kinematics.h"
-#include "bodydynamics/factor/factor_force_torque_preint.h"
+#include "bodydynamics/factor/factor_force_torque_.h"
 
 #include "bodydynamics/capture/capture_point_feet_nomove.h"
 #include "bodydynamics/sensor/sensor_point_feet_nomove.h"
@@ -284,7 +284,7 @@ int main (int argc, char **argv) {
     SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
     // SensorBasePtr sen_ft_base = problem->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
     SensorForceTorquePtr sen_ft = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
-    ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>();
+    ParamsProcessorForceTorquePtr params_sen_ft = std::make_shared<ParamsProcessorForceTorque>();
     params_sen_ft->sensor_ikin_name = "SenIK";
     params_sen_ft->sensor_angvel_name = "SenImu";
     params_sen_ft->nbc = nbc;
@@ -296,9 +296,9 @@ int main (int argc, char **argv) {
     params_sen_ft->dist_traveled = 20000.0;
     params_sen_ft->angle_turned = 1000;
     params_sen_ft->voting_active = false;
-    ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft, params_sen_ft);
-    // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint.yaml");
-    ProcessorForceTorquePreintPtr proc_ftpreint = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
+    ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorque", "PFT", sen_ft, params_sen_ft);
+    // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorque", "PFT", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint.yaml");
+    ProcessorForceTorquePtr proc_ft = std::static_pointer_cast<ProcessorForceTorque>(proc_ft_base);
 
     // SENSOR + PROCESSOR POINT FEET NOMOVE
     ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
@@ -355,7 +355,7 @@ int main (int argc, char **argv) {
     VectorXd meas_ikin(9); meas_ikin << i_p_ic, i_v_ic, i_omg_oi;
     auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0, sen_ikin, meas_ikin, i_Iw, i_Lc_gesti);
     CIKin0->process();
-    proc_ftpreint->setOrigin(KF1);
+    proc_ft->setOrigin(KF1);
 
     ////////////////////////////////////////////
     // INITIAL BIAS FACTORS
@@ -557,7 +557,7 @@ int main (int argc, char **argv) {
         CImu->process();
         auto CIKin = std::make_shared<CaptureInertialKinematics>(ts, sen_ikin, meas_ikin, i_Iw, i_Lc_gesti);
         CIKin->process();
-        auto CFTpreint = std::make_shared<CaptureForceTorquePreint>(ts, sen_ft, CIKin, CImu, cap_ftp_data, Qftp);
+        auto CFTpreint = std::make_shared<CaptureForceTorque>(ts, sen_ft, CIKin, CImu, cap_ftp_data, Qftp);
         CFTpreint->process();
 
 
diff --git a/include/bodydynamics/capture/capture_force_torque_preint.h b/include/bodydynamics/capture/capture_force_torque.h
similarity index 94%
rename from include/bodydynamics/capture/capture_force_torque_preint.h
rename to include/bodydynamics/capture/capture_force_torque.h
index 81db7bf05f90fc9048a907253b477b73f8c748a5..fab1ca4951a7ec09fef965b2176a5d8a848aee66 100644
--- a/include/bodydynamics/capture/capture_force_torque_preint.h
+++ b/include/bodydynamics/capture/capture_force_torque.h
@@ -32,9 +32,9 @@
 
 namespace wolf {
     
-WOLF_PTR_TYPEDEFS(CaptureForceTorquePreint);
+WOLF_PTR_TYPEDEFS(CaptureForceTorque);
 
-class CaptureForceTorquePreint : public CaptureMotion
+class CaptureForceTorque : public CaptureMotion
 {
     public:
         /* 
@@ -51,7 +51,7 @@ class CaptureForceTorquePreint : public CaptureMotion
         qbl1, 4 : orientation of foot 1 in base frame
         qbl2, 4 : orientation of foot 2 in base frame
         */
-        CaptureForceTorquePreint(
+        CaptureForceTorque(
                     const TimeStamp& _init_ts,
                     SensorBasePtr _sensor_ptr,
                     CaptureInertialKinematicsPtr _capture_IK_ptr,  // to get the pbc bias
@@ -60,7 +60,7 @@ class CaptureForceTorquePreint : public CaptureMotion
                     const Eigen::MatrixXd& _data_cov,
                     CaptureBasePtr _capture_origin_ptr = nullptr);
     
-        ~CaptureForceTorquePreint() override;
+        ~CaptureForceTorque() override;
 
         CaptureBaseConstPtr getIkinCaptureOther() const { return cap_ikin_other_;}
         CaptureBasePtr getIkinCaptureOther(){ return cap_ikin_other_;}
diff --git a/src/feature/feature_force_torque_preint.cpp b/include/bodydynamics/capture/capture_force_torque_inertial.h
similarity index 56%
rename from src/feature/feature_force_torque_preint.cpp
rename to include/bodydynamics/capture/capture_force_torque_inertial.h
index 1a21081d827843645a62c75508b3ceda10794c71..b6ba6d7752cd7c4427df706e84ffedd07ab9873e 100644
--- a/src/feature/feature_force_torque_preint.cpp
+++ b/include/bodydynamics/capture/capture_force_torque_inertial.h
@@ -19,18 +19,27 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-#include "bodydynamics/feature/feature_force_torque_preint.h"
-namespace wolf {
+#ifndef CAPTURE_FORCE_TORQUE_INERTIAL_H
+#define CAPTURE_FORCE_TORQUE_INERTIAL_H
 
-FeatureForceTorquePreint::FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated,
-                       const Eigen::MatrixXd& _delta_preintegrated_covariance,
-                       const Eigen::Vector6d& _biases_preint,
-                       const Eigen::Matrix<double,12,6>& _J_delta_biases) :
-    FeatureBase("FeatureForceTorquePreint", _delta_preintegrated, _delta_preintegrated_covariance),
-    pbc_bias_preint_(_biases_preint.head<3>()),
-    gyro_bias_preint_(_biases_preint.tail<3>()),
-    J_delta_bias_(_J_delta_biases)
+#include <core/capture/capture_motion.h>
+
+namespace wolf
 {
-}
 
-} // namespace wolf
+WOLF_PTR_TYPEDEFS(CaptureForceTorqueInertial);
+
+class CaptureForceTorqueInertial : public CaptureMotion
+{
+  public:
+    CaptureForceTorqueInertial(const TimeStamp&       _init_ts,
+                               SensorBasePtr          _sensor_ptr,
+                               const Eigen::VectorXd& _data,
+                               const Eigen::MatrixXd& _data_cov,
+                               CaptureBasePtr         _capture_origin_ptr = nullptr);
+    virtual ~CaptureForceTorqueInertial();
+};
+
+}  // namespace wolf
+
+#endif  // CAPTURE_FORCE_TORQUE_INERTIAL_H
\ No newline at end of file
diff --git a/include/bodydynamics/factor/factor_angular_momentum.h b/include/bodydynamics/factor/factor_angular_momentum.h
new file mode 100644
index 0000000000000000000000000000000000000000..5c120ae2eb369c9c7e373b66f5d55dbd2996f0ba
--- /dev/null
+++ b/include/bodydynamics/factor/factor_angular_momentum.h
@@ -0,0 +1,172 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTOR_ANGULAR_MOMENTUM_H
+#define FACTOR_ANGULAR_MOMENTUM_H
+
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include <core/feature/feature_motion.h>
+#include <core/factor/factor_autodiff.h>
+
+namespace wolf
+{
+using namespace Eigen;
+using namespace bodydynamics;
+
+WOLF_PTR_TYPEDEFS(FactorAngularMomentum);
+
+/**
+ * @brief
+ *
+ * This factor evaluates the real angular velocity against the one obtained with the pre-integrated angular momentum
+ * and the moment of inertia.
+ *
+ * State blocks considered:
+ *  - Frame
+ *    'L' angular momentum
+ *  - Capture
+ *    'I' IMU biases
+ *  - Sensor Force Torque Inertial
+ *    'i' inertia matrix components (we are assuming that it is a diagonal matrix)
+ *
+ * The residual computed has the following components, in this order
+ *  - angular velocity error according to FT preintegration
+ */
+class FactorAngularMomentum : public FactorAutodiff<FactorAngularMomentum, 3, 3, 3, 6, 3>  // State Blocks: L, I, i
+{
+  public:
+    FactorAngularMomentum(const FeatureMotionPtr& _ftr,        // gets measurement and access to parents
+                          const ProcessorBasePtr& _processor,  // gets access to processor and sensor
+                          bool                    _apply_loss_function,
+                          FactorStatus            _status = FAC_ACTIVE);
+
+    ~FactorAngularMomentum() override = default;
+
+    template <typename T>
+    bool operator()(const T* const _L,  // ang momentum
+                    const T* const _I,  // IMU bias (acc and gyro)
+                    const T* const _i,  // inertia matrix
+                    T*             _res) const;     // residual
+
+    template <typename D1, typename D2, typename D3, typename D4>
+    bool residual(const MatrixBase<D1>& _L,
+                  const MatrixBase<D2>& _I,
+                  const MatrixBase<D3>& _i,
+                  MatrixBase<D4>&       _res) const;
+
+    Vector3d residual() const;
+
+  private:
+    Matrix<double, 12, 1> data;
+    Matrix<double, 3, 3> sqrt_info_upper_;
+};
+
+inline FactorAngularMomentum::FactorAngularMomentum(const FeatureBasePtr& _ftr,
+                                                    const ProcessorBasePtr& _processor,
+                                                    bool         _apply_loss_function,
+                                                    FactorStatus _status)
+    : FactorAutodiff<FactorAngularMomentum, 3, 3, 3, 6, 3>(
+          "FactorAngularMomentum",
+          TOP_MOTION,
+          _ftr,                         // parent feature
+          nullptr,                 // frame other
+          nullptr,              // capture other
+          nullptr,                      // feature other
+          nullptr,                      // landmark other
+          _processor,                   // processor
+          _apply_loss_function,
+          _status,
+          _ftr->getFrame()->getStateBlock('L'),    // previous frame L
+          _capture_origin->getStateBlock('I'),                // previous frame IMU bias
+          _processor->getSensor()->getStateBlock('i'),   // sensor i
+          data(ftr->getMeasurement()),
+          sqrt_info_upper_(_processor->getSensor()->getNoiseCov().block<3,3>(3,3)))
+{
+    //
+}
+
+template <typename D1, typename D2, typename D3, typename D4>
+inline bool FactorAngularMomentum::residual(const MatrixBase<D1>&     _L,
+                                            const MatrixBase<D2>&     _I,
+                                            const MatrixBase<D3>&     _i,
+                                            MatrixBase<D4>&           _res) const
+{
+    MatrixSizeCheck<3, 1>::check(_res);
+
+    /* Will do the following:
+     *
+     * Compute the real angular velocity through the measurement and the IMU bias
+     *    w_real = w_m - w_b
+     *
+     * Compute the angular velocity obtained by the relation between L pre-integrated and the i
+     *    w_est = i^(-1)*L
+     *
+     * Compute error between them
+     *    err = w_m - w_b - i^(-1)*L
+     *
+     * Compute residual
+     *    res = W * err , with W the sqrt of the info matrix.
+     */
+
+    Matrix<T, 3, 1> w_real = data.segment<3>(3) - _I.segment<3>(3);
+    double Lx = _L(0);
+    double Ly = _L(1);
+    double Lz = _L(2);
+    double ixx = _i(0);
+    double iyy = _i(1);
+    double izz = _i(2);
+    Matrix<T, 3, 1> w_est = [Lx/ixx, Ly/iyy, Lz/izz];
+    Matrix<T, 3, 1> err   = w_real - w_est;
+    _res                  = sqrt_info_upper_ * err;
+
+    return true;
+}
+
+inline Vector3d FactorAngularMomentum::residual() const
+{
+    Vector3d res(3);
+    Vector3d L = getFrame()->getStateBlock('L')->getState();
+    Vector6d I = getFeature()->getCapture()->getSensor()->getState();
+    Vector3d i = getFeature()->getSensor()->getStateBlock('i')->getState();
+
+    residual(L, I, i, res);
+    return res;
+}
+
+template <typename T>
+inline bool FactorAngularMomentum::operator()(const T* const _L,
+                                              const T* const _I,
+                                              const T* const _i,
+                                              T*             _res) const
+{
+    Map<const Matrix<T, 3, 1>> L(_L);
+    Map<const Matrix<T, 6, 1>> I(_I);
+    Map<const Matrix<T, 3, 1>> i(_i);
+    Map<Matrix<T, 3, 1>>      res(_res);
+
+    residual(L, I, i, res);
+
+    return true;
+}
+
+}  // namespace wolf
+
+#endif  // FACTOR_ANGULAR_MOMENTUM_H
\ No newline at end of file
diff --git a/include/bodydynamics/factor/factor_force_torque.h b/include/bodydynamics/factor/factor_force_torque.h
index 958398a325c0e6c77fd5b70a2cabb4497fec746b..b9ff1b4c45b3e4108c0ed1e755c7a326044a3f76 100644
--- a/include/bodydynamics/factor/factor_force_torque.h
+++ b/include/bodydynamics/factor/factor_force_torque.h
@@ -19,264 +19,325 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-/**
- * \file factor_force_torque.h
- *
- *  Created on: Feb 19, 2020
- *      \author: mfourmy
- * 
- *  Works only for 2 limbs
- */
-
-
-
 #ifndef FACTOR_FORCE_TORQUE_H_
 #define FACTOR_FORCE_TORQUE_H_
 
-#include <core/math/rotations.h>
-#include <core/math/covariance.h>
-#include <core/factor/factor_autodiff.h>
-#include <core/feature/feature_base.h>
-
+//Wolf includes
+#include "bodydynamics/capture/capture_force_torque.h"
 #include "bodydynamics/feature/feature_force_torque.h"
+#include "bodydynamics/sensor/sensor_force_torque.h"
+#include "core/factor/factor_autodiff.h"
+#include "core/math/rotations.h"
 
-namespace wolf
-{
+//Eigen include
 
+namespace wolf {
+    
 WOLF_PTR_TYPEDEFS(FactorForceTorque);
 
-class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 9, 3,3,3, 3,3,3,3,4>
+//class
+class FactorForceTorque : public FactorAutodiff<FactorForceTorque, 12, 3,3,3,4,3,6, 3,3,3,4>
 {
     public:
-        FactorForceTorque(const FeatureBasePtr&   _feat,
-                          const FrameBasePtr&     _frame_other,
-                          const StateBlockPtr&    _sb_bp_other,
-                          const ProcessorBasePtr& _processor_ptr,
-                          bool                    _apply_loss_function,
-                          FactorStatus            _status = FAC_ACTIVE);
-
-        ~FactorForceTorque() override { /* nothing */ }
-
-       /*
-        _ck   : COM position in world frame, time k
-        _cdk  : COM velocity in world frame, time k
-        _Lck  : Angular momentum in world frame, time k
-        _ckm  : COM position in world frame, time k-1
-        _cdkm : COM velocity in world frame, time k-1
-        _Lckm : Angular momentum in world frame, time k-1
-        _bpkm : COM position measurement bias, time k-1
-        _qkm  : Base orientation in world frame, time k-1
+        FactorForceTorque(const FeatureForceTorquePtr& _ftr_ptr,
+                                const CaptureForceTorquePtr& _cap_origin_ptr, // gets to bp1, bw1
+                                const ProcessorBasePtr&            _processor_ptr,
+                                const CaptureBasePtr&               _cap_ikin_other,
+                                const CaptureBasePtr&               _cap_gyro_other,
+                                bool                               _apply_loss_function,
+                                FactorStatus                       _status = FAC_ACTIVE);
+
+        ~FactorForceTorque() override = default;
+
+        /** \brief : compute the residual from the state blocks being iterated by the solver.
+            -> computes the expected measurement
+            -> corrects actual measurement with new bias
+            -> compares the corrected measurement with the expected one
+            -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
         */
         template<typename T>
-        bool operator () (
-            const T* const _ck,
-            const T* const _cdk,
-            const T* const _Lck,
-            const T* const _ckm,
-            const T* const _cdkm,
-            const T* const _Lckm,
-            const T* const _bpkm,
-            const T* const _qkm,
-            T* _res) const;
-
-        void computeJac(const FeatureForceTorqueConstPtr& _feat, 
-                        const double& _mass, 
-                        const double& _dt, 
-                        const Eigen::VectorXd& _bp, 
-                        Eigen::Matrix<double, 9, 15> & _J_ny_nz) const;
-
-        // void recomputeJac(const FeatureForceTorquePtr& _feat, 
-        //                   const double& _dt, 
-        //                   const Eigen::VectorXd& _bp, 
-        //                   Eigen::Matrix<double, 9, 15>& _J_ny_nz) const;
-
-        void retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov);
-
-        StateBlockPtr sb_bp_other_;
-        double mass_;
-        double dt_;
-        Eigen::Matrix<double,15,15> cov_meas_;
-        // Eigen::Matrix<double, 9, 15> J_ny_nz_;  // cannot be modified in operator() since const function 
-        // Eigen::Matrix9d errCov_;
+        bool operator ()(const T* const _c1,
+                         const T* const _cd1,
+                         const T* const _Lc1,
+                         const T* const _q1,
+                         const T* const _bp1,
+                         const T* const _bw1,
+                         const T* const _c2,
+                         const T* const _cd2,
+                         const T* const _Lc2,
+                         const T* const _q2,
+                         T* _res) const;
+
+        /** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
+            -> computes the expected measurement
+            -> corrects actual measurement with new bias
+            -> compares the corrected measurement with the expected one
+            -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
+         * params :
+         * _c1 : COM position at time t1 in world frame
+         * _cd1: COM velocity at time t1 in world frame
+         * _Lc1: Angular momentum at time t1 in world frame
+         * _q1 : Base orientation at time t1
+         * _bp1: COM position measurement  at time t1 in body frame
+         * _bw1: gyroscope bias at time t1 in body frame 
+         * _c2 : COM position at time t2 in world frame
+         * _cd2: COM velocity at time t2 in world frame
+         * _Lc2: Angular momentum at time t2 in world frame
+         * _q2 : Base orientation at time t2
+         * _res: Factor residuals (c,cd,Lc,o) O is rotation vector... NOT A QUATERNION
+        */
+        template<typename D1, typename D2, typename D3, typename D4>
+        bool residual(const MatrixBase<D1> &     _c1,
+                      const MatrixBase<D1> &     _cd1,
+                      const MatrixBase<D1> &     _Lc1,
+                      const QuaternionBase<D2> & _q1,
+                      const MatrixBase<D1> &     _bp1,
+                      const MatrixBase<D1> &     _bw1,
+                      const MatrixBase<D1> &     _c2,
+                      const MatrixBase<D1> &     _cd2,
+                      const MatrixBase<D1> &     _Lc2,
+                      const QuaternionBase<D3> & _q2,
+                      MatrixBase<D4> &           _res) const;
+
+        // Matrix<double,12,1> residual() const;
+        // double cost() const;
+
+    private:
+        /// Preintegrated delta
+        Vector3d    dc_preint_;
+        Vector3d    dcd_preint_;
+        Vector3d    dLc_preint_;
+        Quaterniond dq_preint_;
+
+        // Biases used during preintegration
+        Vector3d pbc_bias_preint_;
+        Vector3d gyro_bias_preint_;
+
+        // Jacobians of preintegrated deltas wrt biases
+        Matrix3d J_dLc_pb_;
+        Matrix3d J_dc_wb_;
+        Matrix3d J_dcd_wb_;
+        Matrix3d J_dLc_wb_;
+        Matrix3d J_dq_wb_;
+
+        /// Metrics
+        double dt_; ///< delta-time and delta-time-squared between keyframes
+        double mass_; ///< constant total robot mass
+        
 };
 
-} /* namespace wolf */
-
-
-namespace wolf {
-
-FactorForceTorque::FactorForceTorque(
-                            const FeatureBasePtr&   _feat,
-                            const FrameBasePtr&     _frame_other,
-                            const StateBlockPtr&    _sb_bp_other,
-                            const ProcessorBasePtr& _processor_ptr,
-                            bool                    _apply_loss_function,
-                            FactorStatus            _status) :
-    FactorAutodiff("FactorForceTorque",
-                   TOP_GEOM,
-                   _feat,
-                    _frame_other,              // _frame_other_ptr
-                    nullptr,                   // _capture_other_ptr
-                    nullptr,                   // _feature_other_ptr
-                    nullptr,                   // _landmark_other_ptr
-                    _processor_ptr,
-                    _apply_loss_function,
-                    _status,
-                    _feat->getFrame()->getStateBlock('C'),  // COM position, current
-                    _feat->getFrame()->getStateBlock('D'),  // COM velocity (bad name), current
-                    _feat->getFrame()->getStateBlock('L'),  // Angular momentum, current
-                    _frame_other->getStateBlock('C'),       // COM position, previous
-                    _frame_other->getStateBlock('D'),       // COM velocity (bad name), previous
-                    _frame_other->getStateBlock('L'),       // Angular momentum, previous
-                    _sb_bp_other,  // BC relative position bias, previous
-                    _frame_other->getStateBlock('O')        // Base orientation, previous
-    ),
-    sb_bp_other_(_sb_bp_other)
+///////////////////// IMPLEMENTATION ////////////////////////////
+
+inline FactorForceTorque::FactorForceTorque(
+                            const FeatureForceTorquePtr& _ftr_ptr,
+                            const CaptureForceTorquePtr& _cap_origin_ptr,
+                            const ProcessorBasePtr&            _processor_ptr,
+                            const CaptureBasePtr&              _cap_ikin_other,
+                            const CaptureBasePtr&              _cap_gyro_other,
+                            bool                               _apply_loss_function,
+                            FactorStatus                       _status) :
+                FactorAutodiff<FactorForceTorque, 12, 3,3,3,4,3,6, 3,3,3,4>(
+                        "FactorForceTorque",
+                        TOP_MOTION,
+                        _ftr_ptr,
+                        _cap_origin_ptr->getFrame(),
+                        _cap_origin_ptr,
+                        nullptr,
+                        nullptr,
+                        _processor_ptr,
+                        _apply_loss_function,
+                        _status,
+                        _cap_origin_ptr->getFrame()->getStateBlock('C'),
+                        _cap_origin_ptr->getFrame()->getStateBlock('D'),
+                        _cap_origin_ptr->getFrame()->getStateBlock('L'),
+                        _cap_origin_ptr->getFrame()->getO(),
+                        _cap_ikin_other->getSensorIntrinsic(),
+                        _cap_gyro_other->getSensorIntrinsic(),
+                        _ftr_ptr->getFrame()->getStateBlock('C'),
+                        _ftr_ptr->getFrame()->getStateBlock('D'),
+                        _ftr_ptr->getFrame()->getStateBlock('L'),
+                        _ftr_ptr->getFrame()->getO()
+                        ),
+        dc_preint_(_ftr_ptr->getMeasurement().head<3>()), // dc, dcd, dLc, dq at preintegration time
+        dcd_preint_(_ftr_ptr->getMeasurement().segment<3>(3)),
+        dLc_preint_(_ftr_ptr->getMeasurement().segment<3>(6)),
+        dq_preint_(_ftr_ptr->getMeasurement().data()+9),
+        pbc_bias_preint_( std::static_pointer_cast<FeatureForceTorque>(_ftr_ptr)->getPbcBiasPreint()),
+        gyro_bias_preint_(std::static_pointer_cast<FeatureForceTorque>(_ftr_ptr)->getGyroBiasPreint()),
+        J_dLc_pb_(_ftr_ptr->getJacobianBias().block<3,3>(6,0)), // Jac dLc wrt pbc bias
+        J_dc_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(0,3)), // Jac dc wrt gyro bias
+        J_dcd_wb_(_ftr_ptr->getJacobianBias().block<3,3>(3,3)), // Jac dc wrt gyro bias
+        J_dLc_wb_(_ftr_ptr->getJacobianBias().block<3,3>(6,3)), // Jac dcd wrt gyro bias
+        J_dq_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(9,3)), // Jac dLc wrt gyro bias
+        dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getFrame()->getTimeStamp()),
+        mass_(std::static_pointer_cast<SensorForceTorque>(_ftr_ptr->getCapture()->getSensor())->getMass())
 {
-    FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(_feat);
-    mass_ = feat->getMass();
-    dt_   = feat->getDt();
-    retrieveMeasurementCovariance(feat, cov_meas_);
+//
 }
 
-
-void FactorForceTorque::retrieveMeasurementCovariance(const FeatureForceTorquePtr& feat, Eigen::Matrix<double,15,15>& cov)
+template<typename T>
+inline bool FactorForceTorque::operator ()(const T* const _c1,
+                                                 const T* const _cd1,
+                                                 const T* const _Lc1,
+                                                 const T* const _q1,
+                                                 const T* const _bp1,
+                                                 const T* const _bw1,
+                                                 const T* const _c2,
+                                                 const T* const _cd2,
+                                                 const T* const _Lc2,
+                                                 const T* const _q2,
+                                                 T* _res) const
 {
-    cov.setZero();
-    cov.block<6,6>(0,0) =   feat->getCovForcesMeas();  // reset some extra zeros
-    cov.block<6,6>(6,6) =   feat->getCovTorquesMeas();  // reset some extra zeros
-    cov.block<3,3>(12,12) = feat->getCovPbcMeas();
-}
+    Map<const Matrix<T,3,1> > c1(_c1);
+    Map<const Matrix<T,3,1> > cd1(_cd1);
+    Map<const Matrix<T,3,1> > Lc1(_Lc1);
+    Map<const Quaternion<T> > q1(_q1);
+    Map<const Matrix<T,3,1> > bp1(_bp1);
+    Map<const Matrix<T,3,1> > bw1(_bw1 + 3);  // bw1 = bimu = [ba, bw]
+    Map<const Matrix<T,3,1> > c2(_c2);
+    Map<const Matrix<T,3,1> > cd2(_cd2);
+    Map<const Matrix<T,3,1> > Lc2(_Lc2);
+    Map<const Quaternion<T> > q2(_q2);
+    Map<Matrix<T,12,1> > res(_res);
+
+    residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res);
 
+    return true;
+}
 
-void FactorForceTorque::computeJac(const FeatureForceTorqueConstPtr& _feat, 
-                                   const double& _mass, 
-                                   const double& _dt,  
-                                   const Eigen::VectorXd& _bp, 
-                                   Eigen::Matrix<double, 9, 15>& _J_ny_nz) const
+template<typename D1, typename D2, typename D3, typename D4>
+inline bool FactorForceTorque::residual(const MatrixBase<D1> &     _c1,
+                                              const MatrixBase<D1> &     _cd1,
+                                              const MatrixBase<D1> &     _Lc1,
+                                              const QuaternionBase<D2> & _q1,
+                                              const MatrixBase<D1> &     _bp1,
+                                              const MatrixBase<D1> &     _bw1,
+                                              const MatrixBase<D1> &     _c2,
+                                              const MatrixBase<D1> &     _cd2,
+                                              const MatrixBase<D1> &     _Lc2,
+                                              const QuaternionBase<D3> & _q2,
+                                              MatrixBase<D4> &           _res) const
 {
-    _J_ny_nz.setZero();
-
-    // Measurements retrieval
-    Eigen::Map<const Eigen::Vector3d>    f1  (_feat->getForcesMeas().data());
-    Eigen::Map<const Eigen::Vector3d>    f2  (_feat->getForcesMeas().data()    + 3  );
-    Eigen::Map<const Eigen::Vector3d>    tau1(_feat->getTorquesMeas().data());
-    Eigen::Map<const Eigen::Vector3d>    tau2(_feat->getTorquesMeas().data()   + 3 );
-    Eigen::Map<const Eigen::Vector3d>    pbl1(_feat->getKinMeas().data());
-    Eigen::Map<const Eigen::Vector3d>    pbl2(_feat->getKinMeas().data()       + 3 );
-    Eigen::Map<const Eigen::Quaterniond> bql1(_feat->getKinMeas().data()       + 6);
-    Eigen::Map<const Eigen::Quaterniond> bql2(_feat->getKinMeas().data()   + 10);
-    Eigen::Map<const Eigen::Vector3d>    pbc (_feat->getPbcMeas().data());
-
-    Eigen::Matrix3d bRl1 = q2R(bql1);
-    Eigen::Matrix3d bRl2 = q2R(bql2);
-    _J_ny_nz.block<3,3>(0,0)  = 0.5*bRl1*pow(_dt,2)/_mass;
-    _J_ny_nz.block<3,3>(0,3)  = 0.5*bRl2*pow(_dt,2)/_mass;
-    _J_ny_nz.block<3,3>(3,0)  = bRl1*_dt/_mass;
-    _J_ny_nz.block<3,3>(3,3)  = bRl2*_dt/_mass;
-    _J_ny_nz.block<3,3>(6,0)  = skew(pbl1 - pbc + _bp)*bRl1*_dt;
-    _J_ny_nz.block<3,3>(6,3)  = skew(pbl2 - pbc + _bp)*bRl2*_dt;
-    _J_ny_nz.block<3,3>(6,6)  = bRl1*_dt;
-    _J_ny_nz.block<3,3>(6,9)  = bRl2*_dt;
-    _J_ny_nz.block<3,3>(6,12) = (skew(bRl1*f1) + skew(bRl2*f2))*_dt;
+    /*  Help for the Imu residual function
+     *
+     *  Notations:
+     *    D_* : a motion in the Delta manifold                           -- implemented as 10-vectors with [Dp, Dq, Dv]
+     *    T_* : a motion difference in the Tangent space to the manifold -- implemented as  9-vectors with [Dp, Do, Dv]
+     *    b*  : a bias
+     *    J*  : a Jacobian matrix
+     *
+     *  We use the following functions from imu_tools.h:
+     *    D  = betweenStates(x1,x2,dt) : obtain a Delta from two states separated dt=t2-t1
+     *    D2 = plus(D1,T)              : plus operator,  D2 = D1 (+) T
+     *    T  = diff(D1,D2)             : minus operator, T  = D2 (-) D1
+     *
+     *  Two methods are possible (select with #define below this note) :
+     *
+     *  Computations common to methods 1 and 2:
+     *    D_exp  = betweenStates(x1,x2,dt)   // Predict delta from states
+     *    T_step = J_preint * (b - b_preint) // compute the delta correction step due to bias change
+     *
+     *  Method 1:
+     *    D_corr = plus(D_preint, T_step)    // correct the pre-integrated delta with correction step due to bias change
+     *    T_err  = diff(D_exp, D_corr)       // compare against expected delta
+     *    res    = W.sqrt * T_err
+     *
+     *   results in:
+     *    res    = W.sqrt * ( diff ( D_exp, plus(D_preint, J_preint * (b - b_preint) ) ) )
+     *
+     *  Method 2:
+     *    T_diff = diff(D_preint, D_exp)     // compare pre-integrated against expected delta
+     *    T_err  = T_diff - T_step           // the difference should match the correction step due to bias change
+     *    res    = W.sqrt * err
+     *
+     *   results in :
+     *    res    = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b - b_preint) )
+     *
+     * NOTE: See optimization report at the end of this file for comparisons of both methods.
+     */
+
+    //needed typedefs
+    typedef typename D1::Scalar T;
+
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D4, 12);
+
+    // 1. Expected delta from states
+    Matrix<T, 3, 1 >   dc_exp;
+    Matrix<T, 3, 1 >   dcd_exp;
+    Matrix<T, 3, 1 >   dLc_exp;
+    Quaternion<T>      dq_exp;
+
+    bodydynamics::betweenStates(_c1, _cd1, _Lc1, _q1, _c2, _cd2, _Lc2, _q2, dt_, dc_exp, dcd_exp, dLc_exp, dq_exp);
+
+    // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
+
+    // 2.a. Compute the delta step in tangent space:   step = J_bias * (bias - bias_preint)
+    Matrix<T, 3, 1> dc_step  =                                          J_dc_wb_ * (_bw1 - gyro_bias_preint_);
+    Matrix<T, 3, 1> dcd_step =                                         J_dcd_wb_ * (_bw1 - gyro_bias_preint_);
+    Matrix<T, 3, 1> dLc_step = J_dLc_pb_ * (_bp1 - pbc_bias_preint_) + J_dLc_wb_ * (_bw1 - gyro_bias_preint_);
+    Matrix<T, 3, 1> do_step  =                                          J_dq_wb_ * (_bw1 - gyro_bias_preint_);
+
+    // 2.b. Add the correction step to the preintegrated delta:    delta_cor = delta_preint (+) step
+    Matrix<T,3,1> dc_correct;
+    Matrix<T,3,1> dcd_correct;
+    Matrix<T,3,1> dLc_correct;
+    Quaternion<T> dq_correct;
+
+    bodydynamics::plus(dc_preint_.cast<T>(), dcd_preint_.cast<T>(), dLc_preint_.cast<T>(), dq_preint_.cast<T>(),
+              dc_step, dcd_step, dLc_step, do_step,
+              dc_correct, dcd_correct, dLc_correct, dq_correct);
+
+    // 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
+    // Note the Dt here is zero because it's the delta-time between the same time stamps!
+    Matrix<T, 12, 1> d_error;
+    Map<Matrix<T, 3, 1> >   dc_error (d_error.data()    );
+    Map<Matrix<T, 3, 1> >   dcd_error(d_error.data() + 3);
+    Map<Matrix<T, 3, 1> >   dLc_error(d_error.data() + 6);
+    Map<Matrix<T, 3, 1> >   do_error (d_error.data() + 9);
+
+
+    bodydynamics::diff(dc_exp, dcd_exp, dLc_exp, dq_exp,
+              dc_correct, dcd_correct, dLc_correct, dq_correct, 
+              dc_error, dcd_error, dLc_error, do_error);
+
+    _res = getMeasurementSquareRootInformationUpper() * d_error;
+
+    return true;
 }
 
-// void FactorForceTorque::recomputeJac(const FeatureForceTorquePtr& _feat, 
-//                                      const double& _dt, 
-//                                      const Eigen::VectorXd& _bp, 
-//                                      Eigen::Matrix<double, 9, 15>& _J_ny_nz)
+// Matrix<double,12,1> FactorForceTorque::residual() const
 // {
-//     FeatureForceTorquePtr feat = std::static_pointer_cast<FeatureForceTorque>(_feat);
-
-//     // Measurements retrieval
-//     // Eigen::Map<const Eigen::Vector3d>    f1  (feat->getForcesMeas().data());
-//     // Eigen::Map<const Eigen::Vector3d>    f2  (feat->getForcesMeas().data()    + 3  );
-//     // Eigen::Map<const Eigen::Vector3d>    tau1(feat->getTorquesMeas().data());
-//     // Eigen::Map<const Eigen::Vector3d>    tau2(feat->getTorquesMeas().data()   + 3 );
-//     Eigen::Map<const Eigen::Vector3d>    pbl1(feat->getKinMeas().data());
-//     Eigen::Map<const Eigen::Vector3d>    pbl2(feat->getKinMeas().data()       + 3 );
-//     Eigen::Map<const Eigen::Quaterniond> bql1(feat->getKinMeas().data()       + 6);
-//     Eigen::Map<const Eigen::Quaterniond> bql2(feat->getKinMeas().data()   + 10);
-//     Eigen::Map<const Eigen::Vector3d>    pbc (feat->getPbcMeas().data());
-
-//     Eigen::Matrix3d bRl1 = q2R(bql1); 
-//     Eigen::Matrix3d bRl2 = q2R(bql2); 
-//     // _J_ny_nz.block<3,3>(0,0)  = 0.5*bRl1*pow(_dt,2)/_mass;
-//     // _J_ny_nz.block<3,3>(0,3)  = 0.5*bRl2*pow(_dt,2)/_mass;
-//     // _J_ny_nz.block<3,3>(3,0)  = bRl1*_dt/_mass;
-//     // _J_ny_nz.block<3,3>(3,3)  = bRl2*_dt/_mass;
-//     _J_ny_nz.block<3,3>(6,0)  = skew(pbl1 - pbc + _bp)*bRl1*_dt;
-//     _J_ny_nz.block<3,3>(6,3)  = skew(pbl2 - pbc + _bp)*bRl2*_dt;
-//     // _J_ny_nz.block<3,3>(6,6)  = bRl1*_dt;    
-//     // _J_ny_nz.block<3,3>(6,9)  = bRl2*_dt;    
-//     // _J_ny_nz.block<3,3>(6,12) = (skew(bRl1*f1) + skew(bRl2*f2))*_dt;     
-// }
+//     Matrix<double,12,1> res;
 
 
-template<typename T>
-bool FactorForceTorque::operator () (
-            const T* const _ck,
-            const T* const _cdk,
-            const T* const _Lck,
-            const T* const _ckm,
-            const T* const _cdkm,
-            const T* const _Lckm,
-            const T* const _bpkm,
-            const T* const _qkm,
-            T* _res) const   
-{
-    auto feat = std::static_pointer_cast<const FeatureForceTorque>(getFeature());
-
-    // State variables instanciation
-    Eigen::Map<const Eigen::Matrix<T,3,1> > ck(_ck);
-    Eigen::Map<const Eigen::Matrix<T,3,1> > cdk(_cdk);
-    Eigen::Map<const Eigen::Matrix<T,3,1> > Lck(_Lck);
-    Eigen::Map<const Eigen::Matrix<T,3,1> > ckm(_ckm);
-    Eigen::Map<const Eigen::Matrix<T,3,1> > cdkm(_cdkm);
-    Eigen::Map<const Eigen::Matrix<T,3,1> > Lckm(_Lckm);
-    Eigen::Map<const Eigen::Matrix<T,3,1> > bpkm(_bpkm);
-    Eigen::Map<const Eigen::Quaternion<T> > qkm(_qkm);
-    Eigen::Map<Eigen::Matrix<T,9,1> > res(_res);
-
-    // Retrieve all measurements
-    Eigen::Map<const Eigen::Vector3d>    f1  (feat->getForcesMeas().data());
-    Eigen::Map<const Eigen::Vector3d>    f2  (feat->getForcesMeas().data()    + 3  );
-    Eigen::Map<const Eigen::Vector3d>    tau1(feat->getTorquesMeas().data());
-    Eigen::Map<const Eigen::Vector3d>    tau2(feat->getTorquesMeas().data()   + 3 );
-    Eigen::Map<const Eigen::Vector3d>    pbc (feat->getPbcMeas().data());
-    Eigen::Map<const Eigen::Vector3d>    pbl1(feat->getKinMeas().data());
-    Eigen::Map<const Eigen::Vector3d>    pbl2(feat->getKinMeas().data()       + 3 );
-    Eigen::Map<const Eigen::Quaterniond> bql1(feat->getKinMeas().data()       + 6);
-    Eigen::Map<const Eigen::Quaterniond> bql2(feat->getKinMeas().data()   + 10);
-
-    // Recompute the error variable covariance according to the new bias bp estimation
-
-    Eigen::Matrix<double, 9, 15> J_ny_nz;
-    computeJac(feat, mass_, dt_, sb_bp_other_->getState(), J_ny_nz);  // bp
-    Eigen::Matrix9d errCov = J_ny_nz * cov_meas_ * J_ny_nz.transpose();
-    errCov.block<6,6>(0,0) = errCov.block<6,6>(0,0) + 1e-12 * Eigen::Matrix6d::Identity();
-
-    // Error variable instanciation
-    Eigen::Matrix<T, 9, 1> err;
-    Eigen::Map<Eigen::Matrix<T, 3, 1> >   err_c (err.data());
-    Eigen::Map<Eigen::Matrix<T, 3, 1> >   err_cd(err.data() + 3);
-    Eigen::Map<Eigen::Matrix<T, 3, 1> >   err_Lc(err.data() + 6);
-
-    err_c = qkm.conjugate()*(ck - ckm - cdkm * dt_ - 0.5*wolf::gravity()*pow(dt_, 2))
-          - (0.5/mass_) * (bql1 * f1 * pow(dt_,2) + bql2 * f2 * pow(dt_,2));
-
-    err_cd = qkm.conjugate()*(cdk - cdkm - wolf::gravity()*dt_)
-           - (1/mass_) * (bql1 * f1 * dt_ + bql2 * f2 * dt_);
-    
-    err_Lc = qkm.conjugate()*(Lck - Lckm)
-           - (  bql1 * tau1 + (pbl1 - pbc + bpkm).cross(bql1 * f1) 
-              + bql2 * tau2 + (pbl2 - pbc + bpkm).cross(bql2 * f2));
-    
-    res = wolf::computeSqrtUpper(errCov.inverse()) * err;
+//     FrameBasePtr frm_prev = getFrameOther();
+//     FrameBasePtr frm_curr = getFeature()->getFrame();
 
-    return true;
-}
+//     CaptureBaseWPtrList cap_lst = getCaptureOtherList();  // !! not retrieving the rigt captures...
+//     auto cap_ikin_other = cap_lst.front().lock(); 
+//     auto cap_gyro_other = cap_lst.back().lock();
+
+//     Map<const Matrix<double,3,1> > c1( frm_prev->getStateBlock('C')->getState().data());
+//     Map<const Matrix<double,3,1> > cd1(frm_prev->getStateBlock('D')->getState().data());
+//     Map<const Matrix<double,3,1> > Lc1(frm_prev->getStateBlock('L')->getState().data());
+//     Map<const Quaternion<double> > q1( frm_prev->getStateBlock('O')->getState().data());
+//     Map<const Matrix<double,3,1> > bp1(cap_ikin_other->getStateBlock('I')->getState().data());
+//     Map<const Matrix<double,3,1> > bw1(cap_gyro_other->getStateBlock('I')->getState().data() + 3);  // bw1 = bimu = [ba, bw]
+//     Map<const Matrix<double,3,1> > c2 (frm_curr->getStateBlock('C')->getState().data());
+//     Map<const Matrix<double,3,1> > cd2(frm_curr->getStateBlock('D')->getState().data());
+//     Map<const Matrix<double,3,1> > Lc2(frm_curr->getStateBlock('L')->getState().data());
+//     Map<const Quaternion<double> > q2 (frm_curr->getStateBlock('O')->getState().data());
+
+//     residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res);
+
+//     return res;
+// }
+
+// double FactorForceTorque::cost() const
+// {
+//     Matrix<double,12,1> toto = residual();
+// }
 
 } // namespace wolf
 
-#endif /* FACTOR_FORCE_TORQUE_H_ */
+#endif
diff --git a/include/bodydynamics/factor/factor_force_torque_inertial.h b/include/bodydynamics/factor/factor_force_torque_inertial.h
index 51c60184de4a56d64da4f20992a7ade895baea72..cfea278e30816f355353fbc833d4647c66cf6726 100644
--- a/include/bodydynamics/factor/factor_force_torque_inertial.h
+++ b/include/bodydynamics/factor/factor_force_torque_inertial.h
@@ -35,9 +35,9 @@ WOLF_PTR_TYPEDEFS(FactorForceTorqueInertial);
 
 /**
  * @brief Pre-integrated factor taking IMU and force-torque measurements (FTI)
- * 
+ *
  * This factor evaluates the preintegrated FTI against pose, linear velocity, IMU bias and angular momentum.
- * 
+ *
  * State blocks considered:
  *  - Frame 1 (origin)
  *    'P' position
@@ -51,7 +51,7 @@ WOLF_PTR_TYPEDEFS(FactorForceTorqueInertial);
  *    'V' velocity
  *    'O' orientation
  *    'L' angular momentum
- * 
+ *
  * The residual computed has the following components, in this order
  *  - position delta error according to IMU preintegration
  *  - velocity delta error according to IMU preintegration
@@ -191,10 +191,11 @@ inline bool FactorForceTorqueInertial::residual(const MatrixBase<D1>&     _p1,
     fti::betweenStates(_p1, _v1, _L1, _q1, _p2, _v2, _L2, _q2, dt_, dpi, dvi, dpd, dvd, dL, dq);
 
     Matrix<T, 19, 1> delta_preint = delta_preint_.cast<T>();
-    Matrix<T, 18, 1> delta_step   = J_delta_bias_ * (_b1 - bias_preint_); // canviar _b1 per calib = (I,C,i,m), de mida 13 Matrix<T,13,1>
-    Matrix<T, 19, 1> delta_corr   = fti::plus(delta_preint, delta_step);
-    Matrix<T, 18, 1> delta_err    = fti::diff(delta_est, delta_corr);
-    _res                          = sqrt_info_upper_ * delta_err;
+    Matrix<T, 18, 1> delta_step =
+        J_delta_bias_ * (_b1 - bias_preint_);  // canviar _b1 per calib = (I,C,i,m), de mida 13 Matrix<T,13,1>
+    Matrix<T, 19, 1> delta_corr = fti::plus(delta_preint, delta_step);
+    Matrix<T, 18, 1> delta_err  = fti::diff(delta_est, delta_corr);
+    _res                        = sqrt_info_upper_ * delta_err;
 
     return true;
 }
diff --git a/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
index f33b5d10d34d1bd659fbe5aca49e92dfac05a7b8..abece6e0d706e25da604a03d8c13f70dafdb2ed6 100644
--- a/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
+++ b/include/bodydynamics/factor/factor_force_torque_inertial_dynamics.h
@@ -149,9 +149,9 @@ inline FactorForceTorqueInertialDynamics::FactorForceTorqueInertialDynamics(cons
           _ftr->getFrame()->getStateBlock('O'),               // current frame O
           _ftr->getFrame()->getStateBlock('V'),               // current frame V
           _ftr->getFrame()->getStateBlock('L'),               // current frame L
-          _capture_origin->getSensor()->getStateBlock('C'),   // sensor C
-          _capture_origin->getSensor()->getStateBlock('i'),   // sensor i
-          _capture_origin->getSensor()->getStateBlock('m')),  // sensor m
+          _ftr->getCapture()->getStateBlock('C'),             // sensor C
+          _ftr->getCapture()->getStateBlock('i'),             // sensor i
+          _ftr->getCapture()->getStateBlock('m')),            // sensor m
       dt_(_ftr->getFrame()->getTimeStamp() - _capture_origin->getFrame()->getTimeStamp()),
       delta_preint_(_ftr->getDeltaPreint()),
       calib_preint_(_ftr->getCalibrationPreint()),
@@ -173,7 +173,7 @@ inline bool FactorForceTorqueInertialDynamics::residual(const MatrixBase<D1>&
                                                         const MatrixBase<D3>&     _L2,
                                                         const MatrixBase<D6>&     _C,
                                                         const MatrixBase<D6>&     _i,
-                                                        const D7&                  _m,
+                                                        const D7&                 _m,
                                                         MatrixBase<D8>&           _res) const
 {
     MatrixSizeCheck<18, 1>::check(_res);
@@ -228,21 +228,21 @@ inline bool FactorForceTorqueInertialDynamics::residual(const MatrixBase<D1>&
 
 inline VectorXd FactorForceTorqueInertialDynamics::residual() const
 {
-    VectorXd res(18);
-    Vector3d p0 = getFrameOther()->getStateBlock('P')->getState(); // previous frame P
-    Quaterniond q0 = Quaterniond(getFrameOther()->getStateBlock('O')->getState().data());
-    Vector3d v0 = getFrameOther()->getStateBlock('V')->getState();
-    Vector3d L0 = getFrameOther()->getStateBlock('L')->getState();
-    Vector6d bias = getCaptureOther()->getSensorIntrinsic()->getState();
-    Vector3d p1 = getFrame()->getStateBlock('P')->getState(); // previous frame P
-    Quaterniond q1 = Quaterniond(getFrame()->getStateBlock('O')->getState().data());
-    Vector3d v1 = getFrame()->getStateBlock('V')->getState();
-    Vector3d L1 = getFrame()->getStateBlock('L')->getState();
-    Vector3d C = getCapture()->getSensor()->getStateBlock('C')->getState();
-    Vector3d i = getCapture()->getSensor()->getStateBlock('i')->getState();
-    double m = getCapture()->getSensor()->getStateBlock('m')->getState()(0);
+    VectorXd    res(18);
+    Vector3d    p0   = getFrameOther()->getStateBlock('P')->getState();  // previous frame P
+    Quaterniond q0   = Quaterniond(getFrameOther()->getStateBlock('O')->getState().data());
+    Vector3d    v0   = getFrameOther()->getStateBlock('V')->getState();
+    Vector3d    L0   = getFrameOther()->getStateBlock('L')->getState();
+    Vector6d    bias = getCaptureOther()->getSensorIntrinsic()->getState();
+    Vector3d    p1   = getFrame()->getStateBlock('P')->getState();  // previous frame P
+    Quaterniond q1   = Quaterniond(getFrame()->getStateBlock('O')->getState().data());
+    Vector3d    v1   = getFrame()->getStateBlock('V')->getState();
+    Vector3d    L1   = getFrame()->getStateBlock('L')->getState();
+    Vector3d    C    = getCapture()->getSensor()->getStateBlockDynamic('C')->getState();
+    Vector3d    i    = getCapture()->getSensor()->getStateBlockDynamic('i')->getState();
+    double      m    = getCapture()->getSensor()->getStateBlockDynamic('m')->getState()(0);
 
-    residual(p0, q0, v0, L0, bias, p1, q1, v1, L1, C, i, m, res); 
+    residual(p0, q0, v0, L0, bias, p1, q1, v1, L1, C, i, m, res);
     return res;
 }
 
@@ -259,7 +259,7 @@ inline bool FactorForceTorqueInertialDynamics::operator()(const T* const _p1,
                                                           const T* const _C,
                                                           const T* const _i,
                                                           const T* const _m,
-                                                          T* _res) const
+                                                          T*             _res) const
 {
     Map<const Matrix<T, 3, 1>> p1(_p1);
     Map<const Quaternion<T>>   q1(_q1);
diff --git a/include/bodydynamics/factor/factor_force_torque_preint.h b/include/bodydynamics/factor/factor_force_torque_preint.h
deleted file mode 100644
index 841bb63d6f94ffa9dc4ec115e5f4a3c3dd06d20a..0000000000000000000000000000000000000000
--- a/include/bodydynamics/factor/factor_force_torque_preint.h
+++ /dev/null
@@ -1,343 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef FACTOR_FORCE_TORQUE_PREINT_THETA_H_
-#define FACTOR_FORCE_TORQUE_PREINT_THETA_H_
-
-//Wolf includes
-#include "bodydynamics/capture/capture_force_torque_preint.h"
-#include "bodydynamics/feature/feature_force_torque_preint.h"
-#include "bodydynamics/sensor/sensor_force_torque.h"
-#include "core/factor/factor_autodiff.h"
-#include "core/math/rotations.h"
-
-//Eigen include
-
-namespace wolf {
-    
-WOLF_PTR_TYPEDEFS(FactorForceTorquePreint);
-
-//class
-class FactorForceTorquePreint : public FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4>
-{
-    public:
-        FactorForceTorquePreint(const FeatureForceTorquePreintPtr& _ftr_ptr,
-                                const CaptureForceTorquePreintPtr& _cap_origin_ptr, // gets to bp1, bw1
-                                const ProcessorBasePtr&            _processor_ptr,
-                                const CaptureBasePtr&               _cap_ikin_other,
-                                const CaptureBasePtr&               _cap_gyro_other,
-                                bool                               _apply_loss_function,
-                                FactorStatus                       _status = FAC_ACTIVE);
-
-        ~FactorForceTorquePreint() override = default;
-
-        /** \brief : compute the residual from the state blocks being iterated by the solver.
-            -> computes the expected measurement
-            -> corrects actual measurement with new bias
-            -> compares the corrected measurement with the expected one
-            -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
-        */
-        template<typename T>
-        bool operator ()(const T* const _c1,
-                         const T* const _cd1,
-                         const T* const _Lc1,
-                         const T* const _q1,
-                         const T* const _bp1,
-                         const T* const _bw1,
-                         const T* const _c2,
-                         const T* const _cd2,
-                         const T* const _Lc2,
-                         const T* const _q2,
-                         T* _res) const;
-
-        /** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
-            -> computes the expected measurement
-            -> corrects actual measurement with new bias
-            -> compares the corrected measurement with the expected one
-            -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
-         * params :
-         * _c1 : COM position at time t1 in world frame
-         * _cd1: COM velocity at time t1 in world frame
-         * _Lc1: Angular momentum at time t1 in world frame
-         * _q1 : Base orientation at time t1
-         * _bp1: COM position measurement  at time t1 in body frame
-         * _bw1: gyroscope bias at time t1 in body frame 
-         * _c2 : COM position at time t2 in world frame
-         * _cd2: COM velocity at time t2 in world frame
-         * _Lc2: Angular momentum at time t2 in world frame
-         * _q2 : Base orientation at time t2
-         * _res: Factor residuals (c,cd,Lc,o) O is rotation vector... NOT A QUATERNION
-        */
-        template<typename D1, typename D2, typename D3, typename D4>
-        bool residual(const MatrixBase<D1> &     _c1,
-                      const MatrixBase<D1> &     _cd1,
-                      const MatrixBase<D1> &     _Lc1,
-                      const QuaternionBase<D2> & _q1,
-                      const MatrixBase<D1> &     _bp1,
-                      const MatrixBase<D1> &     _bw1,
-                      const MatrixBase<D1> &     _c2,
-                      const MatrixBase<D1> &     _cd2,
-                      const MatrixBase<D1> &     _Lc2,
-                      const QuaternionBase<D3> & _q2,
-                      MatrixBase<D4> &           _res) const;
-
-        // Matrix<double,12,1> residual() const;
-        // double cost() const;
-
-    private:
-        /// Preintegrated delta
-        Vector3d    dc_preint_;
-        Vector3d    dcd_preint_;
-        Vector3d    dLc_preint_;
-        Quaterniond dq_preint_;
-
-        // Biases used during preintegration
-        Vector3d pbc_bias_preint_;
-        Vector3d gyro_bias_preint_;
-
-        // Jacobians of preintegrated deltas wrt biases
-        Matrix3d J_dLc_pb_;
-        Matrix3d J_dc_wb_;
-        Matrix3d J_dcd_wb_;
-        Matrix3d J_dLc_wb_;
-        Matrix3d J_dq_wb_;
-
-        /// Metrics
-        double dt_; ///< delta-time and delta-time-squared between keyframes
-        double mass_; ///< constant total robot mass
-        
-};
-
-///////////////////// IMPLEMENTATION ////////////////////////////
-
-inline FactorForceTorquePreint::FactorForceTorquePreint(
-                            const FeatureForceTorquePreintPtr& _ftr_ptr,
-                            const CaptureForceTorquePreintPtr& _cap_origin_ptr,
-                            const ProcessorBasePtr&            _processor_ptr,
-                            const CaptureBasePtr&              _cap_ikin_other,
-                            const CaptureBasePtr&              _cap_gyro_other,
-                            bool                               _apply_loss_function,
-                            FactorStatus                       _status) :
-                FactorAutodiff<FactorForceTorquePreint, 12, 3,3,3,4,3,6, 3,3,3,4>(
-                        "FactorForceTorquePreint",
-                        TOP_MOTION,
-                        _ftr_ptr,
-                        _cap_origin_ptr->getFrame(),
-                        _cap_origin_ptr,
-                        nullptr,
-                        nullptr,
-                        _processor_ptr,
-                        _apply_loss_function,
-                        _status,
-                        _cap_origin_ptr->getFrame()->getStateBlock('C'),
-                        _cap_origin_ptr->getFrame()->getStateBlock('D'),
-                        _cap_origin_ptr->getFrame()->getStateBlock('L'),
-                        _cap_origin_ptr->getFrame()->getO(),
-                        _cap_ikin_other->getSensorIntrinsic(),
-                        _cap_gyro_other->getSensorIntrinsic(),
-                        _ftr_ptr->getFrame()->getStateBlock('C'),
-                        _ftr_ptr->getFrame()->getStateBlock('D'),
-                        _ftr_ptr->getFrame()->getStateBlock('L'),
-                        _ftr_ptr->getFrame()->getO()
-                        ),
-        dc_preint_(_ftr_ptr->getMeasurement().head<3>()), // dc, dcd, dLc, dq at preintegration time
-        dcd_preint_(_ftr_ptr->getMeasurement().segment<3>(3)),
-        dLc_preint_(_ftr_ptr->getMeasurement().segment<3>(6)),
-        dq_preint_(_ftr_ptr->getMeasurement().data()+9),
-        pbc_bias_preint_( std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getPbcBiasPreint()),
-        gyro_bias_preint_(std::static_pointer_cast<FeatureForceTorquePreint>(_ftr_ptr)->getGyroBiasPreint()),
-        J_dLc_pb_(_ftr_ptr->getJacobianBias().block<3,3>(6,0)), // Jac dLc wrt pbc bias
-        J_dc_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(0,3)), // Jac dc wrt gyro bias
-        J_dcd_wb_(_ftr_ptr->getJacobianBias().block<3,3>(3,3)), // Jac dc wrt gyro bias
-        J_dLc_wb_(_ftr_ptr->getJacobianBias().block<3,3>(6,3)), // Jac dcd wrt gyro bias
-        J_dq_wb_ (_ftr_ptr->getJacobianBias().block<3,3>(9,3)), // Jac dLc wrt gyro bias
-        dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getFrame()->getTimeStamp()),
-        mass_(std::static_pointer_cast<SensorForceTorque>(_ftr_ptr->getCapture()->getSensor())->getMass())
-{
-//
-}
-
-template<typename T>
-inline bool FactorForceTorquePreint::operator ()(const T* const _c1,
-                                                 const T* const _cd1,
-                                                 const T* const _Lc1,
-                                                 const T* const _q1,
-                                                 const T* const _bp1,
-                                                 const T* const _bw1,
-                                                 const T* const _c2,
-                                                 const T* const _cd2,
-                                                 const T* const _Lc2,
-                                                 const T* const _q2,
-                                                 T* _res) const
-{
-    Map<const Matrix<T,3,1> > c1(_c1);
-    Map<const Matrix<T,3,1> > cd1(_cd1);
-    Map<const Matrix<T,3,1> > Lc1(_Lc1);
-    Map<const Quaternion<T> > q1(_q1);
-    Map<const Matrix<T,3,1> > bp1(_bp1);
-    Map<const Matrix<T,3,1> > bw1(_bw1 + 3);  // bw1 = bimu = [ba, bw]
-    Map<const Matrix<T,3,1> > c2(_c2);
-    Map<const Matrix<T,3,1> > cd2(_cd2);
-    Map<const Matrix<T,3,1> > Lc2(_Lc2);
-    Map<const Quaternion<T> > q2(_q2);
-    Map<Matrix<T,12,1> > res(_res);
-
-    residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res);
-
-    return true;
-}
-
-template<typename D1, typename D2, typename D3, typename D4>
-inline bool FactorForceTorquePreint::residual(const MatrixBase<D1> &     _c1,
-                                              const MatrixBase<D1> &     _cd1,
-                                              const MatrixBase<D1> &     _Lc1,
-                                              const QuaternionBase<D2> & _q1,
-                                              const MatrixBase<D1> &     _bp1,
-                                              const MatrixBase<D1> &     _bw1,
-                                              const MatrixBase<D1> &     _c2,
-                                              const MatrixBase<D1> &     _cd2,
-                                              const MatrixBase<D1> &     _Lc2,
-                                              const QuaternionBase<D3> & _q2,
-                                              MatrixBase<D4> &           _res) const
-{
-    /*  Help for the Imu residual function
-     *
-     *  Notations:
-     *    D_* : a motion in the Delta manifold                           -- implemented as 10-vectors with [Dp, Dq, Dv]
-     *    T_* : a motion difference in the Tangent space to the manifold -- implemented as  9-vectors with [Dp, Do, Dv]
-     *    b*  : a bias
-     *    J*  : a Jacobian matrix
-     *
-     *  We use the following functions from imu_tools.h:
-     *    D  = betweenStates(x1,x2,dt) : obtain a Delta from two states separated dt=t2-t1
-     *    D2 = plus(D1,T)              : plus operator,  D2 = D1 (+) T
-     *    T  = diff(D1,D2)             : minus operator, T  = D2 (-) D1
-     *
-     *  Two methods are possible (select with #define below this note) :
-     *
-     *  Computations common to methods 1 and 2:
-     *    D_exp  = betweenStates(x1,x2,dt)   // Predict delta from states
-     *    T_step = J_preint * (b - b_preint) // compute the delta correction step due to bias change
-     *
-     *  Method 1:
-     *    D_corr = plus(D_preint, T_step)    // correct the pre-integrated delta with correction step due to bias change
-     *    T_err  = diff(D_exp, D_corr)       // compare against expected delta
-     *    res    = W.sqrt * T_err
-     *
-     *   results in:
-     *    res    = W.sqrt * ( diff ( D_exp, plus(D_preint, J_preint * (b - b_preint) ) ) )
-     *
-     *  Method 2:
-     *    T_diff = diff(D_preint, D_exp)     // compare pre-integrated against expected delta
-     *    T_err  = T_diff - T_step           // the difference should match the correction step due to bias change
-     *    res    = W.sqrt * err
-     *
-     *   results in :
-     *    res    = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b - b_preint) )
-     *
-     * NOTE: See optimization report at the end of this file for comparisons of both methods.
-     */
-
-    //needed typedefs
-    typedef typename D1::Scalar T;
-
-    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D4, 12);
-
-    // 1. Expected delta from states
-    Matrix<T, 3, 1 >   dc_exp;
-    Matrix<T, 3, 1 >   dcd_exp;
-    Matrix<T, 3, 1 >   dLc_exp;
-    Quaternion<T>      dq_exp;
-
-    bodydynamics::betweenStates(_c1, _cd1, _Lc1, _q1, _c2, _cd2, _Lc2, _q2, dt_, dc_exp, dcd_exp, dLc_exp, dq_exp);
-
-    // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
-
-    // 2.a. Compute the delta step in tangent space:   step = J_bias * (bias - bias_preint)
-    Matrix<T, 3, 1> dc_step  =                                          J_dc_wb_ * (_bw1 - gyro_bias_preint_);
-    Matrix<T, 3, 1> dcd_step =                                         J_dcd_wb_ * (_bw1 - gyro_bias_preint_);
-    Matrix<T, 3, 1> dLc_step = J_dLc_pb_ * (_bp1 - pbc_bias_preint_) + J_dLc_wb_ * (_bw1 - gyro_bias_preint_);
-    Matrix<T, 3, 1> do_step  =                                          J_dq_wb_ * (_bw1 - gyro_bias_preint_);
-
-    // 2.b. Add the correction step to the preintegrated delta:    delta_cor = delta_preint (+) step
-    Matrix<T,3,1> dc_correct;
-    Matrix<T,3,1> dcd_correct;
-    Matrix<T,3,1> dLc_correct;
-    Quaternion<T> dq_correct;
-
-    bodydynamics::plus(dc_preint_.cast<T>(), dcd_preint_.cast<T>(), dLc_preint_.cast<T>(), dq_preint_.cast<T>(),
-              dc_step, dcd_step, dLc_step, do_step,
-              dc_correct, dcd_correct, dLc_correct, dq_correct);
-
-    // 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
-    // Note the Dt here is zero because it's the delta-time between the same time stamps!
-    Matrix<T, 12, 1> d_error;
-    Map<Matrix<T, 3, 1> >   dc_error (d_error.data()    );
-    Map<Matrix<T, 3, 1> >   dcd_error(d_error.data() + 3);
-    Map<Matrix<T, 3, 1> >   dLc_error(d_error.data() + 6);
-    Map<Matrix<T, 3, 1> >   do_error (d_error.data() + 9);
-
-
-    bodydynamics::diff(dc_exp, dcd_exp, dLc_exp, dq_exp,
-              dc_correct, dcd_correct, dLc_correct, dq_correct, 
-              dc_error, dcd_error, dLc_error, do_error);
-
-    _res = getMeasurementSquareRootInformationUpper() * d_error;
-
-    return true;
-}
-
-// Matrix<double,12,1> FactorForceTorquePreint::residual() const
-// {
-//     Matrix<double,12,1> res;
-
-
-//     FrameBasePtr frm_prev = getFrameOther();
-//     FrameBasePtr frm_curr = getFeature()->getFrame();
-
-//     CaptureBaseWPtrList cap_lst = getCaptureOtherList();  // !! not retrieving the rigt captures...
-//     auto cap_ikin_other = cap_lst.front().lock(); 
-//     auto cap_gyro_other = cap_lst.back().lock();
-
-//     Map<const Matrix<double,3,1> > c1( frm_prev->getStateBlock('C')->getState().data());
-//     Map<const Matrix<double,3,1> > cd1(frm_prev->getStateBlock('D')->getState().data());
-//     Map<const Matrix<double,3,1> > Lc1(frm_prev->getStateBlock('L')->getState().data());
-//     Map<const Quaternion<double> > q1( frm_prev->getStateBlock('O')->getState().data());
-//     Map<const Matrix<double,3,1> > bp1(cap_ikin_other->getStateBlock('I')->getState().data());
-//     Map<const Matrix<double,3,1> > bw1(cap_gyro_other->getStateBlock('I')->getState().data() + 3);  // bw1 = bimu = [ba, bw]
-//     Map<const Matrix<double,3,1> > c2 (frm_curr->getStateBlock('C')->getState().data());
-//     Map<const Matrix<double,3,1> > cd2(frm_curr->getStateBlock('D')->getState().data());
-//     Map<const Matrix<double,3,1> > Lc2(frm_curr->getStateBlock('L')->getState().data());
-//     Map<const Quaternion<double> > q2 (frm_curr->getStateBlock('O')->getState().data());
-
-//     residual(c1, cd1, Lc1, q1, bp1, bw1, c2, cd2, Lc2, q2, res);
-
-//     return res;
-// }
-
-// double FactorForceTorquePreint::cost() const
-// {
-//     Matrix<double,12,1> toto = residual();
-// }
-
-} // namespace wolf
-
-#endif
diff --git a/include/bodydynamics/feature/feature_force_torque.h b/include/bodydynamics/feature/feature_force_torque.h
index 14594821f3171f7628199e4b3b5f76026f4f7f63..c9fdd98d49b1619db1bf1d424cddfb59ae84a3b9 100644
--- a/include/bodydynamics/feature/feature_force_torque.h
+++ b/include/bodydynamics/feature/feature_force_torque.h
@@ -23,70 +23,73 @@
 #define FEATURE_FORCE_TORQUE_H_
 
 //Wolf includes
-#include "core/feature/feature_base.h"
+#include <core/feature/feature_base.h>
+#include <core/common/wolf.h>
 
 //std includes
+
 namespace wolf {
-    
+
+//WOLF_PTR_TYPEDEFS(CaptureImu);
 WOLF_PTR_TYPEDEFS(FeatureForceTorque);
 
-//class FeatureApriltag
 class FeatureForceTorque : public FeatureBase
 {
     public:
 
-        FeatureForceTorque(
-            const double& _dt,
-            const double& _mass,
-            const Eigen::Vector6d& _forces_meas,
-            const Eigen::Vector6d& _torques_meas,
-            const Eigen::Vector3d& _pbc_meas,
-            const Eigen::Matrix<double,14,1>& _kin_meas,
-            const Eigen::Matrix6d& _cov_forces_meas,
-            const Eigen::Matrix6d& _cov_torques_meas,
-            const Eigen::Matrix3d& _cov_pbc_meas,
-            const Eigen::Matrix<double,14,14>& _cov_kin_meas = Eigen::Matrix<double,14,14>::Zero()
-            );
-
-        ~FeatureForceTorque() override;
-
-        const double & getDt() const {return dt_;}
-        const double & getMass() const {return mass_;}
-        void setDt(const double & _dt){dt_ = _dt;}
-        void setMass(const double & _mass){mass_ = _mass;}
-
-        const Eigen::Vector6d& getForcesMeas() const {return forces_meas_;}
-        const Eigen::Vector6d& getTorquesMeas() const {return torques_meas_;}
-        const Eigen::Vector3d& getPbcMeas() const {return pbc_meas_;}
-        const Eigen::Matrix<double,14,1>& getKinMeas() const {return kin_meas_;}
-        const Eigen::Matrix6d& getCovForcesMeas() const {return cov_forces_meas_;}
-        const Eigen::Matrix6d& getCovTorquesMeas() const {return cov_torques_meas_;}
-        const Eigen::Matrix3d& getCovPbcMeas() const {return cov_pbc_meas_;}
-        const Eigen::Matrix<double,14,14>& getCovKinMeas() const {return cov_kin_meas_;}
-
-        void setForcesMeas(const Eigen::Vector6d& _forces_meas){forces_meas_ = _forces_meas;}
-        void setTorquesMeas(const Eigen::Vector6d& _torques_meas){torques_meas_ = _torques_meas;}
-        void setKinMeas(const Eigen::Matrix<double,14,1>& _kin_meas){kin_meas_ = _kin_meas;}
-        void setPbcMeas(const Eigen::Vector3d& _pbc_meas){pbc_meas_ = _pbc_meas;}
-        void setCovForcesMeas(const Eigen::Matrix6d& _cov_forces_meas){cov_forces_meas_ = _cov_forces_meas;}
-        void setCovTorquesMeas(const Eigen::Matrix6d& _cov_torques_meas){cov_torques_meas_ = _cov_torques_meas;}
-        void setCovPbcMeas(const Eigen::Matrix3d& _cov_pbc_meas){cov_pbc_meas_ = _cov_pbc_meas;}
-        void setCovKinMeas(const Eigen::Matrix<double,14,14>& _cov_kin_meas){cov_kin_meas_ = _cov_kin_meas;}
-    
+        /** \brief Constructor from and measures
+         *
+         * \param _measurement the measurement
+         * \param _meas_covariance the noise of the measurement
+         * \param _J_delta_bias Jacobians of preintegrated delta wrt [pbc,gyro] biases
+         * \param _pbc_bias COM position relative to bias bias of origin frame time
+         * \param _gyro_bias gyroscope bias of origin frame time
+         * \param _cap_ft_ptr pointer to parent CaptureMotion (CaptureForceTorque)
+         */
+        FeatureForceTorque(const Eigen::VectorXd& _delta_preintegrated,
+                                 const Eigen::MatrixXd& _delta_preintegrated_covariance,
+                                 const Eigen::Vector6d& _biases_preint,
+                                 const Eigen::Matrix<double,12,6>& _J_delta_biases);
+
+//        /** \brief Constructor from capture pointer
+//         *
+//         * \param _cap_imu_ptr pointer to parent CaptureMotion
+//         */
+//        FeatureForceTorque(CaptureMotionPtr _cap_imu_ptr);
+
+        ~FeatureForceTorque() override = default;
+
+        const Eigen::Vector3d&               getPbcBiasPreint()  const;
+        const Eigen::Vector3d&               getGyroBiasPreint() const;
+        const Eigen::Matrix<double, 12, 6>&  getJacobianBias()   const;
+
     private:
-        double dt_;
-        double mass_;
-        Eigen::Vector6d forces_meas_;
-        Eigen::Vector6d torques_meas_;
-        Eigen::Vector3d pbc_meas_;
-        Eigen::Matrix<double,14,1> kin_meas_;
-        Eigen::Matrix6d cov_forces_meas_;
-        Eigen::Matrix6d cov_torques_meas_;
-        Eigen::Matrix3d cov_pbc_meas_;
-        Eigen::Matrix<double,14,14> cov_kin_meas_;
 
+        // Used biases
+        Eigen::Vector3d pbc_bias_preint_;       ///< Acceleration bias used for delta preintegration
+        Eigen::Vector3d gyro_bias_preint_;      ///< Gyrometer bias used for delta preintegration
+
+        Eigen::Matrix<double, 12, 6> J_delta_bias_;
+
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 };
 
+inline const Eigen::Vector3d& FeatureForceTorque::getPbcBiasPreint() const
+{
+    return pbc_bias_preint_;
+}
+
+inline const Eigen::Vector3d& FeatureForceTorque::getGyroBiasPreint() const
+{
+    return gyro_bias_preint_;
+}
+
+inline const Eigen::Matrix<double, 12, 6>& FeatureForceTorque::getJacobianBias() const
+{
+    return J_delta_bias_;
+}
+
 } // namespace wolf
 
-#endif
+#endif // FEATURE_FORCE_TORQUE_H_
diff --git a/include/bodydynamics/feature/feature_force_torque_preint.h b/include/bodydynamics/feature/feature_force_torque_preint.h
deleted file mode 100644
index 9a03d2a73e668123b29dd948c7988795ea45f9cf..0000000000000000000000000000000000000000
--- a/include/bodydynamics/feature/feature_force_torque_preint.h
+++ /dev/null
@@ -1,95 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-#ifndef FEATURE_FORCE_TORQUE_PREINT_H_
-#define FEATURE_FORCE_TORQUE_PREINT_H_
-
-//Wolf includes
-#include <core/feature/feature_base.h>
-#include <core/common/wolf.h>
-
-//std includes
-
-namespace wolf {
-
-//WOLF_PTR_TYPEDEFS(CaptureImu);
-WOLF_PTR_TYPEDEFS(FeatureForceTorquePreint);
-
-class FeatureForceTorquePreint : public FeatureBase
-{
-    public:
-
-        /** \brief Constructor from and measures
-         *
-         * \param _measurement the measurement
-         * \param _meas_covariance the noise of the measurement
-         * \param _J_delta_bias Jacobians of preintegrated delta wrt [pbc,gyro] biases
-         * \param _pbc_bias COM position relative to bias bias of origin frame time
-         * \param _gyro_bias gyroscope bias of origin frame time
-         * \param _cap_ftpreint_ptr pointer to parent CaptureMotion (CaptureForceTorquePreint)
-         */
-        FeatureForceTorquePreint(const Eigen::VectorXd& _delta_preintegrated,
-                                 const Eigen::MatrixXd& _delta_preintegrated_covariance,
-                                 const Eigen::Vector6d& _biases_preint,
-                                 const Eigen::Matrix<double,12,6>& _J_delta_biases);
-
-//        /** \brief Constructor from capture pointer
-//         *
-//         * \param _cap_imu_ptr pointer to parent CaptureMotion
-//         */
-//        FeatureForceTorquePreint(CaptureMotionPtr _cap_imu_ptr);
-
-        ~FeatureForceTorquePreint() override = default;
-
-        const Eigen::Vector3d&               getPbcBiasPreint()  const;
-        const Eigen::Vector3d&               getGyroBiasPreint() const;
-        const Eigen::Matrix<double, 12, 6>&  getJacobianBias()   const;
-
-    private:
-
-        // Used biases
-        Eigen::Vector3d pbc_bias_preint_;       ///< Acceleration bias used for delta preintegration
-        Eigen::Vector3d gyro_bias_preint_;      ///< Gyrometer bias used for delta preintegration
-
-        Eigen::Matrix<double, 12, 6> J_delta_bias_;
-
-    public:
-      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-};
-
-inline const Eigen::Vector3d& FeatureForceTorquePreint::getPbcBiasPreint() const
-{
-    return pbc_bias_preint_;
-}
-
-inline const Eigen::Vector3d& FeatureForceTorquePreint::getGyroBiasPreint() const
-{
-    return gyro_bias_preint_;
-}
-
-inline const Eigen::Matrix<double, 12, 6>& FeatureForceTorquePreint::getJacobianBias() const
-{
-    return J_delta_bias_;
-}
-
-} // namespace wolf
-
-#endif // FEATURE_FORCE_TORQUE_PREINT_H_
diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque.h
similarity index 83%
rename from include/bodydynamics/processor/processor_force_torque_preint.h
rename to include/bodydynamics/processor/processor_force_torque.h
index 6cd089107cfebd55fad2a51ff1b6cf5eddb0b036..8c9a8d4ab0a8bf944a86f88c8d7ad44bfb330d33 100644
--- a/include/bodydynamics/processor/processor_force_torque_preint.h
+++ b/include/bodydynamics/processor/processor_force_torque.h
@@ -19,24 +19,24 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-#ifndef PROCESSOR_FORCE_TORQUE_PREINT_H
-#define PROCESSOR_FORCE_TORQUE_PREINT_H
+#ifndef PROCESSOR_FORCE_TORQUE_H
+#define PROCESSOR_FORCE_TORQUE_H
 
 // Wolf core
 #include <core/processor/processor_motion.h>
 
 namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorquePreint);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorque);
 
-struct ParamsProcessorForceTorquePreint : public ParamsProcessorMotion
+struct ParamsProcessorForceTorque : public ParamsProcessorMotion
 {
         std::string sensor_ikin_name;
         std::string sensor_angvel_name;
         int nbc;  // Number of contacts
         int dimc; // Dimension of the contact: 3D == point feet = 3D force, 6D = humanoid = wrench
 
-        ParamsProcessorForceTorquePreint() = default;
-        ParamsProcessorForceTorquePreint(std::string _unique_name, const ParamsServer& _server):
+        ParamsProcessorForceTorque() = default;
+        ParamsProcessorForceTorque(std::string _unique_name, const ParamsServer& _server):
             ParamsProcessorMotion(_unique_name, _server)
         {
             sensor_ikin_name   = _server.getParam<std::string>(_unique_name + "/sensor_ikin_name");
@@ -44,7 +44,7 @@ struct ParamsProcessorForceTorquePreint : public ParamsProcessorMotion
             nbc                = _server.getParam<int>(_unique_name + "/nbc_");
             dimc               = _server.getParam<int>(_unique_name + "/dimc_");
         }
-        ~ParamsProcessorForceTorquePreint() override = default;
+        ~ParamsProcessorForceTorque() override = default;
         std::string print() const override
         {
             return ParamsProcessorMotion::print()                    + "\n"
@@ -56,16 +56,16 @@ struct ParamsProcessorForceTorquePreint : public ParamsProcessorMotion
         }
 };
 
-WOLF_PTR_TYPEDEFS(ProcessorForceTorquePreint);
+WOLF_PTR_TYPEDEFS(ProcessorForceTorque);
     
 //class
-class ProcessorForceTorquePreint : public ProcessorMotion{
+class ProcessorForceTorque : public ProcessorMotion{
     public:
-        ProcessorForceTorquePreint(ParamsProcessorForceTorquePreintPtr _params_motion_force_torque_preint);
-        ~ProcessorForceTorquePreint() override;
+        ProcessorForceTorque(ParamsProcessorForceTorquePtr _params_motion_force_torque);
+        ~ProcessorForceTorque() override;
         void configure(SensorBasePtr _sensor) override;
 
-        WOLF_PROCESSOR_CREATE(ProcessorForceTorquePreint, ParamsProcessorForceTorquePreint);
+        WOLF_PROCESSOR_CREATE(ProcessorForceTorque, ParamsProcessorForceTorque);
 
     protected:
         void computeCurrentDelta(const Eigen::VectorXd& _data,
@@ -109,7 +109,7 @@ class ProcessorForceTorquePreint : public ProcessorMotion{
                                             CaptureBasePtr _capture_origin) override;
 
     private:
-        ParamsProcessorForceTorquePreintPtr params_motion_force_torque_preint_;
+        ParamsProcessorForceTorquePtr params_motion_force_torque_;
         SensorBasePtr sensor_ikin_;
         SensorBasePtr sensor_angvel_;
         int nbc_;
@@ -124,17 +124,17 @@ class ProcessorForceTorquePreint : public ProcessorMotion{
 
 namespace wolf{
 
-inline void ProcessorForceTorquePreint::configure(SensorBasePtr _sensor)
+inline void ProcessorForceTorque::configure(SensorBasePtr _sensor)
 {
-    sensor_ikin_   = _sensor->getProblem()->findSensor(params_motion_force_torque_preint_->sensor_ikin_name);
-    sensor_angvel_ = _sensor->getProblem()->findSensor(params_motion_force_torque_preint_->sensor_angvel_name);
+    sensor_ikin_   = _sensor->getProblem()->findSensor(params_motion_force_torque_->sensor_ikin_name);
+    sensor_angvel_ = _sensor->getProblem()->findSensor(params_motion_force_torque_->sensor_angvel_name);
 };
 
-inline Eigen::VectorXd ProcessorForceTorquePreint::deltaZero() const
+inline Eigen::VectorXd ProcessorForceTorque::deltaZero() const
 {
     return (Eigen::VectorXd(13) << 0,0,0, 0,0,0,  0,0,0,  0,0,0,1 ).finished(); // com, com vel, ang momentum, orientation
 }
 
 } // namespace wolf
 
-#endif // PROCESSOR_FORCE_TORQUE_PREINT_H
+#endif // PROCESSOR_FORCE_TORQUE_H
diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_preint.h b/include/bodydynamics/processor/processor_force_torque_inertial.h
similarity index 89%
rename from include/bodydynamics/processor/processor_force_torque_inertial_preint.h
rename to include/bodydynamics/processor/processor_force_torque_inertial.h
index 503247053b9f77094b4000d5ad5ecbfa54f50baa..616411f510f74ff6fa12d26d5270ce0252bb5dee 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial_preint.h
+++ b/include/bodydynamics/processor/processor_force_torque_inertial.h
@@ -20,14 +20,14 @@
 //
 //--------LICENSE_END--------
 /*
- * processor_preintegrator_force_torque_inertial.h
+ * processor_force_torque_inertial.h
  *
  *  Created on: Aug 19, 2021
  *      Author: jsola
  */
 
-#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_H_
-#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_H_
+#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_H_
+#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_H_
 
 #include "bodydynamics/math/force_torque_inertial_delta_tools.h"
 #include "bodydynamics/sensor/sensor_force_torque_inertial.h"
@@ -37,12 +37,12 @@
 namespace wolf
 {
 
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertialPreint);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertial);
 
-struct ParamsProcessorForceTorqueInertialPreint : public ParamsProcessorMotion
+struct ParamsProcessorForceTorqueInertial : public ParamsProcessorMotion
 {
-    ParamsProcessorForceTorqueInertialPreint() = default;
-    ParamsProcessorForceTorqueInertialPreint(std::string _unique_name, const ParamsServer& _server)
+    ParamsProcessorForceTorqueInertial() = default;
+    ParamsProcessorForceTorqueInertial(std::string _unique_name, const ParamsServer& _server)
         : ParamsProcessorMotion(_unique_name, _server)
     {
         n_propellers = _server.getParam<int>(prefix + _unique_name + "/n_propellers");
@@ -57,19 +57,19 @@ struct ParamsProcessorForceTorqueInertialPreint : public ParamsProcessorMotion
     int n_propellers;
 };
 
-WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertialPreint);
-class ProcessorForceTorqueInertialPreint : public ProcessorMotion
+WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertial);
+class ProcessorForceTorqueInertial : public ProcessorMotion
 {
   public:
-    ProcessorForceTorqueInertialPreint(
-        ParamsProcessorForceTorqueInertialPreintPtr _params_force_torque_inertial_preint);
-    ~ProcessorForceTorqueInertialPreint() override;
+    ProcessorForceTorqueInertial(
+        ParamsProcessorForceTorqueInertialPtr _params_force_torque_inertial);
+    ~ProcessorForceTorqueInertial() override;
     void configure(SensorBasePtr _sensor) override;
-    WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialPreint, ParamsProcessorForceTorqueInertialPreint);
+    WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertial, ParamsProcessorForceTorqueInertial);
 
   protected:
-    ParamsProcessorForceTorqueInertialPreintPtr params_force_torque_inertial_preint_;
-    SensorForceTorqueInertialPtr                sensor_fti_;
+    ParamsProcessorForceTorqueInertialPtr params_force_torque_inertial_;
+    SensorForceTorqueInertialPtr          sensor_fti_;
 
   public:
     /** \brief convert raw CaptureMotion data to the delta-state format.
@@ -242,11 +242,11 @@ class ProcessorForceTorqueInertialPreint : public ProcessorMotion
     virtual VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
 };
 
-inline Eigen::VectorXd ProcessorForceTorqueInertialPreint::deltaZero() const
+inline Eigen::VectorXd ProcessorForceTorqueInertial::deltaZero() const
 {
     return bodydynamics::fti::identity();
 }
 
 } /* namespace wolf */
 
-#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_H_ */
+#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_H_ */
diff --git a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
similarity index 89%
rename from include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
rename to include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
index 8bcc1608659f2a373282c323099bdd38dc8d0666..c05887e9614252f80afd8861e4b7fc16fe45243c 100644
--- a/include/bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h
+++ b/include/bodydynamics/processor/processor_force_torque_inertial_dynamics.h
@@ -20,14 +20,14 @@
 //
 //--------LICENSE_END--------
 /*
- * processor_preintegrator_force_torque_inertial.h
+ * processor_force_torque_inertial_dynamics.h
  *
  *  Created on: Aug 19, 2021
  *      Author: jsola
  */
 
-#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_
-#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_
+#ifndef PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H_
+#define PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H_
 
 #include "bodydynamics/math/force_torque_inertial_delta_tools.h"
 #include "bodydynamics/sensor/sensor_force_torque_inertial.h"
@@ -37,12 +37,12 @@
 namespace wolf
 {
 
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertialPreintDynamics);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorForceTorqueInertialDynamics);
 
-struct ParamsProcessorForceTorqueInertialPreintDynamics : public ParamsProcessorMotion
+struct ParamsProcessorForceTorqueInertialDynamics : public ParamsProcessorMotion
 {
-    ParamsProcessorForceTorqueInertialPreintDynamics() = default;
-    ParamsProcessorForceTorqueInertialPreintDynamics(std::string _unique_name, const ParamsServer& _server)
+    ParamsProcessorForceTorqueInertialDynamics() = default;
+    ParamsProcessorForceTorqueInertialDynamics(std::string _unique_name, const ParamsServer& _server)
         : ParamsProcessorMotion(_unique_name, _server)
     {
         // n_propellers = _server.getParam<int>(prefix + _unique_name + "/n_propellers");
@@ -57,20 +57,20 @@ struct ParamsProcessorForceTorqueInertialPreintDynamics : public ParamsProcessor
     // int n_propellers;
 };
 
-WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertialPreintDynamics);
-class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
+WOLF_PTR_TYPEDEFS(ProcessorForceTorqueInertialDynamics);
+class ProcessorForceTorqueInertialDynamics : public ProcessorMotion
 {
   public:
-    ProcessorForceTorqueInertialPreintDynamics(
-        ParamsProcessorForceTorqueInertialPreintDynamicsPtr _params_force_torque_inertial_preint_dynamics_);
-    ~ProcessorForceTorqueInertialPreintDynamics() override;
+    ProcessorForceTorqueInertialDynamics(
+        ParamsProcessorForceTorqueInertialDynamicsPtr _params_force_torque_inertial_dynamics_);
+    ~ProcessorForceTorqueInertialDynamics() override;
     void configure(SensorBasePtr _sensor) override;
-    WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialPreintDynamics,
-                          ParamsProcessorForceTorqueInertialPreintDynamics);
+    WOLF_PROCESSOR_CREATE(ProcessorForceTorqueInertialDynamics,
+                          ParamsProcessorForceTorqueInertialDynamics);
 
   protected:
-    ParamsProcessorForceTorqueInertialPreintDynamicsPtr params_force_torque_inertial_preint_dynamics_;
-    SensorForceTorqueInertialPtr                        sensor_fti_;
+    ParamsProcessorForceTorqueInertialDynamicsPtr params_force_torque_inertial_dynamics_;
+    SensorForceTorqueInertialPtr                  sensor_fti_;
     Matrix6d imu_drift_cov_;
     Matrix3d gyro_noise_cov_;
 
@@ -268,11 +268,11 @@ class ProcessorForceTorqueInertialPreintDynamics : public ProcessorMotion
                       Eigen::MatrixXd&       _J_tangent_model) const;
 };
 
-inline Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::deltaZero() const
+inline Eigen::VectorXd ProcessorForceTorqueInertialDynamics::deltaZero() const
 {
     return bodydynamics::fti::identity();
 }
 
 } /* namespace wolf */
 
-#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_PREINT_DYNAMICS_H_ */
+#endif /* PROCESSOR_PROCESSOR_FORCE_TORQUE_INERTIAL_DYNAMICS_H_ */
diff --git a/src/capture/capture_force_torque_preint.cpp b/src/capture/capture_force_torque.cpp
similarity index 87%
rename from src/capture/capture_force_torque_preint.cpp
rename to src/capture/capture_force_torque.cpp
index 83e145e4b85a4895a81dce2251fcddd10681460d..3b09bdda9e821497e4e7cc77300a8e06cb656e71 100644
--- a/src/capture/capture_force_torque_preint.cpp
+++ b/src/capture/capture_force_torque.cpp
@@ -20,13 +20,13 @@
 //
 //--------LICENSE_END--------
 #include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_force_torque.h"
 #include "bodydynamics/sensor/sensor_force_torque.h"
 #include "core/state_block/state_quaternion.h"
 
 namespace wolf {
 
-CaptureForceTorquePreint::CaptureForceTorquePreint(
+CaptureForceTorque::CaptureForceTorque(
                     const TimeStamp& _init_ts,
                     SensorBasePtr _sensor_ptr,
                     CaptureInertialKinematicsPtr _capture_IK_ptr,  // to get the pbc bias
@@ -34,14 +34,14 @@ CaptureForceTorquePreint::CaptureForceTorquePreint(
                     const Eigen::VectorXd& _data,
                     const Eigen::MatrixXd& _data_cov,  // TODO: no uncertainty from kinematics
                     CaptureBasePtr _capture_origin_ptr) :
-                CaptureMotion("CaptureForceTorquePreint", _init_ts, _sensor_ptr, _data, _data_cov,
+                CaptureMotion("CaptureForceTorque", _init_ts, _sensor_ptr, _data, _data_cov,
                               _capture_origin_ptr, nullptr, nullptr, nullptr),
                 cap_ikin_other_(_capture_IK_ptr),
                 cap_gyro_other_(_capture_motion_ptr)
 {
 }
 
-CaptureForceTorquePreint::~CaptureForceTorquePreint()
+CaptureForceTorque::~CaptureForceTorque()
 {
 
 }
diff --git a/src/capture/capture_force_torque_inertial.cpp b/src/capture/capture_force_torque_inertial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..85a7f3603e24aaa04de9bfaafa5b33f32aa58da4
--- /dev/null
+++ b/src/capture/capture_force_torque_inertial.cpp
@@ -0,0 +1,50 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "bodydynamics/capture/capture_force_torque_inertial.h"
+
+#include <core/state_block/state_block_derived.h>
+
+namespace wolf
+{
+CaptureForceTorqueInertial::CaptureForceTorqueInertial(const TimeStamp&       _init_ts,
+                                                       SensorBasePtr          _sensor_ptr,
+                                                       const Eigen::VectorXd& _data,
+                                                       const Eigen::MatrixXd& _data_cov,
+                                                       CaptureBasePtr         _capture_origin_ptr)
+    : CaptureMotion("CaptureForceTorqueInertial",
+                    _init_ts,
+                    _sensor_ptr,
+                    _data,
+                    _data_cov,
+                    _capture_origin_ptr,
+                    nullptr,                                // position is static
+                    nullptr,                                // orientation is static
+                    (_sensor_ptr->isStateBlockDynamic('I')  // dynamic intrinsics are IMU bias
+                         ? std::make_shared<StateParams6>(_sensor_ptr->getStateBlockDynamic('I')->getState(), false)
+                         : nullptr))
+{
+    //
+}
+
+CaptureForceTorqueInertial::~CaptureForceTorqueInertial() {}
+
+}  // namespace wolf
\ No newline at end of file
diff --git a/src/feature/feature_force_torque.cpp b/src/feature/feature_force_torque.cpp
index 83c12548fd923e7621b56465ecedbcc12b673500..04eacfa7e8c4c02944bb2fd6a151f05551030c47 100644
--- a/src/feature/feature_force_torque.cpp
+++ b/src/feature/feature_force_torque.cpp
@@ -20,35 +20,18 @@
 //
 //--------LICENSE_END--------
 #include "bodydynamics/feature/feature_force_torque.h"
+namespace wolf
+{
 
-namespace wolf {
-    
-WOLF_PTR_TYPEDEFS(FeatureForceTorque);
+FeatureForceTorque::FeatureForceTorque(const Eigen::VectorXd&              _delta_preintegrated,
+                                       const Eigen::MatrixXd&              _delta_preintegrated_covariance,
+                                       const Eigen::Vector6d&              _biases_preint,
+                                       const Eigen::Matrix<double, 12, 6>& _J_delta_biases)
+    : FeatureBase("FeatureForceTorque", _delta_preintegrated, _delta_preintegrated_covariance),
+      pbc_bias_preint_(_biases_preint.head<3>()),
+      gyro_bias_preint_(_biases_preint.tail<3>()),
+      J_delta_bias_(_J_delta_biases)
+{
+}
 
-FeatureForceTorque::FeatureForceTorque(
-            const double& _dt,
-            const double& _mass,
-            const Eigen::Vector6d& _forces_meas,
-            const Eigen::Vector6d& _torques_meas,
-            const Eigen::Vector3d& _pbc_meas,
-            const Eigen::Matrix<double,14,1>& _kin_meas,
-            const Eigen::Matrix6d& _cov_forces_meas,
-            const Eigen::Matrix6d& _cov_torques_meas,
-            const Eigen::Matrix3d& _cov_pbc_meas,
-            const Eigen::Matrix<double,14,14>& _cov_kin_meas) :
-        FeatureBase("FORCETORQUE", 42*Eigen::Matrix1d::Identity(), 42*Eigen::Matrix1d::Identity(), UNCERTAINTY_IS_COVARIANCE),  // Pass dummmy values -> we are bypassing the way meas and cov is usually stored because too many of them == vector is too big -> error prone
-        dt_(_dt),
-        mass_(_mass),
-        forces_meas_(_forces_meas),
-        torques_meas_(_torques_meas),
-        pbc_meas_(_pbc_meas),
-        kin_meas_(_kin_meas),
-        cov_forces_meas_(_cov_forces_meas),
-        cov_torques_meas_(_cov_torques_meas),
-        cov_pbc_meas_(_cov_pbc_meas),
-        cov_kin_meas_(_cov_kin_meas)
-{}
-
-FeatureForceTorque::~FeatureForceTorque(){}
-
-} // namespace wolf
+}  // namespace wolf
diff --git a/src/processor/processor_force_torque.cpp b/src/processor/processor_force_torque.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d9bd77e5474df95514c60c72c5f3f138e92415b4
--- /dev/null
+++ b/src/processor/processor_force_torque.cpp
@@ -0,0 +1,294 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+// wolf
+#include "bodydynamics/math/force_torque_delta_tools.h"
+#include "bodydynamics/capture/capture_force_torque.h"
+#include "bodydynamics/feature/feature_force_torque.h"
+#include "bodydynamics/processor/processor_force_torque.h"
+#include "bodydynamics/factor/factor_force_torque.h"
+
+namespace wolf
+{
+
+int compute_data_size(int nbc, int dimc)
+{
+    // compute the size of the data/body vectors used by processorMotion API depending
+    // on the number of contacts (nbc) and contact dimension (dimc)
+    // result: forces (+torques for 6D contacts) + pbc + wb + contact/base positions + contact/base orientations
+    return nbc * dimc + 3 + 3 + nbc * 3 + nbc * 4;
+}
+
+using namespace Eigen;
+
+ProcessorForceTorque::ProcessorForceTorque(ParamsProcessorForceTorquePtr _params_motion_force_torque)
+    : ProcessorMotion("ProcessorForceTorque",
+                      "CDLO",
+                      3,
+                      13,
+                      13,
+                      12,
+                      compute_data_size(_params_motion_force_torque->nbc, _params_motion_force_torque->dimc),
+                      6,
+                      _params_motion_force_torque),
+      params_motion_force_torque_(std::make_shared<ParamsProcessorForceTorque>(*_params_motion_force_torque)),
+      nbc_(_params_motion_force_torque->nbc),
+      dimc_(_params_motion_force_torque->dimc)
+
+{
+    //
+}
+
+ProcessorForceTorque::~ProcessorForceTorque()
+{
+    //
+}
+
+bool ProcessorForceTorque::voteForKeyFrame() const
+{
+    // time span
+    if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_force_torque_->max_time_span)
+    {
+        WOLF_DEBUG("PM: vote: time span");
+        return true;
+    }
+    // buffer length
+    if (getBuffer().size() > params_motion_force_torque_->max_buff_length)
+    {
+        WOLF_DEBUG("PM: vote: buffer length");
+        return true;
+    }
+
+    // Some other measure of movement?
+
+    // WOLF_DEBUG( "PM: do not vote" );
+    return false;
+}
+
+CaptureMotionPtr ProcessorForceTorque::emplaceCapture(const FrameBasePtr&   _frame_own,
+                                                      const SensorBasePtr&  _sensor,
+                                                      const TimeStamp&      _ts,
+                                                      const VectorXd&       _data,
+                                                      const MatrixXd&       _data_cov,
+                                                      const VectorXd&       _calib,
+                                                      const VectorXd&       _calib_preint,
+                                                      const CaptureBasePtr& _capture_origin)
+{
+    // Here we have to retrieve the origin capture no
+    // !! PROBLEM:
+    // when doing setOrigin, emplaceCapture is called 2 times
+    // - first on the first KF (usually created by setPrior), this one contains the biases
+    // - secondly on the inner frame (last) which does not contains other captures
+    auto capture_ikin   = _frame_own->getCaptureOf(sensor_ikin_);
+    auto capture_angvel = _frame_own->getCaptureOf(sensor_angvel_);
+    if ((capture_ikin == nullptr) || (capture_angvel == nullptr))
+    {
+        // We have to retrieve the bias from a former Keyframe: origin
+        capture_ikin   = origin_ptr_->getFrame()->getCaptureOf(sensor_ikin_);
+        capture_angvel = origin_ptr_->getFrame()->getCaptureOf(sensor_angvel_);
+    }
+    auto cap =
+        CaptureBase::emplace<CaptureForceTorque>(_frame_own,
+                                                 _ts,
+                                                 _sensor,
+                                                 std::static_pointer_cast<CaptureInertialKinematics>(capture_ikin),
+                                                 std::static_pointer_cast<CaptureMotion>(capture_angvel),
+                                                 _data,
+                                                 _data_cov,
+                                                 _capture_origin);
+
+    // Cannot be recovered from the _calib and _calib_preint which are initialized using calib_size_
+    // which is zero in our case since the calibration stateblocks are not actually in the FTPreint sensor/captures
+
+    auto     cap_ft = std::static_pointer_cast<CaptureForceTorque>(cap);
+    Vector6d calib  = getCalibration(cap_ft);
+    setCalibration(cap_ft, calib);
+    cap_ft->setCalibrationPreint(calib);
+
+    return cap;
+}
+
+FeatureBasePtr ProcessorForceTorque::emplaceFeature(CaptureMotionPtr _capture_motion)
+{
+    // Retrieving the origin capture and getting the bias from here should be done here?
+    auto feature = FeatureBase::emplace<FeatureForceTorque>(
+        _capture_motion,
+        _capture_motion->getBuffer().back().delta_integr_,
+        _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+        _capture_motion->getCalibrationPreint(),
+        _capture_motion->getBuffer().back().jacobian_calib_);
+    return feature;
+}
+
+Eigen::VectorXd ProcessorForceTorque::correctDelta(const Eigen::VectorXd& delta_preint,
+                                                   const Eigen::VectorXd& delta_step) const
+{
+    return bodydynamics::plus(delta_preint, delta_step);
+}
+
+VectorXd ProcessorForceTorque::getCalibration(const CaptureBaseConstPtr _capture) const
+{
+    VectorXd bias_vec(6);
+
+    if (_capture)  // access from capture is quicker
+    {
+        auto cap_ft(std::static_pointer_cast<const CaptureForceTorque>(_capture));
+
+        // get calib part from Ikin capture
+        bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState();
+
+        // get calib part from IMU capture
+        bias_vec.segment<3>(3) =
+            cap_ft->getGyroCaptureOther()
+                ->getSensorIntrinsic()
+                ->getState()
+                .tail<3>();  // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+    }
+    else  // access from sensor is slower
+    {
+        // get calib part from Ikin capture
+        bias_vec.segment<3>(0) = sensor_ikin_->getIntrinsic()->getState();
+
+        // get calib part from IMU capture
+        bias_vec.segment<3>(3) =
+            sensor_angvel_->getIntrinsic()
+                ->getState()
+                .tail<3>();  // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+    }
+    return bias_vec;
+}
+
+void ProcessorForceTorque::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+    CaptureForceTorquePtr cap_ft(std::static_pointer_cast<CaptureForceTorque>(_capture));
+
+    // set calib part in Ikin capture
+    cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->setState(_calibration.head<3>());
+
+    // set calib part in IMU capture
+    Vector6d calib_imu  = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState();
+    calib_imu.tail<3>() = _calibration.tail<3>();
+
+    cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->setState(calib_imu);
+}
+
+FactorBasePtr ProcessorForceTorque::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+{
+    CaptureForceTorquePtr cap_ft_origin = std::static_pointer_cast<CaptureForceTorque>(_capture_origin);
+    FeatureForceTorquePtr ftr_ft        = std::static_pointer_cast<FeatureForceTorque>(_feature_motion);
+    CaptureForceTorquePtr cap_ft_new    = std::static_pointer_cast<CaptureForceTorque>(ftr_ft->getCapture());
+
+    auto fac_ft = FactorBase::emplace<FactorForceTorque>(ftr_ft,
+                                                         ftr_ft,
+                                                         cap_ft_origin,
+                                                         shared_from_this(),
+                                                         cap_ft_origin->getIkinCaptureOther(),  // to retrieve sb_bp1
+                                                         cap_ft_origin->getGyroCaptureOther(),  // to retrieve sb_w1
+                                                         false);
+
+    return fac_ft;
+}
+
+void ProcessorForceTorque::computeCurrentDelta(const Eigen::VectorXd& _data,
+                                               const Eigen::MatrixXd& _data_cov,
+                                               const Eigen::VectorXd& _calib,
+                                               const double           _dt,
+                                               Eigen::VectorXd&       _delta,
+                                               Eigen::MatrixXd&       _delta_cov,
+                                               Eigen::MatrixXd&       _jac_delta_calib) const
+{
+    assert(_data.size() == data_size_ && "Wrong data size!");
+
+    // create delta
+    MatrixXd jac_body_bias(data_size_ - nbc_, 6);
+    VectorXd body(data_size_);
+    bodydynamics::debiasData(_data, _calib, nbc_, dimc_, body, jac_body_bias);
+
+    MatrixXd jac_delta_body(12, data_size_ - nbc_);
+    bodydynamics::body2delta(body,
+                             _dt,
+                             std::static_pointer_cast<const SensorForceTorque>(getSensor())->getMass(),
+                             nbc_,
+                             dimc_,
+                             _delta,
+                             jac_delta_body);  // Jacobians tested in bodydynamics_tools
+
+    // [f], [tau], pbc, wb
+    MatrixXd jac_delta_body_reduced = jac_delta_body.leftCols(nbc_ * dimc_ + 6);
+
+    // compute delta_cov (using uncertainty on f,tau,pbc)
+    _delta_cov = jac_delta_body_reduced * _data_cov *
+                 jac_delta_body_reduced.transpose();  // trivially:  jac_body_delta = Identity
+    // _delta_cov = jac_delta_body * _data_cov * jac_delta_body.transpose();  // trivially:  jac_body_delta = Identity
+
+    // compute jacobian_calib
+    _jac_delta_calib = jac_delta_body * jac_body_bias;
+}
+
+void ProcessorForceTorque::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+                                          const Eigen::VectorXd& _delta,
+                                          const double           _dt,
+                                          Eigen::VectorXd&       _delta_preint_plus_delta) const
+{
+    _delta_preint_plus_delta = bodydynamics::compose(_delta_preint, _delta, _dt);
+}
+
+void ProcessorForceTorque::statePlusDelta(const VectorComposite& _x,
+                                          const Eigen::VectorXd& _delta,
+                                          const double           _dt,
+                                          VectorComposite&       _x_plus_delta) const
+{
+    assert(_delta.size() == 13 && "Wrong _delta vector size");
+    assert(_dt >= 0 && "Time interval _dt is negative!");
+
+    // verbose way : create Eigen vectors, then compute, then convert back to Composite
+
+    auto x = _x.vector("CDLO");
+
+    VectorXd x_plus_delta = bodydynamics::composeOverState(x, _delta, _dt);
+
+    _x_plus_delta = VectorComposite(x_plus_delta, "CDLO", {3, 3, 3, 4});
+}
+
+void ProcessorForceTorque::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+                                          const Eigen::VectorXd& _delta,
+                                          const double           _dt,
+                                          Eigen::VectorXd&       _delta_preint_plus_delta,
+                                          Eigen::MatrixXd&       _jacobian_delta_preint,
+                                          Eigen::MatrixXd&       _jacobian_delta) const
+{
+    bodydynamics::compose(_delta_preint,
+                          _delta,
+                          _dt,
+                          _delta_preint_plus_delta,
+                          _jacobian_delta_preint,
+                          _jacobian_delta);  // jacobians tested in bodydynamics_tools
+}
+
+}  // namespace wolf
+
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
+
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorForceTorque)
+}
diff --git a/src/processor/processor_force_torque_inertial.cpp b/src/processor/processor_force_torque_inertial.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8b54a8cb543393a0bd4198d95e214fc9c65f0d0a
--- /dev/null
+++ b/src/processor/processor_force_torque_inertial.cpp
@@ -0,0 +1,176 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * processor_force_torque_inertial.cpp
+ *
+ *  Created on: Aug 19, 2021
+ *      Author: jsola
+ */
+
+// bodydynamics
+#include "bodydynamics/processor/processor_force_torque_inertial.h"
+#include "bodydynamics/factor/factor_force_torque_inertial.h"
+#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+
+// wolf
+#include <core/state_block/state_composite.h>
+#include <core/capture/capture_motion.h>
+
+namespace wolf
+{
+
+ProcessorForceTorqueInertial::ProcessorForceTorqueInertial(
+    ParamsProcessorForceTorqueInertialPtr _params_force_torque_inertial)
+    : ProcessorMotion("ProcessorForceTorqueInertial",
+                      "POVL",
+                      3,   // dim
+                      13,  // state size [p, q, v, L]
+                      19,  // delta size [pi, vi, pd, vd, L, q]
+                      18,  // delta cov size
+                      12,  // data size [a, w, f, t]
+                      6,   // calib size [ab, wb]
+                      _params_force_torque_inertial),
+      params_force_torque_inertial_(_params_force_torque_inertial)
+{
+    // TODO Auto-generated constructor stub
+}
+
+ProcessorForceTorqueInertial::~ProcessorForceTorqueInertial()
+{
+    // TODO Auto-generated destructor stub
+}
+
+CaptureMotionPtr ProcessorForceTorqueInertial::emplaceCapture(const FrameBasePtr&   _frame_own,
+                                                              const SensorBasePtr&  _sensor,
+                                                              const TimeStamp&      _ts,
+                                                              const VectorXd&       _data,
+                                                              const MatrixXd&       _data_cov,
+                                                              const VectorXd&       _calib,
+                                                              const VectorXd&       _calib_preint,
+                                                              const CaptureBasePtr& _capture_origin_ptr)
+{
+    CaptureMotionPtr capture = CaptureBase::emplace<CaptureMotion>(
+        _frame_own, "CaptureForceTorqueInertial", _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
+    setCalibration(capture, _calib);
+    capture->setCalibrationPreint(_calib_preint);
+    return capture;
+}
+
+FeatureBasePtr ProcessorForceTorqueInertial::emplaceFeature(CaptureMotionPtr _capture_own)
+{
+    FeatureBasePtr returnValue;
+    return returnValue;
+}
+
+FactorBasePtr ProcessorForceTorqueInertial::emplaceFactor(FeatureBasePtr _feature_motion,
+                                                          CaptureBasePtr _capture_origin)
+{
+    FactorBasePtr returnValue;
+    return returnValue;
+}
+
+void ProcessorForceTorqueInertial::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
+    _capture->getStateBlock('I')->setState(_calibration);  // Set IMU bias
+}
+
+void ProcessorForceTorqueInertial::configure(SensorBasePtr _sensor)
+{
+    sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
+}
+
+void ProcessorForceTorqueInertial::computeCurrentDelta(const Eigen::VectorXd& _data,
+                                                       const Eigen::MatrixXd& _data_cov,
+                                                       const Eigen::VectorXd& _calib,
+                                                       const double           _dt,
+                                                       Eigen::VectorXd&       _delta,
+                                                       Eigen::MatrixXd&       _delta_cov,
+                                                       Eigen::MatrixXd&       _jacobian_calib) const
+{
+    // compute tangent by removing IMU bias
+    Matrix<double, 12, 1> tangent = _data;
+    tangent.head<6>() -= _calib;            // J_tangent_data  = Identity(12,12)
+    Matrix<double, 12, 6> J_tangent_calib;  // J_tangent_calib = [-Identity(6,6) ; Zero(6,6)]
+    J_tangent_calib.topRows<6>()    = -Matrix6d::Identity();
+    J_tangent_calib.bottomRows<6>() = Matrix6d::Zero();
+
+    // go from tangent to delta. We need to dynamic model for this
+    const Matrix<double, 7, 1>& model = sensor_fti_->getModel();
+    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);
+
+    // Compute cov and compose jacobians
+    const auto& J_delta_data = J_delta_tangent;  //  * J_tangent_data, which is the Identity;
+    _delta_cov               = J_delta_data * _data_cov * J_delta_data.transpose();
+    _jacobian_calib          = J_delta_tangent * J_tangent_calib;
+}
+
+void ProcessorForceTorqueInertial::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+                                                  const Eigen::VectorXd& _delta2,
+                                                  const double           _dt2,
+                                                  Eigen::VectorXd&       _delta1_plus_delta2) const
+{
+    bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2);
+}
+
+void ProcessorForceTorqueInertial::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+                                                  const Eigen::VectorXd& _delta2,
+                                                  const double           _dt2,
+                                                  Eigen::VectorXd&       _delta1_plus_delta2,
+                                                  Eigen::MatrixXd&       _jacobian1,
+                                                  Eigen::MatrixXd&       _jacobian2) const
+{
+    bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
+}
+
+void ProcessorForceTorqueInertial::statePlusDelta(const VectorComposite& _x,
+                                                  const Eigen::VectorXd& _delta,
+                                                  const double           _dt,
+                                                  VectorComposite&       _x_plus_delta) const
+{
+    auto     x            = _x.vector("PVLO");
+    VectorXd x_plus_delta = bodydynamics::fti::composeOverState(x, _delta, _dt);
+    _x_plus_delta         = VectorComposite(x_plus_delta, "PVLO", {3, 3, 3, 4});
+}
+
+Eigen::VectorXd ProcessorForceTorqueInertial::correctDelta(const Eigen::VectorXd& _delta_preint,
+                                                           const Eigen::VectorXd& _delta_step) const
+{
+    return fti::plus(_delta_preint, _delta_step);
+}
+
+VectorXd ProcessorForceTorqueInertial::getCalibration(const CaptureBaseConstPtr _capture) const
+{
+    if (_capture)
+        return _capture->getStateBlock('I')->getState();  // IMU bias
+    else
+        return getSensor()->getStateBlock('I')->getState();  // IMU bias
+}
+} /* namespace wolf */
+
+#include <core/processor/factory_processor.h>
+namespace wolf
+{
+WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertial);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertial);
+}  // namespace wolf
\ No newline at end of file
diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_dynamics.cpp
similarity index 74%
rename from src/processor/processor_force_torque_inertial_preint_dynamics.cpp
rename to src/processor/processor_force_torque_inertial_dynamics.cpp
index 4e8d1b278e302ebe7db8867296432754e893ece5..8becfc0a198f2c4169067d9ceae7b81ecc0ed4bd 100644
--- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp
+++ b/src/processor/processor_force_torque_inertial_dynamics.cpp
@@ -20,16 +20,17 @@
 //
 //--------LICENSE_END--------
 /*
- * processor_preintegrator_force_torque_inertial_dynamics.cpp
+ * processor_force_torque_inertial_dynamics.cpp
  *
  *  Created on: Aug 1, 2022
  *      Author: asanjuan
  */
 
 // bodydynamics
-#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
 #include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
 #include "bodydynamics/math/force_torque_inertial_delta_tools.h"
+#include "bodydynamics/capture/capture_force_torque_inertial.h"
 
 // wolf
 #include <core/state_block/state_composite.h>
@@ -39,9 +40,9 @@
 namespace wolf
 {
 
-ProcessorForceTorqueInertialPreintDynamics::ProcessorForceTorqueInertialPreintDynamics(
-    ParamsProcessorForceTorqueInertialPreintDynamicsPtr _params_force_torque_inertial_preint_dynamics)
-    : ProcessorMotion("ProcessorForceTorqueInertialPreintDynamics",
+ProcessorForceTorqueInertialDynamics::ProcessorForceTorqueInertialDynamics(
+    ParamsProcessorForceTorqueInertialDynamicsPtr _params_force_torque_inertial_dynamics)
+    : ProcessorMotion("ProcessorForceTorqueInertialDynamics",
                       "POVL",
                       3,   // dim
                       13,  // state size [p, q, v, L]
@@ -49,37 +50,35 @@ ProcessorForceTorqueInertialPreintDynamics::ProcessorForceTorqueInertialPreintDy
                       18,  // delta cov size
                       12,  // data size [a, w, f, t]
                       13,  // calib size [ab, wb, c, i, m]
-                      _params_force_torque_inertial_preint_dynamics),
-      params_force_torque_inertial_preint_dynamics_(_params_force_torque_inertial_preint_dynamics)
+                      _params_force_torque_inertial_dynamics),
+      params_force_torque_inertial_dynamics_(_params_force_torque_inertial_dynamics)
 {
     //
 }
 
-ProcessorForceTorqueInertialPreintDynamics::~ProcessorForceTorqueInertialPreintDynamics()
+ProcessorForceTorqueInertialDynamics::~ProcessorForceTorqueInertialDynamics()
 {
     //
 }
 
-CaptureMotionPtr ProcessorForceTorqueInertialPreintDynamics::emplaceCapture(const FrameBasePtr&   _frame_own,
-                                                                            const SensorBasePtr&  _sensor,
-                                                                            const TimeStamp&      _ts,
-                                                                            const VectorXd&       _data,
-                                                                            const MatrixXd&       _data_cov,
-                                                                            const VectorXd&       _calib,
-                                                                            const VectorXd&       _calib_preint,
-                                                                            const CaptureBasePtr& _capture_origin_ptr)
+CaptureMotionPtr ProcessorForceTorqueInertialDynamics::emplaceCapture(const FrameBasePtr&   _frame_own,
+                                                                      const SensorBasePtr&  _sensor,
+                                                                      const TimeStamp&      _ts,
+                                                                      const VectorXd&       _data,
+                                                                      const MatrixXd&       _data_cov,
+                                                                      const VectorXd&       _calib,
+                                                                      const VectorXd&       _calib_preint,
+                                                                      const CaptureBasePtr& _capture_origin_ptr)
 {
-    CaptureMotionPtr capture = CaptureBase::emplace<CaptureMotion>(
-        _frame_own, "CaptureMotion", _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
+    CaptureMotionPtr capture = CaptureBase::emplace<CaptureForceTorqueInertial>(
+        _frame_own, _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
     setCalibration(capture, _calib);
     capture->setCalibrationPreint(_calib_preint);
     return capture;
 }
 
-
-FeatureBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFeature(CaptureMotionPtr _capture_own)
+FeatureBasePtr ProcessorForceTorqueInertialDynamics::emplaceFeature(CaptureMotionPtr _capture_own)
 {
-
     auto           motion = _capture_own->getBuffer().back();
     FeatureBasePtr ftr    = FeatureBase::emplace<FeatureMotion>(_capture_own,
                                                              "FeatureMotion",
@@ -90,8 +89,8 @@ FeatureBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFeature(Captur
     return ftr;
 }
 
-FactorBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFactor(FeatureBasePtr _feature_motion,
-                                                                        CaptureBasePtr _capture_origin)
+FactorBasePtr ProcessorForceTorqueInertialDynamics::emplaceFactor(FeatureBasePtr _feature_motion,
+                                                                  CaptureBasePtr _capture_origin)
 {
     FeatureMotionPtr ftr_ftipd = std::static_pointer_cast<FeatureMotion>(_feature_motion);
 
@@ -100,18 +99,18 @@ FactorBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFactor(FeatureB
     return fac_ftipd;
 }
 
-void ProcessorForceTorqueInertialPreintDynamics::emplaceFeaturesAndFactors(CaptureBasePtr   _capture_origin,
-                                                                           CaptureMotionPtr _capture_own)
+void ProcessorForceTorqueInertialDynamics::emplaceFeaturesAndFactors(CaptureBasePtr   _capture_origin,
+                                                                     CaptureMotionPtr _capture_own)
 {
     // 1. Feature and factor FTI -- force-torque-inertial
     //----------------------------------------------------
     auto motion  = _capture_own->getBuffer().back();
     auto ftr_fti = FeatureBase::emplace<FeatureMotion>(_capture_own,
-                                                   "FeatureMotion",
-                                                   motion.delta_integr_,
-                                                   motion.delta_integr_cov_ + unmeasured_perturbation_cov_,
-                                                   _capture_own->getCalibrationPreint(),
-                                                   motion.jacobian_calib_);
+                                                       "FeatureMotion",
+                                                       motion.delta_integr_,
+                                                       motion.delta_integr_cov_ + unmeasured_perturbation_cov_,
+                                                       _capture_own->getCalibrationPreint(),
+                                                       motion.jacobian_calib_);
 
     FactorBase::emplace<FactorForceTorqueInertialDynamics>(
         ftr_fti, ftr_fti, _capture_origin, shared_from_this(), params_->apply_loss_function);
@@ -120,11 +119,10 @@ void ProcessorForceTorqueInertialPreintDynamics::emplaceFeaturesAndFactors(Captu
     //---------------------------------------------
     // - feature has the current gyro measurement
     // - factor relates w_measured, IMU_bias(last capture), L(last capture) and i(sensor)
-    
+
     // auto measurement_gyro = motion.data_.segment<3>(3);
     // auto meas_cov = sensor_fti_->getNoiseCov().block<3,3>(3,3); // ??
 
-
     // 3. Feature and factor bias -- IMU bias drift for acc and gyro
     //---------------------------------------------------------------
     // - This factor only if IMU biases are Dynamic (check sensor->isStateBlockDynamic('I'))
@@ -160,8 +158,7 @@ void ProcessorForceTorqueInertialPreintDynamics::emplaceFeaturesAndFactors(Captu
     }
 }
 
-void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBasePtr _capture,
-                                                                const VectorXd&      _calibration)
+void ProcessorForceTorqueInertialDynamics::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
 {
     _capture->getStateBlock('I')->setState(_calibration.segment<6>(0));   // Set IMU bias
     _capture->getStateBlock('C')->setState(_calibration.segment<3>(6));   // Set C
@@ -169,7 +166,7 @@ void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBas
     _capture->getStateBlock('m')->setState(_calibration.segment<1>(12));  // Set m
 }
 
-void ProcessorForceTorqueInertialPreintDynamics::configure(SensorBasePtr _sensor)
+void ProcessorForceTorqueInertialDynamics::configure(SensorBasePtr _sensor)
 {
     sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
 
@@ -183,21 +180,21 @@ void ProcessorForceTorqueInertialPreintDynamics::configure(SensorBasePtr _sensor
     gyro_noise_cov_ = Array3d(gyro_noise_std, gyro_noise_std, gyro_noise_std).square().matrix().asDiagonal();
 }
 
-void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::VectorXd& _data,
-                                                              const Eigen::VectorXd& _bias,
-                                                              const Eigen::VectorXd& _model,
-                                                              Eigen::VectorXd&       _tangent,
-                                                              Eigen::MatrixXd&       _J_tangent_data,
-                                                              Eigen::MatrixXd&       _J_tangent_bias,
-                                                              Eigen::MatrixXd&       _J_tangent_model) const
+void ProcessorForceTorqueInertialDynamics::data2tangent(const Eigen::VectorXd& _data,
+                                                        const Eigen::VectorXd& _bias,
+                                                        const Eigen::VectorXd& _model,
+                                                        Eigen::VectorXd&       _tangent,
+                                                        Eigen::MatrixXd&       _J_tangent_data,
+                                                        Eigen::MatrixXd&       _J_tangent_bias,
+                                                        Eigen::MatrixXd&       _J_tangent_model) const
 
 {
-    const Vector6d& awd = _data.segment<6>(0);  // this is (a,w) of data in a 6-vector
-    const Vector3d& fd  = _data.segment<3>(6);
-    const Vector3d& td  = _data.segment<3>(9);
-    const Vector3d& c   = _model.segment<3>(0);
+    const Vector6d& awd      = _data.segment<6>(0);  // this is (a,w) of data in a 6-vector
+    const Vector3d& fd       = _data.segment<3>(6);
+    const Vector3d& td       = _data.segment<3>(9);
+    const Vector3d& c        = _model.segment<3>(0);
     const Matrix3d& fd_cross = skew(fd);
-    const Matrix3d& c_cross = skew(c);
+    const Matrix3d& c_cross  = skew(c);
 
     // tangent(a,w) = data(a,w) - bias(a,w)
     // tangent(f)   = data(f)
@@ -215,17 +212,17 @@ void ProcessorForceTorqueInertialPreintDynamics::data2tangent(const Eigen::Vecto
     _J_tangent_bias.bottomRows<6>() = Matrix6d::Zero();
 
     // J_tangent_model
-    _J_tangent_model.setZero(12,7);
+    _J_tangent_model.setZero(12, 7);
     _J_tangent_model.block<3, 3>(9, 0) = fd_cross;  // J_tt_c = fd_cross
 }
 
-void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen::VectorXd& _data,
-                                                                     const Eigen::MatrixXd& _data_cov,
-                                                                     const Eigen::VectorXd& _calib,
-                                                                     const double           _dt,
-                                                                     Eigen::VectorXd&       _delta,
-                                                                     Eigen::MatrixXd&       _delta_cov,
-                                                                     Eigen::MatrixXd&       _jacobian_calib) const
+void ProcessorForceTorqueInertialDynamics::computeCurrentDelta(const Eigen::VectorXd& _data,
+                                                               const Eigen::MatrixXd& _data_cov,
+                                                               const Eigen::VectorXd& _calib,
+                                                               const double           _dt,
+                                                               Eigen::VectorXd&       _delta,
+                                                               Eigen::MatrixXd&       _delta_cov,
+                                                               Eigen::MatrixXd&       _jacobian_calib) const
 {
     // compute tangent by removing IMU bias
 
@@ -424,41 +421,41 @@ void ProcessorForceTorqueInertialPreintDynamics::computeCurrentDelta(const Eigen
     //////////////////////////////////////////////////////////////////////////////////////////////////////////////
 }
 
-void ProcessorForceTorqueInertialPreintDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
-                                                                const Eigen::VectorXd& _delta2,
-                                                                const double           _dt2,
-                                                                Eigen::VectorXd&       _delta1_plus_delta2) const
+void ProcessorForceTorqueInertialDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+                                                          const Eigen::VectorXd& _delta2,
+                                                          const double           _dt2,
+                                                          Eigen::VectorXd&       _delta1_plus_delta2) const
 {
     bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2);
 }
 
-void ProcessorForceTorqueInertialPreintDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
-                                                                const Eigen::VectorXd& _delta2,
-                                                                const double           _dt2,
-                                                                Eigen::VectorXd&       _delta1_plus_delta2,
-                                                                Eigen::MatrixXd&       _jacobian1,
-                                                                Eigen::MatrixXd&       _jacobian2) const
+void ProcessorForceTorqueInertialDynamics::deltaPlusDelta(const Eigen::VectorXd& _delta1,
+                                                          const Eigen::VectorXd& _delta2,
+                                                          const double           _dt2,
+                                                          Eigen::VectorXd&       _delta1_plus_delta2,
+                                                          Eigen::MatrixXd&       _jacobian1,
+                                                          Eigen::MatrixXd&       _jacobian2) const
 {
     bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
 }
 
-void ProcessorForceTorqueInertialPreintDynamics::statePlusDelta(const VectorComposite& _x,
-                                                                const Eigen::VectorXd& _delta,
-                                                                const double           _dt,
-                                                                VectorComposite&       _x_plus_delta) const
+void ProcessorForceTorqueInertialDynamics::statePlusDelta(const VectorComposite& _x,
+                                                          const Eigen::VectorXd& _delta,
+                                                          const double           _dt,
+                                                          VectorComposite&       _x_plus_delta) const
 {
     auto     x            = _x.vector("PVLO");
     VectorXd x_plus_delta = bodydynamics::fti::composeOverState(x, _delta, _dt);
     _x_plus_delta         = VectorComposite(x_plus_delta, "PVLO", {3, 3, 3, 4});
 }
 
-Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::correctDelta(const Eigen::VectorXd& _delta_preint,
-                                                                         const Eigen::VectorXd& _delta_step) const
+Eigen::VectorXd ProcessorForceTorqueInertialDynamics::correctDelta(const Eigen::VectorXd& _delta_preint,
+                                                                   const Eigen::VectorXd& _delta_step) const
 {
     return fti::plus(_delta_preint, _delta_step);
 }
 
-VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const CaptureBaseConstPtr _capture) const
+VectorXd ProcessorForceTorqueInertialDynamics::getCalibration(const CaptureBaseConstPtr _capture) const
 {
     if (_capture)
     {
@@ -473,26 +470,25 @@ VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const Captur
     else
     {
         VectorXd        calibration(13);
-        const Vector6d& I_calib = getSensor()->getStateBlock('I')->getState();
-        const Vector3d& C_calib = getSensor()->getStateBlock('C')->getState();
-        const Vector3d& i_calib = getSensor()->getStateBlock('i')->getState();
-        const Vector1d& m_calib = getSensor()->getStateBlock('m')->getState();
+        const Vector6d& I_calib = getSensor()->getStateBlockDynamic('I')->getState();
+        const Vector3d& C_calib = getSensor()->getStateBlockDynamic('C')->getState();
+        const Vector3d& i_calib = getSensor()->getStateBlockDynamic('i')->getState();
+        const Vector1d& m_calib = getSensor()->getStateBlockDynamic('m')->getState();
         calibration << I_calib, C_calib, i_calib, m_calib;
         return calibration;
     }
 }
 
-bool ProcessorForceTorqueInertialPreintDynamics::voteForKeyFrame() const
+bool ProcessorForceTorqueInertialDynamics::voteForKeyFrame() const
 {
     // time span
-    if (getBuffer().back().ts_ - getBuffer().front().ts_ >
-        params_force_torque_inertial_preint_dynamics_->max_time_span)
+    if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_force_torque_inertial_dynamics_->max_time_span)
     {
         WOLF_DEBUG("PM: vote: time span");
         return true;
     }
     // buffer length
-    if (getBuffer().size() > params_force_torque_inertial_preint_dynamics_->max_buff_length)
+    if (getBuffer().size() > params_force_torque_inertial_dynamics_->max_buff_length)
     {
         WOLF_DEBUG("PM: vote: buffer length");
         return true;
@@ -503,14 +499,14 @@ bool ProcessorForceTorqueInertialPreintDynamics::voteForKeyFrame() const
     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)
+    if (dist > params_force_torque_inertial_dynamics_->dist_traveled)
     {
         WOLF_DEBUG("PM: vote: distance traveled");
         return true;
     }
     // angle turned
     double angle = 2.0 * acos(getMotion().delta_integr_(15));
-    if (angle > params_force_torque_inertial_preint_dynamics_->angle_turned)
+    if (angle > params_force_torque_inertial_dynamics_->angle_turned)
     {
         WOLF_DEBUG("PM: vote: angle turned");
         return true;
@@ -524,6 +520,6 @@ bool ProcessorForceTorqueInertialPreintDynamics::voteForKeyFrame() const
 #include <core/processor/factory_processor.h>
 namespace wolf
 {
-WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertialPreintDynamics);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertialPreintDynamics);
+WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertialDynamics);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertialDynamics);
 }  // namespace wolf
\ No newline at end of file
diff --git a/src/processor/processor_force_torque_inertial_preint.cpp b/src/processor/processor_force_torque_inertial_preint.cpp
deleted file mode 100644
index 508be9dd558e32b11ff6ec151659562731bbb367..0000000000000000000000000000000000000000
--- a/src/processor/processor_force_torque_inertial_preint.cpp
+++ /dev/null
@@ -1,176 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/*
- * processor_preintegrator_force_torque_inertial.cpp
- *
- *  Created on: Aug 19, 2021
- *      Author: jsola
- */
-
-// bodydynamics
-#include "bodydynamics/processor/processor_force_torque_inertial_preint.h"
-#include "bodydynamics/factor/factor_force_torque_inertial.h"
-#include "bodydynamics/math/force_torque_inertial_delta_tools.h"
-
-// wolf
-#include <core/state_block/state_composite.h>
-#include <core/capture/capture_motion.h>
-
-namespace wolf
-{
-
-ProcessorForceTorqueInertialPreint::ProcessorForceTorqueInertialPreint(
-    ParamsProcessorForceTorqueInertialPreintPtr _params_force_torque_inertial_preint)
-    : ProcessorMotion("ProcessorForceTorqueInertialPreint",
-                      "POVL",
-                      3,   // dim
-                      13,  // state size [p, q, v, L]
-                      19,  // delta size [pi, vi, pd, vd, L, q]
-                      18,  // delta cov size
-                      12,  // data size [a, w, f, t]
-                      6,   // calib size [ab, wb]
-                      _params_force_torque_inertial_preint),
-      params_force_torque_inertial_preint_(_params_force_torque_inertial_preint)
-{
-    // TODO Auto-generated constructor stub
-}
-
-ProcessorForceTorqueInertialPreint::~ProcessorForceTorqueInertialPreint()
-{
-    // TODO Auto-generated destructor stub
-}
-
-CaptureMotionPtr ProcessorForceTorqueInertialPreint::emplaceCapture(const FrameBasePtr&   _frame_own,
-                                                                    const SensorBasePtr&  _sensor,
-                                                                    const TimeStamp&      _ts,
-                                                                    const VectorXd&       _data,
-                                                                    const MatrixXd&       _data_cov,
-                                                                    const VectorXd&       _calib,
-                                                                    const VectorXd&       _calib_preint,
-                                                                    const CaptureBasePtr& _capture_origin_ptr)
-{
-    CaptureMotionPtr capture = CaptureBase::emplace<CaptureMotion>(_frame_own, "CaptureForceTorqueInertial", _ts,
-                                                                   _sensor, _data, _data_cov, _capture_origin_ptr);
-    setCalibration(capture, _calib);
-    capture->setCalibrationPreint(_calib_preint);
-    return capture;
-}
-
-FeatureBasePtr ProcessorForceTorqueInertialPreint::emplaceFeature(CaptureMotionPtr _capture_own)
-{
-    FeatureBasePtr returnValue;
-    return returnValue;
-}
-
-FactorBasePtr ProcessorForceTorqueInertialPreint::emplaceFactor(FeatureBasePtr _feature_motion,
-                                                                CaptureBasePtr _capture_origin)
-{
-    FactorBasePtr returnValue;
-    return returnValue;
-}
-
-void ProcessorForceTorqueInertialPreint::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) 
-{
-    _capture->getStateBlock('I')->setState(_calibration); // Set IMU bias
-}
-
-void ProcessorForceTorqueInertialPreint::configure(SensorBasePtr _sensor)
-{
-    sensor_fti_ = std::static_pointer_cast<SensorForceTorqueInertial>(_sensor);
-}
-
-void ProcessorForceTorqueInertialPreint::computeCurrentDelta(const Eigen::VectorXd& _data,
-                                                             const Eigen::MatrixXd& _data_cov,
-                                                             const Eigen::VectorXd& _calib,
-                                                             const double           _dt,
-                                                             Eigen::VectorXd&       _delta,
-                                                             Eigen::MatrixXd&       _delta_cov,
-                                                             Eigen::MatrixXd&       _jacobian_calib) const
-{
-    // compute tangent by removing IMU bias
-    Matrix<double, 12, 1> tangent = _data;
-    tangent.head<6>() -= _calib;            // J_tangent_data  = Identity(12,12)
-    Matrix<double, 12, 6> J_tangent_calib;  // J_tangent_calib = [-Identity(6,6) ; Zero(6,6)]
-    J_tangent_calib.topRows<6>()    = -Matrix6d::Identity();
-    J_tangent_calib.bottomRows<6>() = Matrix6d::Zero();
-
-    // go from tangent to delta. We need to dynamic model for this
-    const Matrix<double, 7, 1>& model = sensor_fti_->getModel();
-    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);
-
-    // Compute cov and compose jacobians
-    const auto& J_delta_data = J_delta_tangent;  //  * J_tangent_data, which is the Identity;
-    _delta_cov               = J_delta_data * _data_cov * J_delta_data.transpose();
-    _jacobian_calib          = J_delta_tangent * J_tangent_calib;
-}
-
-void ProcessorForceTorqueInertialPreint::deltaPlusDelta(const Eigen::VectorXd& _delta1,
-                                                        const Eigen::VectorXd& _delta2,
-                                                        const double           _dt2,
-                                                        Eigen::VectorXd&       _delta1_plus_delta2) const
-{
-    bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2);
-}
-
-void ProcessorForceTorqueInertialPreint::deltaPlusDelta(const Eigen::VectorXd& _delta1,
-                                                        const Eigen::VectorXd& _delta2,
-                                                        const double           _dt2,
-                                                        Eigen::VectorXd&       _delta1_plus_delta2,
-                                                        Eigen::MatrixXd&       _jacobian1,
-                                                        Eigen::MatrixXd&       _jacobian2) const
-{
-    bodydynamics::fti::compose(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
-}
-
-void ProcessorForceTorqueInertialPreint::statePlusDelta(const VectorComposite& _x,
-                                                        const Eigen::VectorXd& _delta,
-                                                        const double           _dt,
-                                                        VectorComposite&       _x_plus_delta) const
-{
-    auto     x            = _x.vector("PVLO");
-    VectorXd x_plus_delta = bodydynamics::fti::composeOverState(x, _delta, _dt);
-    _x_plus_delta         = VectorComposite(x_plus_delta, "PVLO", {3, 3, 3, 4});
-}
-
-Eigen::VectorXd ProcessorForceTorqueInertialPreint::correctDelta(const Eigen::VectorXd& _delta_preint,
-                                                                 const Eigen::VectorXd& _delta_step) const
-{
-    return fti::plus(_delta_preint, _delta_step);
-}
-
-VectorXd ProcessorForceTorqueInertialPreint::getCalibration(const CaptureBaseConstPtr _capture) const
-{
-    if (_capture)
-        return _capture->getStateBlock('I')->getState();  // IMU bias
-    else
-        return getSensor()->getStateBlock('I')->getState();  // IMU bias
-}
-} /* namespace wolf */
-
-#include <core/processor/factory_processor.h>
-namespace wolf
-{
-WOLF_REGISTER_PROCESSOR(ProcessorForceTorqueInertialPreint);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorForceTorqueInertialPreint);
-}  // namespace wolf
\ No newline at end of file
diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp
deleted file mode 100644
index 1b84f786077a0f65ee3cc944bf8ebe4f54511499..0000000000000000000000000000000000000000
--- a/src/processor/processor_force_torque_preint.cpp
+++ /dev/null
@@ -1,272 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-// wolf
-#include "bodydynamics/math/force_torque_delta_tools.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
-#include "bodydynamics/feature/feature_force_torque_preint.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
-#include "bodydynamics/factor/factor_force_torque_preint.h"
-
-namespace wolf {
-
-int compute_data_size(int nbc, int dimc){
-    // compute the size of the data/body vectors used by processorMotion API depending
-    // on the number of contacts (nbc) and contact dimension (dimc)
-    // result: forces (+torques for 6D contacts) + pbc + wb + contact/base positions + contact/base orientations 
-    return nbc*dimc + 3 + 3 + nbc*3 + nbc*4;
-}
-
-using namespace Eigen;
-
-ProcessorForceTorquePreint::ProcessorForceTorquePreint(ParamsProcessorForceTorquePreintPtr _params_motion_force_torque_preint) :
-        ProcessorMotion("ProcessorForceTorquePreint", "CDLO", 3, 13, 13, 12, 
-                        compute_data_size(_params_motion_force_torque_preint->nbc, _params_motion_force_torque_preint->dimc), 
-                        6, _params_motion_force_torque_preint),
-        params_motion_force_torque_preint_(std::make_shared<ParamsProcessorForceTorquePreint>(*_params_motion_force_torque_preint)),
-        nbc_(_params_motion_force_torque_preint->nbc),
-        dimc_(_params_motion_force_torque_preint->dimc)
-
-{
-    //
-}
-
-ProcessorForceTorquePreint::~ProcessorForceTorquePreint()
-{
-    //
-}
-
-bool ProcessorForceTorquePreint::voteForKeyFrame() const
-{
-    // time span
-    if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_force_torque_preint_->max_time_span)
-    {
-        WOLF_DEBUG( "PM: vote: time span" );
-        return true;
-    }
-    // buffer length
-    if (getBuffer().size() > params_motion_force_torque_preint_->max_buff_length)
-    {
-        WOLF_DEBUG( "PM: vote: buffer length" );
-        return true;
-    }
-    
-    // Some other measure of movement?
-
-    //WOLF_DEBUG( "PM: do not vote" );
-    return false;
-}
-
-
-CaptureMotionPtr ProcessorForceTorquePreint::emplaceCapture(const FrameBasePtr& _frame_own,
-                                              const SensorBasePtr& _sensor,
-                                              const TimeStamp& _ts,
-                                              const VectorXd& _data,
-                                              const MatrixXd& _data_cov,
-                                              const VectorXd& _calib,
-                                              const VectorXd& _calib_preint,
-                                              const CaptureBasePtr& _capture_origin)
-{
-
-    // Here we have to retrieve the origin capture no
-    // !! PROBLEM:
-    // when doing setOrigin, emplaceCapture is called 2 times
-    // - first on the first KF (usually created by setPrior), this one contains the biases
-    // - secondly on the inner frame (last) which does not contains other captures
-    auto capture_ikin = _frame_own->getCaptureOf(sensor_ikin_);
-    auto capture_angvel = _frame_own->getCaptureOf(sensor_angvel_);
-    if ((capture_ikin == nullptr) || (capture_angvel == nullptr)){
-        // We have to retrieve the bias from a former Keyframe: origin
-        capture_ikin = origin_ptr_->getFrame()->getCaptureOf(sensor_ikin_);
-        capture_angvel = origin_ptr_->getFrame()->getCaptureOf(sensor_angvel_); 
-    }
-    auto cap = CaptureBase::emplace<CaptureForceTorquePreint>(
-                    _frame_own,
-                    _ts,
-                    _sensor,
-                    std::static_pointer_cast<CaptureInertialKinematics>(capture_ikin),
-                    std::static_pointer_cast<CaptureMotion>(capture_angvel),
-                    _data,
-                    _data_cov,
-                    _capture_origin);
-
-    // Cannot be recovered from the _calib and _calib_preint which are initialized using calib_size_
-    // which is zero in our case since the calibration stateblocks are not actually in the FTPreint sensor/captures
-
-    auto cap_ftpreint = std::static_pointer_cast<CaptureForceTorquePreint>(cap);
-    Vector6d calib = getCalibration(cap_ftpreint);
-    setCalibration(cap_ftpreint, calib);
-    cap_ftpreint->setCalibrationPreint(calib);
-
-    return cap;
-}
-
-FeatureBasePtr ProcessorForceTorquePreint::emplaceFeature(CaptureMotionPtr _capture_motion)
-{
-    // Retrieving the origin capture and getting the bias from here should be done here?
-    auto feature = FeatureBase::emplace<FeatureForceTorquePreint>(_capture_motion,
-                                                    _capture_motion->getBuffer().back().delta_integr_,
-                                                    _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
-                                                    _capture_motion->getCalibrationPreint(),
-                                                    _capture_motion->getBuffer().back().jacobian_calib_);
-    return feature;
-}
-
-Eigen::VectorXd ProcessorForceTorquePreint::correctDelta (const Eigen::VectorXd& delta_preint,
-                                                          const Eigen::VectorXd& delta_step) const
-{
-    return bodydynamics::plus(delta_preint, delta_step);
-}
-
-VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBaseConstPtr _capture) const
-{
-
-    VectorXd bias_vec(6);
-
-    if (_capture) // access from capture is quicker
-    {
-        auto cap_ft(std::static_pointer_cast<const CaptureForceTorquePreint>(_capture));
-
-        // get calib part from Ikin capture
-        bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState();
-
-        // get calib part from IMU capture
-        bias_vec.segment<3>(3) = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState().tail<3>();  // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
-    }
-    else // access from sensor is slower
-    {
-        // get calib part from Ikin capture
-        bias_vec.segment<3>(0) = sensor_ikin_->getIntrinsic()->getState();
-
-        // get calib part from IMU capture
-        bias_vec.segment<3>(3) = sensor_angvel_->getIntrinsic()->getState().tail<3>();  // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
-    }
-    return bias_vec;
-}
-
-void ProcessorForceTorquePreint::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
-{
-    CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture));
-
-    // set calib part in Ikin capture
-    cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->setState(_calibration.head<3>());
-
-    // set calib part in IMU capture
-    Vector6d calib_imu  = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState();
-    calib_imu.tail<3>() = _calibration.tail<3>();
-
-    cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->setState(calib_imu);
-}
-
-FactorBasePtr ProcessorForceTorquePreint::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
-{
-    CaptureForceTorquePreintPtr cap_ftpreint_origin = std::static_pointer_cast<CaptureForceTorquePreint>(_capture_origin);
-    FeatureForceTorquePreintPtr ftr_ftpreint = std::static_pointer_cast<FeatureForceTorquePreint>(_feature_motion);
-    CaptureForceTorquePreintPtr cap_ftpreint_new = std::static_pointer_cast<CaptureForceTorquePreint>(ftr_ftpreint->getCapture());
-
-    auto fac_ftpreint = FactorBase::emplace<FactorForceTorquePreint>(
-            ftr_ftpreint,
-            ftr_ftpreint,
-            cap_ftpreint_origin,
-            shared_from_this(),
-            cap_ftpreint_origin->getIkinCaptureOther(),  // to retrieve sb_bp1
-            cap_ftpreint_origin->getGyroCaptureOther(),  // to retrieve sb_w1
-            false);
-
-    return fac_ftpreint;
-}
-
-void ProcessorForceTorquePreint::computeCurrentDelta(
-                                       const Eigen::VectorXd& _data,
-                                       const Eigen::MatrixXd& _data_cov,
-                                       const Eigen::VectorXd& _calib,
-                                       const double _dt,
-                                       Eigen::VectorXd& _delta,
-                                       Eigen::MatrixXd& _delta_cov,
-                                       Eigen::MatrixXd& _jac_delta_calib) const
-{
-    assert(_data.size() == data_size_ && "Wrong data size!");
-
-    // create delta
-    MatrixXd jac_body_bias(data_size_-nbc_,6);
-    VectorXd body(data_size_);
-    bodydynamics::debiasData(_data, _calib, nbc_, dimc_, body, jac_body_bias);
-
-    MatrixXd jac_delta_body(12,data_size_-nbc_);
-    bodydynamics::body2delta(body, _dt, std::static_pointer_cast<const SensorForceTorque>(getSensor())->getMass(), 
-                             nbc_, dimc_,
-                             _delta, jac_delta_body); // Jacobians tested in bodydynamics_tools
-
-    // [f], [tau], pbc, wb
-    MatrixXd jac_delta_body_reduced = jac_delta_body.leftCols(nbc_*dimc_+6);
-
-    // compute delta_cov (using uncertainty on f,tau,pbc)
-    _delta_cov = jac_delta_body_reduced * _data_cov * jac_delta_body_reduced.transpose();  // trivially:  jac_body_delta = Identity
-    // _delta_cov = jac_delta_body * _data_cov * jac_delta_body.transpose();  // trivially:  jac_body_delta = Identity
-
-    // compute jacobian_calib
-    _jac_delta_calib = jac_delta_body * jac_body_bias;
-}
-
-void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
-                                  const Eigen::VectorXd& _delta,
-                                  const double _dt,
-                                  Eigen::VectorXd& _delta_preint_plus_delta) const
-{
-    _delta_preint_plus_delta = bodydynamics::compose(_delta_preint, _delta, _dt);
-}
-
-void ProcessorForceTorquePreint::statePlusDelta(const VectorComposite& _x,
-                                  const Eigen::VectorXd& _delta,
-                                  const double _dt,
-                                  VectorComposite& _x_plus_delta) const
-{
-    assert(_delta.size() == 13 && "Wrong _delta vector size");
-    assert(_dt >= 0 && "Time interval _dt is negative!");
-
-    // verbose way : create Eigen vectors, then compute, then convert back to Composite
-
-    auto x = _x.vector("CDLO");
-
-    VectorXd x_plus_delta = bodydynamics::composeOverState(x, _delta, _dt);
-
-    _x_plus_delta = VectorComposite(x_plus_delta, "CDLO", {3,3,3,4});
-}
-
-void ProcessorForceTorquePreint::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
-                                  const Eigen::VectorXd& _delta,
-                                  const double _dt,
-                                  Eigen::VectorXd& _delta_preint_plus_delta,
-                                  Eigen::MatrixXd& _jacobian_delta_preint,
-                                  Eigen::MatrixXd& _jacobian_delta) const
-{
-    bodydynamics::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in bodydynamics_tools
-}
-
-} // namespace wolf
-
-// Register in the FactorySensor
-#include "core/processor/factory_processor.h"
-
-namespace wolf
-{
-WOLF_REGISTER_PROCESSOR(ProcessorForceTorquePreint)
-}
diff --git a/src/sensor/sensor_force_torque_inertial.cpp b/src/sensor/sensor_force_torque_inertial.cpp
index 89fac3f9749e224738807467384d8aa837571638..aa38e8ab62d4bd0bdc0fee48bd15c227ea52fc8d 100644
--- a/src/sensor/sensor_force_torque_inertial.cpp
+++ b/src/sensor/sensor_force_torque_inertial.cpp
@@ -41,14 +41,13 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorXd&
                  false),
       params_fti_(_params)
 {
-    addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, true));  // centre of mass
+    addStateBlock('C', FactoryStateBlock::create("StateParams3", params_fti_->com, true));      // centre of mass
     addStateBlock('i', FactoryStateBlock::create("StateParams3", params_fti_->inertia, true));  // inertial vector
     addStateBlock('m', FactoryStateBlock::create("StateParams1", Vector1d(params_fti_->mass), true));  // total mass
     setStateBlockDynamic('I', false);
     setStateBlockDynamic('C', false);
     setStateBlockDynamic('i', false);
     setStateBlockDynamic('m', false);
-
 }
 
 // TODO: Adapt this API to that of MR !448
@@ -76,7 +75,8 @@ SensorForceTorqueInertial::SensorForceTorqueInertial(const VectorComposite&
 }  // namespace wolf
 
 #include <core/sensor/factory_sensor.h>
-namespace wolf{
+namespace wolf
+{
 WOLF_REGISTER_SENSOR(SensorForceTorqueInertial);
 WOLF_REGISTER_SENSOR_AUTO(SensorForceTorqueInertial);
-}
\ No newline at end of file
+}  // namespace wolf
\ No newline at end of file
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 425d6f1291d02b80f53ced0bcbe614bc88c402c1..52e1fd976f3619fc9b303b7c9ef601c740d10677 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -16,8 +16,6 @@ wolf_add_gtest(gtest_feature_inertial_kinematics gtest_feature_inertial_kinemati
 
 wolf_add_gtest(gtest_factor_inertial_kinematics gtest_factor_inertial_kinematics.cpp)
 
-wolf_add_gtest(gtest_factor_force_torque gtest_factor_force_torque.cpp)
-
 wolf_add_gtest(gtest_factor_force_torque_inertial gtest_factor_force_torque_inertial.cpp)
 
 wolf_add_gtest(gtest_factor_force_torque_inertial_dynamics gtest_factor_force_torque_inertial_dynamics.cpp)
@@ -27,11 +25,11 @@ wolf_add_gtest(gtest_force_torque_delta_tools gtest_force_torque_delta_tools.cpp
 wolf_add_gtest(gtest_force_torque_inertial_delta_tools gtest_force_torque_inertial_delta_tools.cpp)
 
 # TODO: revive those
-# wolf_add_gtest(gtest_feature_force_torque_preint gtest_feature_force_torque_preint.cpp)
+# wolf_add_gtest(gtest_feature_force_torque gtest_feature_force_torque.cpp)
 # wolf_add_gtest(gtest_processor_inertial_kinematics gtest_processor_inertial_kinematics.cpp)
-# wolf_add_gtest(gtest_processor_force_torque_preint gtest_processor_force_torque_preint.cpp)
+# wolf_add_gtest(gtest_processor_force_torque gtest_processor_force_torque.cpp)
 
-wolf_add_gtest(gtest_processor_force_torque_inertial_preint_dynamics gtest_processor_force_torque_inertial_preint_dynamics.cpp)
+wolf_add_gtest(gtest_processor_force_torque_inertial_dynamics gtest_processor_force_torque_inertial_dynamics.cpp)
 
 wolf_add_gtest(gtest_processor_point_feet_nomove gtest_processor_point_feet_nomove.cpp)
 
diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
deleted file mode 100644
index 0ccf5135e3d1bbe45a2c07cdbd68cf530dc9c571..0000000000000000000000000000000000000000
--- a/test/gtest_factor_force_torque.cpp
+++ /dev/null
@@ -1,865 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program.  If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-
-/**
- * \file gtest_factor_inertial_kinematics.cpp
- *
- *  Created on: Janv 29, 2020
- *      \author: Mederic Fourmy
- */
-
-/*
-
-
-Organisation: For each test, the problem, sensors, factors (if possible) are instanciated in a base class inheriting fromtesting::Test only.
-Then, each test case is derived in a child class which defines the data processed by the different sensors and expected estimated test values.
-Finally, each of these child classes is used in one or several Test in which basically the expected values are compared against estimation and
-solve is done with a perturbated system.
-
-Tests list:
-
-FactorInertialKinematics_2KF_Meas0_bpfix,ZeroMvt:
-FactorInertialKinematics_2KF_1p_bpfix,ZeroMvt:
-FactorInertialKinematics_2KF_m1p_pfix,ZeroMvt:
-FactorInertialKinematics_2KF_1v_bfix,ZeroMvt:
-*/
-
-
-// debug
-#include <iostream>
-#include <fstream>
-
-#include <core/utils/utils_gtest.h>
-#include <core/utils/logging.h>
-
-// WOLF
-#include <core/problem/problem.h>
-#include <core/ceres_wrapper/solver_ceres.h>
-#include <core/math/rotations.h>
-#include <core/capture/capture_base.h>
-#include <core/feature/feature_base.h>
-#include <core/factor/factor_block_absolute.h>
-#include <core/state_block/state_block_derived.h>
-
-// Imu
-// #include "imu/internal/config.h"
-// #include "imu/capture/capture_imu.h"
-// #include "imu/processor/processor_imu.h"
-// #include "imu/sensor/sensor_imu.h"
-
-// BODYDYNAMICS
-#include "bodydynamics/internal/config.h"
-#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/feature/feature_inertial_kinematics.h"
-#include "bodydynamics/factor/factor_inertial_kinematics.h"
-#include "bodydynamics/feature/feature_force_torque.h"
-#include "bodydynamics/factor/factor_force_torque.h"
-
-// ODOM3d
-#include "core/capture/capture_pose.h"
-#include "core/feature/feature_pose.h"
-#include "core/factor/factor_relative_pose_3d.h"
-
-#include <Eigen/Eigenvalues>
-
-using namespace wolf;
-using namespace Eigen;
-using namespace std;
-
-
-// SOME CONSTANTS
-const double Acc1 = 1;
-const double Acc2 = 3;
-const Vector3d zero3 = Vector3d::Zero();
-const Vector6d zero6 = Vector6d::Zero();
-
-
-
-Matrix9d computeKinJac(const Vector3d& w_unb,
-                       const Vector3d& p_unb,
-                       const Matrix3d& Iq)
-{
-    Matrix9d J; J.setZero();
-
-    Matrix3d wx = skew(w_unb);
-    Matrix3d px = skew(p_unb);
-    // Starting from top left, to the right and then next row
-    J.topLeftCorner<3,3>() = Matrix3d::Identity();
-    // J.block<3,3>(0,3) = Matrix3d::Zero();
-    // J.topRightCorner<3,3>() = Matrix3d::Zero();
-    J.block<3,3>(3,0) = -wx;
-    J.block<3,3>(3,3) = -Matrix3d::Identity();
-    J.block<3,3>(3,6) = px;
-    // J.bottomLeftCorner<3,3>() = Matrix3d::Zero();
-    // J.block<3,3>(6,3) = Matrix3d::Zero();
-    J.bottomRightCorner<3,3>() = -Iq;
-
-    return J;
-}
-
-Matrix9d computeKinCov(const Matrix3d& Qp,
-                              const Matrix3d& Qv,
-                              const Matrix3d& Qw,
-                              const Vector3d& w_unb,
-                              const Vector3d& p_unb,
-                              const Matrix3d& Iq)
-{
-    Matrix9d cov; cov.setZero();
-
-    Matrix3d wx = skew(w_unb);
-    Matrix3d px = skew(p_unb);
-    // Starting from top left, to the right and then next row
-    cov.topLeftCorner<3,3>() = Qp;
-    cov.block<3,3>(0,3) = Qp * wx;
-    // cov.topRightCorner<3,3>() = Matrix3d::Zero();
-    cov.block<3,3>(3,0) = cov.block<3,3>(0,3).transpose();
-    cov.block<3,3>(3,3) = -wx*Qp*wx + Qv - px*Qw*px;
-    cov.block<3,3>(3,6) = -px*Qw*Iq;
-    // cov.bottomLeftCorner<3,3>() = Matrix3d::Zero();
-    cov.block<3,3>(6,3) = Iq*Qw*px;
-    cov.bottomRightCorner<3,3>() = Iq*Qw*Iq;
-
-    return cov;
-}
-
-
-void perturbateIfUnFixed(const FrameBasePtr& KF, char sb_name){
-    if(!KF->getStateBlock(sb_name)->isFixed())
-    {
-        if (sb_name == 'O')
-        {
-            double max_rad_pert = M_PI / 3;
-            Vector3d do_pert = max_rad_pert*Vector3d::Random();
-            Quaterniond curr_state_q(KF->getO()->getState().data());
-            KF->getStateBlock('O')->setState((curr_state_q * exp_q(do_pert)).coeffs());
-        }
-        else
-        {
-            VectorXd pert;
-            pert.resize(KF->getStateBlock(sb_name)->getSize());
-            pert.setRandom(); pert *= 0.5;
-            KF->getStateBlock(sb_name)->setState(KF->getStateBlock(sb_name)->getState() + pert);
-        }
-    }
-}
-
-void perturbateAllIfUnFixed(const FrameBasePtr& KF)
-{
-    for (auto it: KF->getStateBlockMap()){
-        perturbateIfUnFixed(KF, it.first);
-    }
-}
-
-FrameBasePtr createKFWithCDLI(const ProblemPtr& problem,
-                              const TimeStamp&  t,
-                              VectorComposite   x_origin,
-                              Vector3d          c,
-                              Vector3d          cd,
-                              Vector3d          Lc,
-                              Vector6d          bias_imu)
-{
-    FrameBasePtr  KF  = FrameBase::emplace<FrameBase>(problem->getTrajectory(), t, "POV", x_origin);
-    StateBlockPtr sbc = make_shared<StatePoint3d>(c);
-    KF->addStateBlock('C', sbc, problem);
-    StateBlockPtr sbd = make_shared<StateVector3d>(cd);
-    KF->addStateBlock('D', sbd, problem);
-    StateBlockPtr sbL = make_shared<StateVector3d>(Lc);
-    KF->addStateBlock('L', sbL, problem);
-    StateBlockPtr sbbimu = make_shared<StateParams6>(bias_imu);
-    KF->addStateBlock('I', sbbimu, problem);  // Simulating IMU
-    return KF;
-}
-
-class FactorInertialKinematics_2KF : public testing::Test
-{
-    public:
-        double mass_;
-        wolf::TimeStamp tA_;
-        wolf::TimeStamp tB_;
-        ProblemPtr problem_;
-        SolverCeresPtr solver_;
-        VectorComposite x_origin_; // initial state
-        VectorComposite s_origin_; // initial sigmas for prior
-        SensorInertialKinematicsPtr SIK_;
-        CaptureInertialKinematicsPtr CIKA_, CIKB_;
-        FrameBasePtr KFA_;
-        FrameBasePtr KFB_;
-        Matrix3d Qp_, Qv_, Qw_;
-        Vector3d bias_p_;
-        Vector6d bias_imu_;
-        FeatureInertialKinematicsPtr FIKA_;
-        FeatureInertialKinematicsPtr FIKB_;
-        FeatureForceTorquePtr FFTAB_;
-        // SensorImuPtr sen_imu_;
-        // ProcessorImuPtr processor_imu_;
-
-    protected:
-    void SetUp() override
-    {
-
-        std::string bodydynamics_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
-
-        mass_ = 10.0; // Small 10 kg robot
-        //===================================================== SETTING PROBLEM
-        // WOLF PROBLEM
-        problem_ = Problem::create("POV", 3);
-
-        VectorXd extr_ik(0);
-        ParamsSensorInertialKinematicsPtr intr_ik = std::make_shared<ParamsSensorInertialKinematics>();
-        intr_ik->std_pbc = 0.1;
-        intr_ik->std_vbc = 0.1;
-        auto senik = problem_->installSensor("SensorInertialKinematics", "senIK", extr_ik, intr_ik);
-        SIK_ = std::static_pointer_cast<SensorInertialKinematics>(senik);
-
-        // CERES WRAPPER
-        solver_ = std::make_shared<SolverCeres>(problem_);
-        solver_->getSolverOptions().max_num_iterations = 1e3;
-        // solver_->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-        // solver_->getSolverOptions().max_line_search_step_contraction = 1e-3;
-
-        // INSTALL Imu SENSOR
-        // Vector7d extrinsics; extrinsics << 0,0,0,0,0,0,1;
-        // SensorBasePtr sen_imu = problem_->installSensor("SensorImu", "Main Imu Sensor", extrinsics, bodydynamics_root + "/demos/sensor_imu.yaml");
-        // sen_imu_ = std::static_pointer_cast<SensorImu>(sen_imu);
-        // ProcessorBasePtr prc_imu = problem_->installProcessor("ProcessorImu", "Imu PROC", "Main Imu Sensor", bodydynamics_root + "/demos/processor_imu.yaml");
-        // Vector6d data = zero6;
-        // wolf::CaptureImuPtr imu_ptr = std::make_shared<CaptureImu>(0, sen_imu_, data, sen_imu_->getNoiseCov(), zero6);
-        // // sen_imu_->process(imu_ptr);
-
-        // ======================= INITIALIZATION KFA WITH PRIOR + INERTIAL KINEMATIC FACTOR
-        tA_.set(0);
-        x_origin_ = VectorComposite("POV", {Vector3d(0,0,0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
-        s_origin_ = VectorComposite("POV", {1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1), 1e-3*Vector3d(1,1,1)});
-        KFA_ = problem_->setPriorFactor(x_origin_, s_origin_, tA_);
-
-
-        // ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
-        bias_p_ = zero3;
-        bias_imu_ = zero6;
-        StateBlockPtr sbcA = make_shared<StatePoint3d>(zero3); KFA_->addStateBlock('C', sbcA, problem_);
-        StateBlockPtr sbdA = make_shared<StateVector3d>(zero3); KFA_->addStateBlock('D', sbdA, problem_);
-        StateBlockPtr sbLA = make_shared<StateVector3d>(zero3); KFA_->addStateBlock('L', sbLA, problem_);
-        StateBlockPtr sbbimuA = make_shared<StateParams6>(bias_imu_); KFA_->addStateBlock('I', sbbimuA, problem_);
-
-        // Fix the one we cannot estimate
-        // KFA_->getP()->fix();
-        // KFA_->getO()->fix();
-        // KFA_->getV()->fix();
-        KFA_->getStateBlock('I')->fix(); // Imu
-
-        // INERTIAL KINEMATICS FACTOR
-        // Measurements
-        Vector3d pBC_measA = zero3;
-        Vector3d vBC_measA = zero3;
-        Vector3d w_measA = zero3;
-        Vector9d meas_ikinA; meas_ikinA << pBC_measA, vBC_measA, w_measA;
-        // momentum parameters
-        Matrix3d Iq; Iq.setIdentity();
-        Vector3d Lq = zero3;
-
-        Qp_ = pow(1e-2, 2)*Matrix3d::Identity();
-        Qv_ = pow(1e-2, 2)*Matrix3d::Identity();
-        Qw_ = pow(1e-2, 2)*Matrix3d::Identity();
-        Eigen::Matrix9d Q_ikin_err = computeKinCov(Qp_, Qv_, Qw_, meas_ikinA.head<3>()-bias_p_, meas_ikinA.tail<3>()-bias_imu_.tail<3>(), Iq);
-
-        CIKA_ = CaptureBase::emplace<CaptureInertialKinematics>(KFA_, tA_, SIK_, meas_ikinA, Iq, Lq, bias_p_);
-        CIKA_->getStateBlock('I')->fix(); // IK bias
-        FIKA_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKA_, meas_ikinA, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
-        FactorBase::emplace<FactorInertialKinematics>(FIKA_, FIKA_, sbbimuA, nullptr, false);
-
-
-        // =============== NEW KFB WITH CORRESPONDING STATEBLOCKS
-        tB_.set(1);
-
-        KFB_ = createKFWithCDLI(problem_, tB_, x_origin_,
-                                zero3, zero3, zero3, bias_imu_);
-        // Fix the one we cannot estimate
-        // KFB_->getP()->fix();
-        KFB_->getO()->fix();  // Not in the FT factor, so should be fixed (or absolute factor)
-        // KFB_->getV()->fix();
-        KFB_->getStateBlock('I')->fix();  // Imu
-
-
-        // // ================ PROCESS Imu DATA
-        // Vector6d data_imu;
-        // data_imu << -wolf::gravity(), 0,0,0;
-        // CaptureImuPtr cap_imu = std::make_shared<CaptureImu>(tB_, sen_imu_, data_imu, sen_imu_->getNoiseCov(), bias_imu_); //set data on Imu (measure only gravity here)
-        // // process data in capture
-        // // sen_imu_->process(cap_imu);
-
-        // ================ FACTOR INERTIAL KINEMATICS ON KFB
-        Vector3d pBC_measB = zero3;
-        Vector3d vBC_measB = zero3;
-        Vector3d w_measB = zero3;
-        Vector9d meas_ikinB; meas_ikinB << pBC_measB, vBC_measB, w_measB;
-        CIKB_ = CaptureBase::emplace<CaptureInertialKinematics>(KFB_, tB_, SIK_, meas_ikinB, Iq, Lq, bias_p_);
-        CIKB_->getSensorIntrinsic()->fix(); // IK bias
-        FIKB_ = FeatureBase::emplace<FeatureInertialKinematics>(CIKB_, meas_ikinB, Q_ikin_err, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
-        auto fac_inkinB = FactorBase::emplace<FactorInertialKinematics>(FIKB_, FIKB_, KFB_->getStateBlock('I'), nullptr, false);
-
-
-        // FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
-        Vector3d f1;     f1 << -mass_*wolf::gravity()/2;  // Only gravity -> static robot on both feet
-        Vector3d tau1; tau1 << 0,0,0;
-        Vector3d pbl1; pbl1 << 0,0,0;
-        Vector4d bql1; bql1 << 0,0,0,1;
-        Vector3d f2;     f2 << -mass_*wolf::gravity()/2;  // Only gravity -> static robot on both feet
-        Vector3d tau2; tau2 << 0,0,0;
-        Vector3d pbl2; pbl2 << 0,0,0;
-        Vector4d bql2; bql2 << 0,0,0,1;
-        Vector3d pbc;   pbc << 0,0,0;
-        // aggregated meas
-        Vector6d f_meas; f_meas << f1, f2;
-        Vector6d tau_meas; tau_meas << tau1, tau2;
-        Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
-
-        Matrix6d cov_f =   1e-4 * Matrix6d::Identity();
-        Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
-        Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
-
-        CaptureBasePtr cap_ftB = CaptureBase::emplace<CaptureBase>(KFB_, "CaptureForceTorque", tB_, nullptr);
-        FFTAB_ = FeatureBase::emplace<FeatureForceTorque>(cap_ftB,
-                        tB_ - tA_, mass_,
-                        f_meas, tau_meas, pbc, kin_meas,
-                        cov_f, cov_tau, cov_pbc);
-
-        FactorForceTorquePtr fac_ftAB = FactorBase::emplace<FactorForceTorque>(FFTAB_, FFTAB_, KFA_, CIKA_->getSensorIntrinsic(), nullptr, false);
-
-    }
-};
-
-
-class FactorInertialKinematics_2KF_foot1turned : public FactorInertialKinematics_2KF
-{
-    protected:
-    void SetUp() override
-    {
-        cout << "\n\n\nFactorInertialKinematics_2KF_foot1turned" << endl;
-        FactorInertialKinematics_2KF::SetUp();
-        problem_->print(3,false,true,true);
-        Vector4d WqL; WqL.setRandom(); WqL.normalize();  // random unitary quaternion
-        Quaterniond quat_WqL(WqL.data());
-        Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, WqL, 0,0,0,1;
-        Vector3d f1 = -mass_*wolf::gravity()/2;
-        f1 = quat_WqL.conjugate() * f1;  // Rotate force meas accordingly
-        Vector3d f2 = -mass_*wolf::gravity()/2;
-        Vector6d fmeas; fmeas << f1, f2;
-        FFTAB_->setKinMeas(kin_meas);
-        FFTAB_->setForcesMeas(fmeas);
-        // problem_->print(3,false,true,true);
-    }
-};
-
-class FactorInertialKinematics_2KF_foot2turned : public FactorInertialKinematics_2KF
-{
-    protected:
-    void SetUp() override
-    {
-        FactorInertialKinematics_2KF::SetUp();
-        Vector4d WqL; WqL.setRandom(); WqL.normalize();  // random unitary quaternion
-        Quaterniond quat_WqL(WqL.data());
-        Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, 0,0,0,1, WqL;
-        Vector3d f1 = -mass_*wolf::gravity()/2;
-        Vector3d f2 = -mass_*wolf::gravity()/2;
-        f2 = quat_WqL.conjugate() * f2;  // Rotate force meas accordingly
-        Vector6d fmeas; fmeas << f1, f2;
-        FFTAB_->setKinMeas(kin_meas);
-        FFTAB_->setForcesMeas(fmeas);
-    }
-};
-
-class FactorInertialKinematics_2KF_singleContactFoot2turned : public FactorInertialKinematics_2KF
-{
-    protected:
-    void SetUp() override
-    {
-        FactorInertialKinematics_2KF::SetUp();
-        Vector4d WqL; WqL.setRandom(); WqL.normalize();  // random unitary quaternion
-        Quaterniond quat_WqL(WqL.data());
-        Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, WqL, 0,0,0,1;
-        Vector3d f1 = -mass_*wolf::gravity();
-        f1 = quat_WqL.conjugate() * f1;  // Rotate force meas accordingly
-        Vector3d f2 = zero3;
-        Vector6d fmeas; fmeas << f1, f2;
-        FFTAB_->setKinMeas(kin_meas);
-        FFTAB_->setForcesMeas(fmeas);
-    }
-};
-
-class FactorInertialKinematics_2KF_singleContactFoot1turned : public FactorInertialKinematics_2KF
-{
-    protected:
-    void SetUp() override
-    {
-        FactorInertialKinematics_2KF::SetUp();
-        Vector4d WqL; WqL.setRandom(); WqL.normalize();  // random unitary quaternion
-        Quaterniond quat_WqL(WqL.data());
-        Matrix<double, 14, 1> kin_meas; kin_meas << zero3, zero3, 0,0,0,1, WqL;
-        Vector3d f1 = zero3;
-        Vector3d f2 = -mass_*wolf::gravity();
-        f2 = quat_WqL.conjugate() * f2;  // Rotate force meas accordingly
-        Vector6d fmeas; fmeas << f1, f2;
-        FFTAB_->setKinMeas(kin_meas);
-        FFTAB_->setForcesMeas(fmeas);
-    }
-};
-
-
-
-
-class FactorInertialKinematics_2KF_ForceX : public FactorInertialKinematics_2KF
-{
-    protected:
-    void SetUp() override
-    {
-        FactorInertialKinematics_2KF::SetUp();
-        Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
-        Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<mass_*Acc1/2,0,0).finished());
-        Vector6d fmeas; fmeas << f1, f2;
-        FFTAB_->setForcesMeas(fmeas);
-    }
-};
-
-class FactorInertialKinematics_2KF_ForceY : public FactorInertialKinematics_2KF
-{
-    protected:
-    void SetUp() override
-    {
-        FactorInertialKinematics_2KF::SetUp();
-        Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,mass_*Acc2/2,0).finished());
-        Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,mass_*Acc2/2,0).finished());
-        Vector6d fmeas; fmeas << f1, f2;
-        FFTAB_->setForcesMeas(fmeas);
-    }
-};
-
-class FactorInertialKinematics_2KF_ForceZ : public FactorInertialKinematics_2KF
-{
-    protected:
-    void SetUp() override
-    {
-        FactorInertialKinematics_2KF::SetUp();
-        Vector3d f1 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
-        Vector3d f2 = -mass_*wolf::gravity()/2 + ((Vector3d()<<0,0,mass_*Acc1/2).finished());
-        Vector6d fmeas; fmeas << f1, f2;
-        FFTAB_->setForcesMeas(fmeas);
-    }
-};
-
-class FactorInertialKinematics_3KF_ForceX : public FactorInertialKinematics_2KF_ForceX
-{
-    public:
-        FrameBasePtr KFC_;
-        CaptureInertialKinematicsPtr CIKC_;
-        TimeStamp tC_;
-
-    protected:
-    void SetUp() override
-    {
-        FactorInertialKinematics_2KF_ForceX::SetUp();
-        tC_.set(2);
-
-        // KFB_->getStateBlock("O")->unfix();
-
-        KFC_ = createKFWithCDLI(problem_, tC_, x_origin_,
-                                 zero3, zero3, zero3, bias_imu_);
-        // Fix the one we cannot estimate
-        // KFB_->getP()->fix();
-        // KFC_->getO()->fix();  // Not in the FT factor, so should be fixed (or absolute factor)
-        // KFB_->getV()->fix();
-        KFC_->getStateBlock('I')->fix();
-
-        // ================ FACTOR INERTIAL KINEMATICS ON KFB
-        Vector3d pBC_measC = zero3;
-        Vector3d vBC_measC = zero3;
-        Vector3d w_measC = zero3;
-        Vector9d meas_ikinC; meas_ikinC << pBC_measC, vBC_measC, w_measC;
-        Matrix3d Iq; Iq.setIdentity();
-        Vector3d Lq = zero3;
-        Eigen::Matrix9d Q_ikin_errC = computeKinCov(Qp_, Qv_, Qw_, meas_ikinC.head<3>()-bias_p_, meas_ikinC.tail<3>()-bias_imu_.tail<3>(), Iq);
-
-        CIKC_ = CaptureBase::emplace<CaptureInertialKinematics>(KFC_, tC_, SIK_, meas_ikinC, Iq, Lq, bias_p_);
-        CIKC_->getStateBlock('I')->fix(); // IK bias
-        auto feat_ikin_C = FeatureBase::emplace<FeatureInertialKinematics>(CIKC_, meas_ikinC, Q_ikin_errC, Iq, Lq, FeatureBase::UncertaintyType::UNCERTAINTY_IS_COVARIANCE);
-        FactorBase::emplace<FactorInertialKinematics>(feat_ikin_C, feat_ikin_C, CIKC_->getStateBlock('I'), nullptr, false);
-
-
-        // FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
-        Vector3d f1;     f1 << -mass_*wolf::gravity()/2;  // Only gravity -> static robot on both feet
-        Vector3d tau1; tau1 << 0,0,0;
-        Vector3d pbl1; pbl1 << 0,0,0;
-        Vector4d bql1; bql1 << 0,0,0,1;
-        Vector3d f2;     f2 << -mass_*wolf::gravity()/2;  // Only gravity -> static robot on both feet
-        Vector3d tau2; tau2 << 0,0,0;
-        Vector3d pbl2; pbl2 << 0,0,0;
-        Vector4d bql2; bql2 << 0,0,0,1;
-        Vector3d pbc;   pbc << 0,0,0;
-        // aggregated meas
-        Vector6d f_meas; f_meas << f1, f2;
-        Vector6d tau_meas; tau_meas << tau1, tau2;
-        Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
-
-        Matrix6d cov_f =   1e-4 * Matrix6d::Identity();
-        Matrix6d cov_tau = 1e-4 * Matrix6d::Identity();
-        Matrix3d cov_pbc = 1e-4 * Matrix3d::Identity();
-
-        CaptureBasePtr cap_ftC = CaptureBase::emplace<CaptureBase>(KFC_, "CaptureForceTorque", tC_, nullptr);
-        auto feat_ftBC = FeatureBase::emplace<FeatureForceTorque>(cap_ftC,
-                        tC_ - tB_, mass_,
-                        f_meas, tau_meas, pbc, kin_meas,
-                        cov_f, cov_tau, cov_pbc);
-        FactorForceTorquePtr fac_ftBC = FactorBase::emplace<FactorForceTorque>(feat_ftBC, feat_ftBC, KFB_, CIKB_->getSensorIntrinsic(), nullptr, false);
-    }
-};
-
-class FactorInertialKinematics_2KF_ForceX_Odom3d : public FactorInertialKinematics_2KF_ForceX
-{
-    protected:
-    void SetUp() override
-    {
-        FactorInertialKinematics_2KF_ForceX::SetUp();
-
-        Vector7d pose_A_B; pose_A_B << (tB_-tA_)*Acc1/2,0,0, 0,0,0,1;
-        Matrix6d rel_pose_cov = Matrix6d::Identity();
-        rel_pose_cov.topLeftCorner(3, 3) *= pow(1e-3, 2);
-        rel_pose_cov.bottomRightCorner(3, 3) *= pow(M_TORAD*1e-3, 2);
-
-        CaptureBasePtr cap_pose_base = CaptureBase::emplace<CapturePose>(KFB_, tB_, nullptr, pose_A_B, rel_pose_cov);
-        FeatureBasePtr ftr_pose_base = FeatureBase::emplace<FeaturePose>(cap_pose_base, pose_A_B, rel_pose_cov);
-        FactorBase::emplace<FactorRelativePose3d>(ftr_pose_base, ftr_pose_base, KFA_, nullptr, false, TOP_MOTION);
-
-        // With Odom3d the bias on relative base COM position is observable, we unfix the KFB state block
-        CIKB_->getStateBlock('I')->unfix();
-        // However this test is not sufficient to observe the bias at KFA
-        // CIKA_->getStateBlock('I')->unfix();  // this is not observable
-    }
-};
-
-
-
-
-
-////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////
-// =============== TEST FONCTIONS ======================
-////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////
-////////////////////////////////////////////////////////
-
-
-TEST_F(FactorInertialKinematics_2KF,ZeroMvt)
-{
-    Vector3d c_exp  = zero3;
-    Vector3d cd_exp = zero3;
-    Vector3d Lc_exp = zero3;
-    Vector3d bp_exp = zero3;
-
-    string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
-    perturbateAllIfUnFixed(KFA_);
-    perturbateAllIfUnFixed(KFB_);
-
-    // problem_->print(4,true,true,true);
-    report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-    // std::cout << report << std::endl;
-    // problem_->print(4,true,true,true);
-
-    ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
-    // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);  // due to opposite
-    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-TEST_F(FactorInertialKinematics_2KF_foot1turned,ZeroMvt)
-{
-    Vector3d c_exp  = zero3;
-    Vector3d cd_exp = zero3;
-    Vector3d Lc_exp = zero3;
-    Vector3d bp_exp = zero3;
-
-    string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
-    perturbateAllIfUnFixed(KFA_);
-    perturbateAllIfUnFixed(KFB_);
-
-    // problem_->print(4,true,true,true);
-    report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-    // std::cout << report << std::endl;
-    // problem_->print(4,true,true,true);
-
-    ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
-    // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);  // due to opposite
-    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-TEST_F(FactorInertialKinematics_2KF_foot2turned,ZeroMvt)
-{
-    Vector3d c_exp  = zero3;
-    Vector3d cd_exp = zero3;
-    Vector3d Lc_exp = zero3;
-    Vector3d bp_exp = zero3;
-
-    string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
-    perturbateAllIfUnFixed(KFA_);
-    perturbateAllIfUnFixed(KFB_);
-
-    // problem_->print(4,true,true,true);
-    report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-    // std::cout << report << std::endl;
-    // problem_->print(4,true,true,true);
-
-    ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
-    // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);  // due to opposite
-    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-
-TEST_F(FactorInertialKinematics_2KF_singleContactFoot1turned,ZeroMvt)
-{
-    Vector3d c_exp  = zero3;
-    Vector3d cd_exp = zero3;
-    Vector3d Lc_exp = zero3;
-    Vector3d bp_exp = zero3;
-
-    string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
-    perturbateAllIfUnFixed(KFA_);
-    perturbateAllIfUnFixed(KFB_);
-
-    // problem_->print(4,true,true,true);
-    report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-    // std::cout << report << std::endl;
-    // problem_->print(4,true,true,true);
-
-    ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
-    // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);  // due to opposite
-    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-
-TEST_F(FactorInertialKinematics_2KF_singleContactFoot2turned,ZeroMvt)
-{
-    Vector3d c_exp  = zero3;
-    Vector3d cd_exp = zero3;
-    Vector3d Lc_exp = zero3;
-    Vector3d bp_exp = zero3;
-
-    string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
-    perturbateAllIfUnFixed(KFA_);
-    perturbateAllIfUnFixed(KFB_);
-
-    // problem_->print(4,true,true,true);
-    report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-    // std::cout << report << std::endl;
-    // problem_->print(4,true,true,true);
-
-    ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
-    // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), c_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), cd_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), c_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), cd_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), Lc_exp, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), bp_exp, 1e-5);
-}
-
-
-
-TEST_F(FactorInertialKinematics_2KF_ForceX,ZeroMvt)
-{
-    // problem_->print(4,true,true,true);
-    string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
-    perturbateAllIfUnFixed(KFA_);
-    perturbateAllIfUnFixed(KFB_);
-
-    // problem_->print(4,true,true,true);
-    report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-    // std::cout << report << std::endl;
-    // problem_->print(4,true,true,true);
-
-    ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-}
-
-
-TEST_F(FactorInertialKinematics_2KF_ForceY,ZeroMvt)
-{
-    string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
-    perturbateAllIfUnFixed(KFA_);
-    perturbateAllIfUnFixed(KFB_);
-
-    // problem_->print(4,true,true,true);
-    report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-    // std::cout << report << std::endl;
-    // problem_->print(4,true,true,true);
-
-    ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
-    // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2/2, 0).finished(), 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, (tB_ - tA_)*Acc2, 0).finished(), 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-}
-
-
-TEST_F(FactorInertialKinematics_2KF_ForceZ,ZeroMvt)
-{
-    string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
-    perturbateAllIfUnFixed(KFA_);
-    perturbateAllIfUnFixed(KFB_);
-
-    // problem_->print(4,true,true,true);
-    report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-    // std::cout << report << std::endl;
-    // problem_->print(4,true,true,true);
-
-    ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
-    // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1/2).finished(), 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<0, 0, (tB_-tA_)*Acc1).finished(), 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-}
-
-// TEST_F(FactorInertialKinematics_3KF_ForceX,ZeroMvt)
-// {
-//     string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
-//     perturbateAllIfUnFixed(KFA_);
-//     perturbateAllIfUnFixed(KFB_);
-//     perturbateAllIfUnFixed(KFC_);
-
-//     // problem_->print(4,true,true,true);
-//     report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-//     // std::cout << report << std::endl;
-//     // problem_->print(4,true,true,true);
-
-//     ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
-//     // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
-//     ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
-//     ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
-//     ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
-//     ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
-//     ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
-//     ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
-//     ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
-//     ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
-//     ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-//     ASSERT_MATRIX_APPROX(KFC_->getStateBlock('C')->getState(), (Vector3d()<<(tC_-tB_)*Acc1/2 + (tB_-tA_)*Acc1, 0, 0).finished(), 1e-5); // + -> due to initial vel of KFB
-//     ASSERT_MATRIX_APPROX(KFC_->getStateBlock('D')->getState(), (Vector3d()<<(tC_-tB_)*Acc1, 0, 0).finished(), 1e-5);        // No acc btw B and C -> same vel
-//     ASSERT_MATRIX_APPROX(KFC_->getStateBlock('L')->getState(), zero3, 1e-5);
-//     ASSERT_MATRIX_APPROX(CIKC_->getStateBlock('I')->getState(), zero3, 1e-5);
-// }
-
-
-TEST_F(FactorInertialKinematics_2KF_ForceX_Odom3d,ZeroMvt)
-{
-    string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-
-    perturbateAllIfUnFixed(KFA_);
-    perturbateAllIfUnFixed(KFB_);
-
-    // problem_->print(4,true,true,true);
-    report = solver_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
-    // std::cout << report << std::endl;
-    // problem_->print(4,true,true,true);
-
-    ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.at('P'), 1e-5);
-    // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.at('O'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.at('V'), 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('C')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('D')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(KFA_->getStateBlock('L')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKA_->getStateBlock('I')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('C')->getState(), (Vector3d()<<(tB_-tA_)*Acc1/2, 0, 0).finished(), 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('D')->getState(), (Vector3d()<<(tB_-tA_)*Acc1, 0, 0).finished(), 1e-5);
-    ASSERT_MATRIX_APPROX(KFB_->getStateBlock('L')->getState(), zero3, 1e-5);
-    ASSERT_MATRIX_APPROX(CIKB_->getStateBlock('I')->getState(), zero3, 1e-5);
-}
-
-
-int main(int argc, char **argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-
-  return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_factor_force_torque_inertial.cpp b/test/gtest_factor_force_torque_inertial.cpp
index 489cb9df7ccde81c0e3f24731b260cc53d88dda6..72e5f59675df274e4387876ae644c081a61798ab 100644
--- a/test/gtest_factor_force_torque_inertial.cpp
+++ b/test/gtest_factor_force_torque_inertial.cpp
@@ -21,7 +21,7 @@
 //--------LICENSE_END--------
 
 #include "bodydynamics/factor/factor_force_torque_inertial.h"
-#include "bodydynamics/processor/processor_force_torque_inertial_preint.h"
+#include "bodydynamics/processor/processor_force_torque_inertial.h"
 #include "bodydynamics/sensor/sensor_force_torque_inertial.h"
 #include "bodydynamics/internal/config.h"
 
@@ -46,7 +46,7 @@ class Test_FactorForceTorqueInertialPreint : public testing::Test
   public:
     ProblemPtr                            problem;
     SensorForceTorqueInertialPtr          sensor;
-    ProcessorForceTorqueInertialPreintPtr processor;
+    ProcessorForceTorqueInertialPtr processor;
     FrameBasePtr                          frame_origin, frame_last, frame;
     CaptureMotionPtr                      capture_origin, capture_last, capture;
     FeatureMotionPtr                      feature;
@@ -71,7 +71,7 @@ class Test_FactorForceTorqueInertialPreint : public testing::Test
         problem                = Problem::autoSetup(server);
 
         sensor = std::static_pointer_cast<SensorForceTorqueInertial>(problem->getHardware()->getSensorList().front());
-        processor = std::static_pointer_cast<ProcessorForceTorqueInertialPreint>(sensor->getProcessorList().front());
+        processor = std::static_pointer_cast<ProcessorForceTorqueInertial>(sensor->getProcessorList().front());
 
         problem->print(4, 1, 1, 1);
 
diff --git a/test/gtest_factor_force_torque_inertial_dynamics.cpp b/test/gtest_factor_force_torque_inertial_dynamics.cpp
index a0a659fa636a0337a570c93f57d819b22f5c9ce5..846807390641df274b1b0d9cf8542183e6948067 100644
--- a/test/gtest_factor_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_factor_force_torque_inertial_dynamics.cpp
@@ -21,7 +21,7 @@
 //--------LICENSE_END--------
 
 #include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
-#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
 #include "bodydynamics/sensor/sensor_force_torque_inertial.h"
 #include "bodydynamics/internal/config.h"
 
@@ -47,7 +47,7 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
   public:
     ProblemPtr                                    P;
     SensorForceTorqueInertialPtr                  S;
-    ProcessorForceTorqueInertialPreintDynamicsPtr p;
+    ProcessorForceTorqueInertialDynamicsPtr       p;
     FrameBasePtr                                  F0, F1, F;
     CaptureMotionPtr                              C0, C1, C;
     FeatureMotionPtr                              f1;
@@ -69,7 +69,7 @@ class Test_FactorForceTorqueInertialDynamics_yaml : public testing::Test
         P                      = Problem::autoSetup(server);
 
         S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
-        p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
+        p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
 
         data     = VectorXd::Zero(12);  // [ a, w, f, t ]
         data_cov = MatrixXd::Identity(12, 12);
@@ -120,7 +120,7 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test
   public:
     ProblemPtr                                    P;
     SensorForceTorqueInertialPtr                  S;
-    ProcessorForceTorqueInertialPreintDynamicsPtr p;
+    ProcessorForceTorqueInertialDynamicsPtr p;
     FrameBasePtr                                  F0, F1;
     CaptureMotionPtr                              C0, C1;
     FeatureMotionPtr                              f1;
@@ -150,8 +150,8 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test
         S->setStateBlockDynamic('I', true);
 
         // crear processador
-        auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialPreintDynamics>();
-        p = ProcessorBase::emplace<ProcessorForceTorqueInertialPreintDynamics>(S, params_processor);
+        auto params_processor = std::make_shared<ParamsProcessorForceTorqueInertialDynamics>();
+        p = ProcessorBase::emplace<ProcessorForceTorqueInertialDynamics>(S, params_processor);
 
         // crear dos frame
         VectorXd state(13);
diff --git a/test/gtest_processor_force_torque_preint.cpp b/test/gtest_processor_force_torque.cpp
similarity index 96%
rename from test/gtest_processor_force_torque_preint.cpp
rename to test/gtest_processor_force_torque.cpp
index 3465ea6187358cffe667af6a2dd326d279e31e47..b00181b9bd356dbabce6e2d45c0d36a8626ac497 100644
--- a/test/gtest_processor_force_torque_preint.cpp
+++ b/test/gtest_processor_force_torque.cpp
@@ -21,7 +21,7 @@
 //--------LICENSE_END--------
 
 /**
- * \file gtest_factor_force_torque_preint.cpp
+ * \file gtest_factor_force_torque.cpp
  *
  *  Created on: March 20, 2020
  *      \author: Mederic Fourmy
@@ -57,9 +57,9 @@
 #include "bodydynamics/sensor/sensor_inertial_kinematics.h"
 #include "bodydynamics/sensor/sensor_force_torque.h"
 #include "bodydynamics/capture/capture_inertial_kinematics.h"
-#include "bodydynamics/capture/capture_force_torque_preint.h"
+#include "bodydynamics/capture/capture_force_torque.h"
 #include "bodydynamics/processor/processor_inertial_kinematics.h"
-#include "bodydynamics/processor/processor_force_torque_preint.h"
+#include "bodydynamics/processor/processor_force_torque.h"
 
 #include <Eigen/Eigenvalues>
 
@@ -81,7 +81,7 @@ const Vector3d PBCY = {0, -0.1, 0};  // Y axis offset
 const Vector3d PBCZ = {0, 0, -0.1};  // Z axis offset
 
 
-class ProcessorForceTorquePreintImuOdom3dIkin2KF : public testing::Test
+class ProcessorForceTorqueImuOdom3dIkin2KF : public testing::Test
 {
 public:
     wolf::TimeStamp t0_;
@@ -92,7 +92,7 @@ public:
     SensorForceTorquePtr sen_ft_;
     ProcessorImuPtr proc_imu_;
     ProcessorInertialKinematicsPtr proc_inkin_;
-    ProcessorForceTorquePreintPtr proc_ftpreint_;
+    ProcessorForceTorquePtr proc_ft_;
     FrameBasePtr KF1_;
 
     void SetUp() override
@@ -138,7 +138,7 @@ public:
         SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), intr_ft);
         // SensorBasePtr sen_ft_base = problem_->installSensor("SensorForceTorque", "SenFT", VectorXd(0), bodydynamics_root_dir + "/demos/sensor_ft.yaml");
         sen_ft_ = std::static_pointer_cast<SensorForceTorque>(sen_ft_base);
-        ParamsProcessorForceTorquePreintPtr params_sen_ft = std::make_shared<ParamsProcessorForceTorquePreint>();
+        ParamsProcessorForceTorquePtr params_sen_ft = std::make_shared<ParamsProcessorForceTorque>();
         params_sen_ft->sensor_ikin_name = "SenIK";
         params_sen_ft->sensor_angvel_name = "SenImu";
         params_sen_ft->nbc = 2;
@@ -150,16 +150,16 @@ public:
         params_sen_ft->dist_traveled = 20000.0;
         params_sen_ft->angle_turned = 1000;
         params_sen_ft->voting_active = true;
-        ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft_, params_sen_ft);
-        // ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint");
-        proc_ftpreint_ = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base);
+        ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorque", "PFT", sen_ft_, params_sen_ft);
+        // ProcessorBasePtr proc_ft_base = problem_->installProcessor("ProcessorForceTorque", "PFT", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint");
+        proc_ft_ = std::static_pointer_cast<ProcessorForceTorque>(proc_ft_base);
     }
 
     void TearDown() override {}
 };
 
 
-class Cst2KFZeroMotion : public ProcessorForceTorquePreintImuOdom3dIkin2KF
+class Cst2KFZeroMotion : public ProcessorForceTorqueImuOdom3dIkin2KF
 {
 public:
     FrameBasePtr KF2_;
@@ -212,7 +212,7 @@ public:
 
     void SetUp() override
     {
-        ProcessorForceTorquePreintImuOdom3dIkin2KF::SetUp();
+        ProcessorForceTorqueImuOdom3dIkin2KF::SetUp();
         t0_.set(0.0);
         TimeStamp t1; t1.set(1*DT);
         TimeStamp t2; t2.set(2*DT);
@@ -246,14 +246,14 @@ public:
         proc_imu_->setOrigin(KF1_);
         auto CIKin0 = std::make_shared<CaptureInertialKinematics>(t0_, sen_ikin_, meas_ikin_, Iq_, Lq_);
         CIKin0->process();
-        proc_ftpreint_->setOrigin(KF1_);
+        proc_ft_->setOrigin(KF1_);
 
         // T1
         CaptureImuPtr CImu1 = std::make_shared<CaptureImu>(t1, sen_imu_, acc_gyr_meas_, acc_gyr_cov_);
         CImu1->process();
         auto CIKin1 = std::make_shared<CaptureInertialKinematics>(t1, sen_ikin_, meas_ikin_, Iq_, Lq_);
         CIKin1->process();
-        auto CFTpreint1 = std::make_shared<CaptureForceTorquePreint>(t1, sen_ft_, CIKin1, CImu1, cap_ftp_data_, Qftp_);
+        auto CFTpreint1 = std::make_shared<CaptureForceTorque>(t1, sen_ft_, CIKin1, CImu1, cap_ftp_data_, Qftp_);
         CFTpreint1->process();
 
 
@@ -265,7 +265,7 @@ public:
         CImu2->process();
         auto CIKin2 = std::make_shared<CaptureInertialKinematics>(t2, sen_ikin_, meas_ikin_, Iq_, Lq_);
         CIKin2->process();
-        auto CFTpreint2 = std::make_shared<CaptureForceTorquePreint>(t2, sen_ft_, CIKin2, CImu2, cap_ftp_data_, Qftp_);
+        auto CFTpreint2 = std::make_shared<CaptureForceTorque>(t2, sen_ft_, CIKin2, CImu2, cap_ftp_data_, Qftp_);
         CFTpreint2->process();
 
         changeForData3();
@@ -275,7 +275,7 @@ public:
         CImu3->process();
         CaptureInertialKinematicsPtr CIKin3 = std::make_shared<CaptureInertialKinematics>(t3, sen_ikin_, meas_ikin_, Iq_, Lq_);
         CIKin3->process();
-        auto CFTpreint3 = std::make_shared<CaptureForceTorquePreint>(t3, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
+        auto CFTpreint3 = std::make_shared<CaptureForceTorque>(t3, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
         CFTpreint3->process();
 
         // T4, just here to make sure the KF at t3 is created
@@ -283,7 +283,7 @@ public:
         CImu4->process();
         CaptureInertialKinematicsPtr CIKin4 = std::make_shared<CaptureInertialKinematics>(t4, sen_ikin_, meas_ikin_, Iq_, Lq_);
         CIKin4->process();
-        auto CFTpreint4 = std::make_shared<CaptureForceTorquePreint>(t4, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
+        auto CFTpreint4 = std::make_shared<CaptureForceTorque>(t4, sen_ft_, CIKin3, CImu3, cap_ftp_data_, Qftp_);
         CFTpreint4->process();
 
         /////////////////////////////////////////////
diff --git a/test/gtest_processor_force_torque_inertial_preint.cpp b/test/gtest_processor_force_torque_inertial.cpp
similarity index 96%
rename from test/gtest_processor_force_torque_inertial_preint.cpp
rename to test/gtest_processor_force_torque_inertial.cpp
index 38910f16f75a89f70432c4d67ec1e9e0d60403a9..b8be3e23b05e5c64f1a63ad0e36c9c3559f81e2c 100644
--- a/test/gtest_processor_force_torque_inertial_preint.cpp
+++ b/test/gtest_processor_force_torque_inertial.cpp
@@ -20,13 +20,13 @@
 //
 //--------LICENSE_END--------
 /*
- * gtest_processor_preintegrator_force_torque_inertial.cpp
+ * gtest_processor_force_torque_inertial.cpp
  *
  *  Created on: Aug 19, 2021
  *      Author: jsola
  */
 
-#include "bodydynamics/processor/processor_force_torque_inertial_preint.h"
+#include "bodydynamics/processor/processor_force_torque_inertial.h"
 
 #include "core/utils/utils_gtest.h"
 #include "core/utils/logging.h"
diff --git a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp b/test/gtest_processor_force_torque_inertial_dynamics.cpp
similarity index 94%
rename from test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
rename to test/gtest_processor_force_torque_inertial_dynamics.cpp
index daa10f8db883230ad842e915b7f2882e02f59dc0..25df6c6d1d00683e5a77bc16f1a90511f41d991a 100644
--- a/test/gtest_processor_force_torque_inertial_preint_dynamics.cpp
+++ b/test/gtest_processor_force_torque_inertial_dynamics.cpp
@@ -21,7 +21,7 @@
 //--------LICENSE_END--------
 
 #include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
-#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
 #include "bodydynamics/sensor/sensor_force_torque_inertial.h"
 #include "bodydynamics/internal/config.h"
 
@@ -42,12 +42,12 @@ using namespace bodydynamics::fti;
 
 WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
 
-class Test_ProcessorForceTorqueInertialPreintDynamics_yaml : public testing::Test
+class Test_ProcessorForceTorqueInertialDynamics_yaml : public testing::Test
 {
   public:
     ProblemPtr                                    P;
     SensorForceTorqueInertialPtr                  S;
-    ProcessorForceTorqueInertialPreintDynamicsPtr p;
+    ProcessorForceTorqueInertialDynamicsPtr p;
 
   protected:
     void SetUp() override
@@ -58,17 +58,17 @@ class Test_ProcessorForceTorqueInertialPreintDynamics_yaml : public testing::Tes
         P                      = Problem::autoSetup(server);
 
         S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
-        p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
+        p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
     }
 };
 
-TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, force_registraion)
+TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, force_registraion)
 {
     FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
 }
 
 // Test to see if the processor works (yaml files). Here the dron is not moving
-TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, not_moving_test)
+TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, not_moving_test)
 {
     VectorXd data             = VectorXd::Zero(12);  // [ a, w, f, t ]
     data.segment<3>(0)        = - gravity();
@@ -137,7 +137,7 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, not_moving_test)
 }
 
 //Test to see if the processor works (yaml files). Here the dron is moving just 1m in x direction
-TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, 1m_x_moving_test__com_zero)
+TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, 1m_x_moving_test__com_zero)
 {
     VectorXd data      = VectorXd::Zero(12);  // [ a, w, f, t ]
     data.segment<3>(0) = -gravity();
@@ -205,7 +205,7 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, 1m_x_moving_test__c
 }
 
 //Test to see if the voteForKeyFrame works (distance traveled)
-TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_dist)
+TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, VoteForKeyFrame_dist)
 {
     VectorXd data      = VectorXd::Zero(12);  // [ a, w, f, t ]
     data.segment<3>(0) = -gravity();
@@ -231,7 +231,7 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_dis
 }
 
 //Test to see if the voteForKeyFrame works (buffer)
-TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_buffer)
+TEST_F(Test_ProcessorForceTorqueInertialDynamics_yaml, VoteForKeyFrame_buffer)
 {
     VectorXd data      = VectorXd::Zero(12);  // [ a, w, f, t ]
     data.segment<3>(0) = -gravity();
@@ -260,6 +260,6 @@ TEST_F(Test_ProcessorForceTorqueInertialPreintDynamics_yaml, VoteForKeyFrame_buf
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
-    // ::testing::GTEST_FLAG(filter) = "Test_ProcessorForceTorqueInertialPreintDynamics_yaml.not_moving_test";
+    // ::testing::GTEST_FLAG(filter) = "Test_ProcessorForceTorqueInertialDynamics_yaml.not_moving_test";
     return RUN_ALL_TESTS();
 }
\ No newline at end of file
diff --git a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
index 3c9ef6531c8608f91cca6818deed2fed6e64ad9b..20e26c06fdee4f8fd03e49b74c201107b1583e36 100644
--- a/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
+++ b/test/gtest_solve_problem_force_torque_inertial_dynamics.cpp
@@ -21,9 +21,10 @@
 //--------LICENSE_END--------
 
 #include "bodydynamics/factor/factor_force_torque_inertial_dynamics.h"
-#include "bodydynamics/processor/processor_force_torque_inertial_preint_dynamics.h"
+#include "bodydynamics/processor/processor_force_torque_inertial_dynamics.h"
 #include "bodydynamics/sensor/sensor_force_torque_inertial.h"
 #include "bodydynamics/internal/config.h"
+#include "bodydynamics/capture/capture_force_torque_inertial.h"
 
 #include <core/sensor/factory_sensor.h>
 
@@ -43,16 +44,16 @@ using namespace bodydynamics::fti;
 
 WOLF_REGISTER_STATEBLOCK_WITH_KEY(L, StateVector3d);
 
-class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::Test
+class Test_SolveProblemForceTorqueInertialDynamics_yaml : public testing::Test
 {
   public:
-    ProblemPtr                                    P;
-    SensorForceTorqueInertialPtr                  S;
-    ProcessorForceTorqueInertialPreintDynamicsPtr p;
-    SolverCeresPtr                                solver;
-    Vector3d                                      cdm_true;
-    Vector3d                                      inertia_true;
-    double                                        mass_true;
+    ProblemPtr                              P;
+    SensorForceTorqueInertialPtr            S;
+    ProcessorForceTorqueInertialDynamicsPtr p;
+    SolverCeresPtr                          solver;
+    Vector3d                                cdm_true;
+    Vector3d                                inertia_true;
+    double                                  mass_true;
 
   protected:
     void SetUp() override
@@ -70,7 +71,7 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::
         // solver->getSolverOptions().parameter_tolerance              = 1e-15;
 
         S = std::static_pointer_cast<SensorForceTorqueInertial>(P->getHardware()->getSensorList().front());
-        p = std::static_pointer_cast<ProcessorForceTorqueInertialPreintDynamics>(S->getProcessorList().front());
+        p = std::static_pointer_cast<ProcessorForceTorqueInertialDynamics>(S->getProcessorList().front());
 
         cdm_true     = {0, 0, 0.0341};
         inertia_true = {0.017598, 0.017957, 0.029599};
@@ -78,13 +79,13 @@ class Test_SolveProblemForceTorqueInertialPreintDynamics_yaml : public testing::
     }
 };
 
-TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, force_registraion)
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, force_registraion)
 {
     FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create);
 }
 
 // Hover test.
-TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hovering)
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_only_hovering)
 {
     double mass_guess = S->getStateBlock('m')->getState()(0);
 
@@ -101,14 +102,14 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
     S->getStateBlock('C')->fix();
     S->getStateBlock('i')->fix();
     S->getStateBlock('m')->unfix();
-    // S->setStateBlockDynamic('I');
+    S->setStateBlockDynamic('I');
 
-    CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
-    CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
-    CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
-    CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
-    CaptureMotionPtr C4_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
-    CaptureMotionPtr C5_1 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+    CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+    CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
+    CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>(0.4, S, data, data_cov, nullptr);
+    CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>(0.6, S, data, data_cov, nullptr);
+    CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>(0.8, S, data, data_cov, nullptr);
+    CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>(1.0, S, data, data_cov, nullptr);
 
     C0_0->process();
     CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
@@ -135,10 +136,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
     WOLF_INFO("-----------------------------");
 
 
-    CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
     for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 7.0 ; t += 1.0)
     {
-        C->setTimeStamp(t);
+        CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
         C->process();
         p->getOrigin()->getFrame()->fix();
         report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
@@ -152,7 +152,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_only_hoveri
     ASSERT_NEAR(S->getStateBlock('m')->getState()(0), mass_true, 1e-3);
 }
 
-TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hovering)
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, cdm_only_hovering)
 {
     S->getStateBlock('C')->setState(Vector3d(0.01, 0.01, 0.033));
     S->getStateBlock('m')->setState(Vector1d(mass_true));
@@ -172,12 +172,12 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
     S->getStateBlock('i')->fix();
     S->getStateBlock('m')->fix();
 
-    CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
-    CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
-    CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
-    CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
-    CaptureMotionPtr C4_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
-    CaptureMotionPtr C5_1 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+    CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>( 0.0, S, data, data_cov, nullptr);
+    CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>( 0.2, S, data, data_cov, nullptr);
+    CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>( 0.4, S, data, data_cov, nullptr);
+    CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>( 0.6, S, data, data_cov, nullptr);
+    CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>( 0.8, S, data, data_cov, nullptr);
+    CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>( 1.0, S, data, data_cov, nullptr);
 
     C0_0->process();
     CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
@@ -204,10 +204,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
     WOLF_INFO("Estimated cdm: ", S->getStateBlock('C')->getState().transpose(), " m.");
     WOLF_INFO("-----------------------------");
 
-    CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
     for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 11.0 ; t += 1.0)
     {
-        C->setTimeStamp(t);
+        CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>( t, S, data, data_cov, nullptr);
         C->process();
         p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
         p->getOrigin()->getFrame()->fix();
@@ -220,7 +219,7 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, cdm_only_hoverin
     ASSERT_MATRIX_APPROX(S->getStateBlock('C')->getState().head(2), cdm_true.head(2), 1e-5); // cdm_z is not observable while hovering
 }
 
-TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hovering)
+TEST_F(Test_SolveProblemForceTorqueInertialDynamics_yaml, mass_and_cdm_hovering)
 {
     S->getStateBlock('C')->setState(Vector3d(0.005, 0.005, 0.05));
     S->getStateBlock('m')->setState(Vector1d(1.900));
@@ -240,13 +239,14 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
     S->getStateBlock('C')->unfix();
     S->getStateBlock('i')->fix();
     S->getStateBlock('m')->unfix();
+    S->setStateBlockDynamic('I');
 
-    CaptureMotionPtr C0_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
-    CaptureMotionPtr C1_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.2, S, data, data_cov, nullptr);
-    CaptureMotionPtr C2_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.4, S, data, data_cov, nullptr);
-    CaptureMotionPtr C3_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.6, S, data, data_cov, nullptr);
-    CaptureMotionPtr C4_0 = std::make_shared<CaptureMotion>("CaptureMotion", 0.8, S, data, data_cov, nullptr);
-    CaptureMotionPtr C5_1 = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
+    CaptureMotionPtr C0_0 = std::make_shared<CaptureForceTorqueInertial>(0.0, S, data, data_cov, nullptr);
+    CaptureMotionPtr C1_0 = std::make_shared<CaptureForceTorqueInertial>(0.2, S, data, data_cov, nullptr);
+    CaptureMotionPtr C2_0 = std::make_shared<CaptureForceTorqueInertial>(0.4, S, data, data_cov, nullptr);
+    CaptureMotionPtr C3_0 = std::make_shared<CaptureForceTorqueInertial>(0.6, S, data, data_cov, nullptr);
+    CaptureMotionPtr C4_0 = std::make_shared<CaptureForceTorqueInertial>(0.8, S, data, data_cov, nullptr);
+    CaptureMotionPtr C5_1 = std::make_shared<CaptureForceTorqueInertial>(1.0, S, data, data_cov, nullptr);
 
     C0_0->process();
     CaptureMotionPtr C_KF0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
@@ -283,9 +283,9 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
     // which if not eliminated contaminate the overall solution.
     // This is due to these first factors relying on the linearization Jacobian to correct the
     // poorly preintegrated delta.
-    CaptureMotionPtr C = std::make_shared<CaptureMotion>("CaptureMotion", 2.0, S, data, data_cov, nullptr);
     for (TimeStamp t = p->getLast()->getTimeStamp() + 1.0 ; t <= 7.0 ; t += 1.0)
     {
+        CaptureMotionPtr C = std::make_shared<CaptureForceTorqueInertial>(t, S, data, data_cov, nullptr);
         C->setTimeStamp(t);
         C->process();
         p->getOrigin()->getFrame()->getStateBlock('L')->setState(Vector3d::Zero()); // Make sure L is not moving due to torques
@@ -307,6 +307,6 @@ TEST_F(Test_SolveProblemForceTorqueInertialPreintDynamics_yaml, mass_and_cdm_hov
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
-    // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialPreintDynamics_yaml.cdm_only_hovering";
+    // ::testing::GTEST_FLAG(filter) = "Test_SolveProblemForceTorqueInertialDynamics_yaml.cdm_only_hovering";
     return RUN_ALL_TESTS();
 }
diff --git a/test/yaml/problem_force_torque_inertial.yaml b/test/yaml/problem_force_torque_inertial.yaml
index 2082a65ad6580b58d0e234a9f9594fd615646010..e2ae117aee579fb6f444cce6f378b2f1c673e739 100644
--- a/test/yaml/problem_force_torque_inertial.yaml
+++ b/test/yaml/problem_force_torque_inertial.yaml
@@ -32,8 +32,8 @@ config:
   processors:
    -
     name: "proc FTI"
-    type: "ProcessorForceTorqueInertialPreint"
+    type: "ProcessorForceTorqueInertial"
     sensor_name: "sensor FTI"
     plugin: "bodydynamics"
-    follow: "processor_force_torque_inertial_preint.yaml"
+    follow: "processor_force_torque_inertial.yaml"
     
\ No newline at end of file
diff --git a/test/yaml/problem_force_torque_inertial_dynamics.yaml b/test/yaml/problem_force_torque_inertial_dynamics.yaml
index 2d9b19cc19d0519737336367906b1ee1e2621f88..207b69bba5597a63cc41167fbe7a3106ffa4606a 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics.yaml
@@ -32,8 +32,8 @@ config:
   processors:
    -
     name: "proc FTID"
-    type: "ProcessorForceTorqueInertialPreintDynamics"
+    type: "ProcessorForceTorqueInertialDynamics"
     sensor_name: "sensor FTI"
     plugin: "bodydynamics"
-    follow: "processor_force_torque_inertial_preint_dynamics.yaml"
+    follow: "processor_force_torque_inertial_dynamics.yaml"
     
\ No newline at end of file
diff --git a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
index 5ef4ecbe3e2c57c493d962ef0d80a72b5d94ff2e..233368f38accb87f4bbd4500e37bede35dd1378a 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_processor_test.yaml
@@ -41,7 +41,7 @@ config:
   processors:
    -
     name: "proc FTID"
-    type: "ProcessorForceTorqueInertialPreintDynamics"
+    type: "ProcessorForceTorqueInertialDynamics"
     sensor_name: "sensor FTI"
     plugin: "bodydynamics"
     time_tolerance: 0.1
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 182966b1bea83384e4f0fd498e98f5970944cade..8eb8204126f94bc008bcb01587ae2fa5482fec6b 100644
--- a/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
+++ b/test/yaml/problem_force_torque_inertial_dynamics_solve_test.yaml
@@ -48,7 +48,7 @@ config:
   processors:
    -
     name: "proc FTID"
-    type: "ProcessorForceTorqueInertialPreintDynamics"
+    type: "ProcessorForceTorqueInertialDynamics"
     sensor_name: "sensor FTI"
     plugin: "bodydynamics"
     time_tolerance: 0.1
diff --git a/test/yaml/processor_force_torque_inertial_preint.yaml b/test/yaml/processor_force_torque_inertial.yaml
similarity index 100%
rename from test/yaml/processor_force_torque_inertial_preint.yaml
rename to test/yaml/processor_force_torque_inertial.yaml
diff --git a/test/yaml/processor_force_torque_inertial_preint_dynamics.yaml b/test/yaml/processor_force_torque_inertial_dynamics.yaml
similarity index 100%
rename from test/yaml/processor_force_torque_inertial_preint_dynamics.yaml
rename to test/yaml/processor_force_torque_inertial_dynamics.yaml