diff --git a/test/gtest_factor_force_torque.cpp b/test/gtest_factor_force_torque.cpp
index 25ac26708a510aaef28a5da6466ef6b3924983ae..5e55b6171b199d9b4228ce5495d910f248f42290 100644
--- a/test/gtest_factor_force_torque.cpp
+++ b/test/gtest_factor_force_torque.cpp
@@ -16,10 +16,10 @@ solve is done with a perturbated system.
 
 Tests list:
 
-FactorInertialKinematics_1KF_Meas0_bpfix,ZeroMvt:
-FactorInertialKinematics_1KF_1p_bpfix,ZeroMvt:
-FactorInertialKinematics_1KF_m1p_pfix,ZeroMvt:
-FactorInertialKinematics_1KF_1v_bfix,ZeroMvt:
+FactorInertialKinematics_2KF_Meas0_bpfix,ZeroMvt:
+FactorInertialKinematics_2KF_1p_bpfix,ZeroMvt:
+FactorInertialKinematics_2KF_m1p_pfix,ZeroMvt:
+FactorInertialKinematics_2KF_1v_bfix,ZeroMvt:
 */
 
 
@@ -58,6 +58,11 @@ using namespace Eigen;
 using namespace std;
 
 
+// SOME CONSTANTS
+const double Acc1 = 1.0;
+const double Acc2 = 2.5;
+
+
 Matrix9d computeKinJac(const Vector3d& w_unb,
                        const Vector3d& p_unb,
                        const Matrix3d& Iq)
@@ -115,14 +120,21 @@ void perturbateIfUnFixed(const FrameBasePtr& KF, std::string sb_name){
         }
         else 
         {   
-            Eigen::Vector3d pert = Eigen::Vector3d::Random();
+            Vector3d pert = Vector3d::Random();
             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);
+    }
+}
+
 
-class FactorInertialKinematics_1KF : public testing::Test
+class FactorInertialKinematics_2KF : public testing::Test
 {
     public:
         double mass_;
@@ -134,10 +146,11 @@ class FactorInertialKinematics_1KF : public testing::Test
         MatrixXd P_origin_;
         FrameBasePtr KFA_;
         FrameBasePtr KFB_;
-        Eigen::Matrix3d Qp_, Qv_, Qw_;
-        Eigen::Vector3d bias_p_, bias_imu_;
+        Matrix3d Qp_, Qv_, Qw_;
+        Vector3d bias_p_, bias_imu_;
         FeatureInertialKinematicsPtr feat_ikinA_;
         FeatureInertialKinematicsPtr feat_ikinB_;
+        FeatureForceTorquePtr feat_ftAB_;
         // SensorIMUPtr sen_imu_;
         // ProcessorIMUPtr processor_imu_;
 
@@ -159,19 +172,19 @@ class FactorInertialKinematics_1KF : public testing::Test
         ceres_manager_ = new CeresManager(problem_, ceres_options);
 
         // INSTALL IMU SENSOR
-        // Eigen::Vector7d extrinsics; extrinsics << 0,0,0,0,0,0,1;
+        // 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");
-        // Eigen::Vector6d data = Eigen::Vector6d::Zero();
-        // wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(0, sen_imu_, data, sen_imu_->getNoiseCov(), Eigen::Vector6d::Zero());
+        // Vector6d data = Vector6d::Zero();
+        // wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(0, sen_imu_, data, sen_imu_->getNoiseCov(), Vector6d::Zero());
         // // sen_imu_->process(imu_ptr);
 
         // ======================= INITIALIZATION KFA WITH PRIOR + INERTIAL KINEMATIC FACTOR
         tA_.set(0);
         x_origin_.resize(10);
         x_origin_ << 0,0,0, 0,0,0,1, 0,0,0;
-        P_origin_ = pow(1e-3, 2) * Eigen::Matrix9d::Identity();
+        P_origin_ = pow(1e-3, 2) * Matrix9d::Identity();
 
         // Set origin of the problem
         KFA_ = problem_->setPrior(x_origin_, P_origin_.topLeftCorner<6,6>(), tA_, 0.005);
@@ -210,9 +223,9 @@ class FactorInertialKinematics_1KF : public testing::Test
         Matrix3d Iq; Iq.setIdentity(); 
         Vector3d Lq = zero3; 
 
-        Qp_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
-        Qv_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
-        Qw_ = pow(1e-2, 2)*Eigen::Matrix3d::Identity();
+        Qp_ = pow(1e-2, 2)*Matrix3d::Identity();
+        Qv_ = pow(1e-2, 2)*Matrix3d::Identity();
+        Qw_ = pow(1e-2, 2)*Matrix3d::Identity();
         Matrix9d Qkin = computeKinCov(Qp_, Qv_, Qw_, pBC_measA-bias_p_, w_measA-bias_imu_.tail(3), Iq);
 
         // FACTOR INERTIAL KINEMATICS ON KFA
@@ -241,7 +254,7 @@ class FactorInertialKinematics_1KF : public testing::Test
 
 
         // // ================ PROCESS IMU DATA
-        // Eigen::Vector6d data_imu;
+        // 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
@@ -259,64 +272,59 @@ class FactorInertialKinematics_1KF : public testing::Test
 
 
         // FORCE TORQUE FACTOR BETWEEEN KFA AND KFB
-        Eigen::Vector3d f1;     f1 << -mass_*wolf::gravity()/2;  // Only gravity -> static robot on both feet
-        Eigen::Vector3d tau1; tau1 << 0,0,0;
-        Eigen::Vector3d pbl1; pbl1 << 0,0,0;
-        Eigen::Vector4d bql1; bql1 << 0,0,0,1;
-        Eigen::Vector3d f2;     f2 << -mass_*wolf::gravity()/2;  // Only gravity -> static robot on both feet
-        Eigen::Vector3d tau2; tau2 << 0,0,0;
-        Eigen::Vector3d pbl2; pbl2 << 0,0,0;
-        Eigen::Vector4d bql2; bql2 << 0,0,0,1;
-        Eigen::Vector3d pbc;   pbc << 0,0,0;
+        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
-        Eigen::Vector6d f_meas; f_meas << f1, f2;
-        Eigen::Vector6d tau_meas; tau_meas << tau1, tau2;
-        Eigen::Matrix<double,14,1> kin_meas; kin_meas << pbl1, pbl2, bql1, bql2;
+        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;
 
-        Eigen::Matrix6d cov_f =   1e-4 * Eigen::Matrix6d::Identity(); 
-        Eigen::Matrix6d cov_tau = 1e-4 * Eigen::Matrix6d::Identity(); 
-        Eigen::Matrix3d cov_pbc = 1e-4 * Eigen::Matrix3d::Identity(); 
+        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);
-        auto feat_ftAB = FeatureBase::emplace<FeatureForceTorque>(cap_ftB,
+        feat_ftAB_ = FeatureBase::emplace<FeatureForceTorque>(cap_ftB,
                         tB_ - tA_, mass_,  
                         f_meas, tau_meas, pbc, kin_meas,
                         cov_f, cov_tau, cov_pbc);
-        FactorForceTorquePtr fac_ftB = FactorBase::emplace<FactorForceTorque>(feat_ftAB, feat_ftAB, KFA_, nullptr, false);
+        FactorForceTorquePtr fac_ftB = FactorBase::emplace<FactorForceTorque>(feat_ftAB_, feat_ftAB_, KFA_, nullptr, false);
     }
 
     virtual void TearDown(){}
 };
 
 
-class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics_1KF
+class FactorInertialKinematics_2KF_FX : public FactorInertialKinematics_2KF
 {
     virtual void SetUp() override
     {
-        FactorInertialKinematics_1KF::SetUp();
-        // Fix the bp bias
-        KFA_->getStateBlock("B")->fix();
-        KFA_->getStateBlock("C")->unfix();
-        Eigen::Vector3d zero3 = Eigen::Vector3d::Zero(); 
-
-        Eigen::Vector3d pBC_measA = zero3; 
-        Eigen::Vector3d vBC_measA = zero3;
-        Eigen::Vector3d w_measA = zero3; 
-        Eigen::Vector9d meas; meas << pBC_measA, vBC_measA, w_measA;
-        // momentum parameters
-        Eigen::Matrix3d Iq; Iq.setIdentity(); 
-        Eigen::Vector3d Lq = zero3; 
-
-        Matrix9d Qkin = computeKinCov(Qp_, Qv_, Qw_, pBC_measA-bias_p_, w_measA-bias_imu_.tail(3), Iq);
-
-
-        feat_ikinB_->setMeasurement(meas);
-        feat_ikinB_->setMeasurementCovariance(Qkin);
-        feat_ikinB_->setBIq(Iq);
-        feat_ikinB_->setBLcm(Lq);
+        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; 
+        feat_ftAB_->setForcesMeas(fmeas);
     }
 };
 
+class FactorInertialKinematics_2KF_FY : public FactorInertialKinematics_2KF
+{
+    virtual 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; 
+        feat_ftAB_->setForcesMeas(fmeas);
+    }
+};
 
 
 ////////////////////////////////////////////////////////
@@ -324,37 +332,26 @@ class FactorInertialKinematics_1KF_Meas0_bpfix : public FactorInertialKinematics
 ////////////////////////////////////////////////////////
 
 
-TEST_F(FactorInertialKinematics_1KF,ZeroMvt)
+TEST_F(FactorInertialKinematics_2KF,ZeroMvt)
 {   
-    Eigen::Vector3d c_exp  = Eigen::Vector3d::Zero(); 
-    Eigen::Vector3d cd_exp = Eigen::Vector3d::Zero(); 
-    Eigen::Vector3d Lc_exp = Eigen::Vector3d::Zero(); 
-    Eigen::Vector3d bp_exp = Eigen::Vector3d::Zero(); 
+    Vector3d c_exp  = Vector3d::Zero(); 
+    Vector3d cd_exp = Vector3d::Zero(); 
+    Vector3d Lc_exp = Vector3d::Zero(); 
+    Vector3d bp_exp = Vector3d::Zero(); 
 
     string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
 
-    perturbateIfUnFixed(KFA_, "P");
-    perturbateIfUnFixed(KFA_, "O");
-    perturbateIfUnFixed(KFA_, "V");
-    perturbateIfUnFixed(KFA_, "C");
-    perturbateIfUnFixed(KFA_, "D");
-    perturbateIfUnFixed(KFA_, "L");
-    perturbateIfUnFixed(KFA_, "B");
-
-    perturbateIfUnFixed(KFB_, "P");
-    perturbateIfUnFixed(KFB_, "O");
-    perturbateIfUnFixed(KFB_, "V");
-    perturbateIfUnFixed(KFB_, "C");
-    perturbateIfUnFixed(KFB_, "D");
-    perturbateIfUnFixed(KFB_, "L");
-    perturbateIfUnFixed(KFB_, "B");
+    perturbateAllIfUnFixed(KFA_);
+    perturbateAllIfUnFixed(KFB_);
 
     problem_->print(4,true,true,true);
     report = ceres_manager_->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_.head(3), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getP()->getState(), x_origin_.head<3>(), 1e-5);
+    // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5);  // due to opposite
+    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 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);
@@ -366,6 +363,56 @@ TEST_F(FactorInertialKinematics_1KF,ZeroMvt)
 }
 
 
+TEST_F(FactorInertialKinematics_2KF_FX,ZeroMvt)
+{   
+    string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+    perturbateAllIfUnFixed(KFA_);
+    perturbateAllIfUnFixed(KFB_);
+
+    problem_->print(4,true,true,true);
+    report = ceres_manager_->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_.head<3>(), 1e-5);
+    // ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<Acc1/2, 0, 0).finished(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<Acc1, 0, 0).finished(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+}
+
+
+TEST_F(FactorInertialKinematics_2KF_FY,ZeroMvt)
+{   
+    string report = ceres_manager_->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport;
+
+    perturbateAllIfUnFixed(KFA_);
+    perturbateAllIfUnFixed(KFB_);
+
+    problem_->print(4,true,true,true);
+    report = ceres_manager_->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_.head<3>(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getO()->getState(), x_origin_.segment<4>(3), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getV()->getState(), x_origin_.tail<3>(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("C")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("D")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFA_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("C")->getState(), (Vector3d()<<0, Acc2/2, 0).finished(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("D")->getState(), (Vector3d()<<0, Acc2, 0).finished(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("L")->getState(), Vector3d::Zero(), 1e-5);
+    ASSERT_MATRIX_APPROX(KFB_->getStateBlock("B")->getState(), Vector3d::Zero(), 1e-5);
+}
 
 int main(int argc, char **argv)
 {