diff --git a/CMakeLists.txt b/CMakeLists.txt
index 58ca2e3ad9f835249db3f7711d72cf78c544245a..7a79ed9a306a55b18844fbfb3fb8e79dc75ceb15 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -107,21 +107,22 @@ SET(HDRS_COMMON
   )
 SET(HDRS_FACTOR
   include/${PROJECT_NAME}/factor/factor_compass_3d.h
-  include/${PROJECT_NAME}/factor/factor_imu.h
-  include/${PROJECT_NAME}/factor/factor_imu2d.h
-  include/${PROJECT_NAME}/factor/factor_imu2d_with_gravity.h
+  include/${PROJECT_NAME}/factor/factor_imu_3d.h
+  include/${PROJECT_NAME}/factor/factor_imu_2d.h
+  include/${PROJECT_NAME}/factor/factor_imu_2d_with_gravity.h
   )
 SET(HDRS_FEATURE
-  include/${PROJECT_NAME}/feature/feature_imu.h
-  include/${PROJECT_NAME}/feature/feature_imu2d.h
+  include/${PROJECT_NAME}/feature/feature_imu_3d.h
+  include/${PROJECT_NAME}/feature/feature_imu_2d.h
   )
 SET(HDRS_MATH
-  include/${PROJECT_NAME}/math/imu_tools.h
+  include/${PROJECT_NAME}/math/imu_3d_tools.h
+  include/${PROJECT_NAME}/math/imu_2d_tools.h
   )
 SET(HDRS_PROCESSOR
   include/${PROJECT_NAME}/processor/processor_compass.h
-  include/${PROJECT_NAME}/processor/processor_imu.h
-  include/${PROJECT_NAME}/processor/processor_imu2d.h
+  include/${PROJECT_NAME}/processor/processor_imu_3d.h
+  include/${PROJECT_NAME}/processor/processor_imu_2d.h
   )
 SET(HDRS_SENSOR
   include/${PROJECT_NAME}/sensor/sensor_compass.h
@@ -136,15 +137,15 @@ SET(SRCS_CAPTURE
   src/capture/capture_imu.cpp
   )
 SET(SRCS_FEATURE
-  src/feature/feature_imu.cpp
-  src/feature/feature_imu2d.cpp
+  src/feature/feature_imu_2d.cpp
+  src/feature/feature_imu_3d.cpp
   )
 SET(SRCS_PROCESSOR
   src/processor/processor_compass.cpp
-  src/processor/processor_imu.cpp
-  src/processor/processor_imu2d.cpp
-  test/processor_imu_tester.cpp
-  test/processor_imu2d_tester.cpp
+  src/processor/processor_imu_2d.cpp
+  src/processor/processor_imu_3d.cpp
+  test/processor_imu_3d_tester.cpp
+  test/processor_imu_2d_tester.cpp
   )
 SET(SRCS_SENSOR
   src/sensor/sensor_compass.cpp
diff --git a/demos/demo_factor_imu.cpp b/demos/demo_factor_imu_3d.cpp
similarity index 99%
rename from demos/demo_factor_imu.cpp
rename to demos/demo_factor_imu_3d.cpp
index cde7848fc2ad375e5f6fcabae94365679a765a43..5896ff0066c4407519d1d0abfe938bfb18c68ddb 100644
--- a/demos/demo_factor_imu.cpp
+++ b/demos/demo_factor_imu_3d.cpp
@@ -22,7 +22,7 @@
 #include <core/ceres_wrapper/solver_ceres.h>
 
 #include "core/capture/capture_imu.h"
-#include "core/processor/processor_imu.h"
+#include "core/processor/processor_imu_3d.h"
 #include "core/sensor/sensor_imu.h"
 #include "core/capture/capture_pose.h"
 #include "core/common/wolf.h"
diff --git a/demos/demo_imuDock.cpp b/demos/demo_imuDock.cpp
index 0e9411ba5db7f28b81c501bcd874b2f8ce8f3216..0a4d9646f87d2e8901aaf2b6db319956ac69ea1f 100644
--- a/demos/demo_imuDock.cpp
+++ b/demos/demo_imuDock.cpp
@@ -20,7 +20,7 @@
 
 #include <core/ceres_wrapper/solver_ceres.h>
 
-#include "core/processor/processor_imu.h"
+#include "core/processor/processor_imu_3d.h"
 #include "core/sensor/sensor_imu.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
diff --git a/demos/demo_imuDock_autoKFs.cpp b/demos/demo_imuDock_autoKFs.cpp
index 72da12854aaa4de36e727d5ce0fd12c9d719fdb1..6b4a5240cc602a5636f241509cc66d4d478a5f93 100644
--- a/demos/demo_imuDock_autoKFs.cpp
+++ b/demos/demo_imuDock_autoKFs.cpp
@@ -20,7 +20,7 @@
 
 #include <core/ceres_wrapper/solver_ceres.h>
 
-#include "core/processor/processor_imu.h"
+#include "core/processor/processor_imu_3d.h"
 #include "core/sensor/sensor_imu.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
diff --git a/demos/demo_imuPlateform_Offline.cpp b/demos/demo_imuPlateform_Offline.cpp
index 9c324c8793349a816ae2c6727e0f63106598d955..fe13bf27c75f0ffcddd282f9e992e95f13fbc3a0 100644
--- a/demos/demo_imuPlateform_Offline.cpp
+++ b/demos/demo_imuPlateform_Offline.cpp
@@ -22,7 +22,7 @@
 #include <core/ceres_wrapper/solver_ceres.h>
 
 #include "core/capture/capture_imu.h"
-#include "core/processor/processor_imu.h"
+#include "core/processor/processor_imu_3d.h"
 #include "core/sensor/sensor_imu.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
diff --git a/demos/demo_imu_constrained0.cpp b/demos/demo_imu_constrained0.cpp
index d7f82f6bdac0be5a69a6523fa5c613df617dd1ea..3c00b52f40f780301c479e9e051ac8d4b36e7e59 100644
--- a/demos/demo_imu_constrained0.cpp
+++ b/demos/demo_imu_constrained0.cpp
@@ -22,7 +22,7 @@
 #include <core/ceres_wrapper/solver_ceres.h>
 
 #include "core/capture/capture_imu.h"
-#include "core/processor/processor_imu.h"
+#include "core/processor/processor_imu_3d.h"
 #include "core/sensor/sensor_imu.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
diff --git a/demos/demo_processor_imu.cpp b/demos/demo_processor_imu.cpp
index 67d202cb9e6a9d884edee97a7f0cd705f3468193..8f372c0d05ecae859d44670f2153a5ec574fc6e4 100644
--- a/demos/demo_processor_imu.cpp
+++ b/demos/demo_processor_imu.cpp
@@ -20,7 +20,7 @@
 
 //Wolf
 #include "core/capture/capture_imu.h"
-#include "core/processor/processor_imu.h"
+#include "core/processor/processor_imu_3d.h"
 #include "core/sensor/sensor_imu.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
diff --git a/demos/demo_processor_imu_jacobians.cpp b/demos/demo_processor_imu_jacobians.cpp
index faacf2b3f8d7aa71e5d98be05cfbcc76fcf154df..a23a4cbdc87eaf57d3c3ff12fcceb1eb0efd54e9 100644
--- a/demos/demo_processor_imu_jacobians.cpp
+++ b/demos/demo_processor_imu_jacobians.cpp
@@ -66,7 +66,7 @@ int main(int argc, char** argv)
 
     //CaptureIMU* imu_ptr;
 
-    ProcessorImuTester processor_imu;
+    ProcessorImu3dTester processor_imu;
     //processor_imu.setOrigin(x0, t);
     double ddelta_bias = 0.00000001;
     double dt = 0.001;
diff --git a/include/imu/capture/capture_imu.h b/include/imu/capture/capture_imu.h
index 325f0e84c5ef4540391ae492e8097310b00ec169..19b4cd96677b570c856434a6dc01ea073e5ea3da 100644
--- a/include/imu/capture/capture_imu.h
+++ b/include/imu/capture/capture_imu.h
@@ -22,7 +22,7 @@
 
 //Wolf includes
 #include "imu/common/imu.h"
-#include "imu/math/imu_tools.h"
+#include "imu/math/imu_3d_tools.h"
 #include <core/capture/capture_motion.h>
 
 namespace wolf {
diff --git a/include/imu/factor/factor_fix_bias.h b/include/imu/factor/factor_fix_bias.h
index a35bc95faee34d910ef40bc1c5b3cbcb524f851b..06144c36f7a01d40b6a7bbc8eacc6ef2da96372b 100644
--- a/include/imu/factor/factor_fix_bias.h
+++ b/include/imu/factor/factor_fix_bias.h
@@ -23,7 +23,7 @@
 // Wolf includes
 #include "imu/common/imu.h"
 #include "imu/capture/capture_imu.h"
-#include "imu/feature/feature_imu.h"
+#include "imu/feature/feature_imu_3d.h"
 #include <core/factor/factor_autodiff.h>
 #include <core/frame/frame_base.h>
 #include <core/math/rotations.h>
diff --git a/include/imu/factor/factor_imu2d.h b/include/imu/factor/factor_imu_2d.h
similarity index 98%
rename from include/imu/factor/factor_imu2d.h
rename to include/imu/factor/factor_imu_2d.h
index d61ffb9efc4d226dd42d82569cdbc2e9744b2ed2..365732bd7c2ba490727ee2e7f6d8f53fb8ae4c73 100644
--- a/include/imu/factor/factor_imu2d.h
+++ b/include/imu/factor/factor_imu_2d.h
@@ -22,8 +22,8 @@
 
 // Wolf includes
 #include "imu/common/imu.h"
-#include "imu/feature/feature_imu2d.h"
-#include "imu/math/imu2d_tools.h"
+#include "imu/feature/feature_imu_2d.h"
+#include "imu/math/imu_2d_tools.h"
 #include <core/factor/factor_autodiff.h>
 #include <core/math/rotations.h>
 
diff --git a/include/imu/factor/factor_imu2d_with_gravity.h b/include/imu/factor/factor_imu_2d_with_gravity.h
similarity index 98%
rename from include/imu/factor/factor_imu2d_with_gravity.h
rename to include/imu/factor/factor_imu_2d_with_gravity.h
index e206b72aa7b4ce8019ff7d5ccd962a80d01a9216..d5ea25c187f5c2a6b49977570b799994e41c6cee 100644
--- a/include/imu/factor/factor_imu2d_with_gravity.h
+++ b/include/imu/factor/factor_imu_2d_with_gravity.h
@@ -22,8 +22,8 @@
 
 // Wolf includes
 #include "imu/common/imu.h"
-#include "imu/feature/feature_imu2d.h"
-#include "imu/math/imu2d_tools.h"
+#include "imu/feature/feature_imu_2d.h"
+#include "imu/math/imu_2d_tools.h"
 #include <core/factor/factor_autodiff.h>
 #include <core/math/rotations.h>
 
@@ -102,11 +102,11 @@ inline FactorImu2dWithGravity::FactorImu2dWithGravity(const FeatureImu2dPtr&  _f
           _ftr_ptr->getMeasurement(),
           _ftr_ptr->getMeasurementSquareRootInformationUpper(),
           _cap_origin_ptr->getSensor()->isStateBlockDynamic('I') and
-                  _cap_origin_ptr->getSensor()->isStateBlockDynamic('F')
+                  _cap_origin_ptr->getSensor()->isStateBlockDynamic('G')
               // b&g dynamic
               ? NodeStateBlocksPtrList{_cap_origin_ptr->getFrame(), _ftr_ptr->getFrame(), _cap_origin_ptr}
               : (not _cap_origin_ptr->getSensor()->isStateBlockDynamic('I') and
-                         not _cap_origin_ptr->getSensor()->isStateBlockDynamic('F')
+                         not _cap_origin_ptr->getSensor()->isStateBlockDynamic('G')
                      // b&g static
                      ? NodeStateBlocksPtrList{_cap_origin_ptr->getFrame(),
                                               _ftr_ptr->getFrame(),
diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu_3d.h
similarity index 96%
rename from include/imu/factor/factor_imu.h
rename to include/imu/factor/factor_imu_3d.h
index 9f4cd187d231ae9c4e16a5b97f11aa4f306098f0..ace5dc609f0465d1e61ca070cffd33fe4b66f6d2 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu_3d.h
@@ -21,7 +21,7 @@
 
 // Wolf includes
 #include "imu/common/imu.h"
-#include "imu/feature/feature_imu.h"
+#include "imu/feature/feature_imu_3d.h"
 #include "imu/sensor/sensor_imu.h"
 #include <core/factor/factor_autodiff.h>
 #include <core/math/rotations.h>
@@ -30,19 +30,19 @@
 
 namespace wolf
 {
-WOLF_PTR_TYPEDEFS(FactorImu);
+WOLF_PTR_TYPEDEFS(FactorImu3d);
 
 // class
-class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
+class FactorImu3d : public FactorAutodiff<FactorImu3d, 9, 3, 4, 3, 6, 3, 4, 3>
 {
   public:
-    FactorImu(const FeatureImuPtr    &_ftr_ptr,
+    FactorImu3d(const FeatureImu3dPtr    &_ftr_ptr,
               const CaptureImuPtr    &_capture_origin_ptr,
               const ProcessorBasePtr &_processor_ptr,
               bool                    _apply_loss_function,
               FactorStatus            _status = FAC_ACTIVE);
 
-    ~FactorImu() override = default;
+    ~FactorImu3d() override = default;
 
     /** \brief : compute the residual from the state blocks being iterated by the
        solver.
@@ -147,13 +147,13 @@ class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
 
 ///////////////////// IMPLEMENTAITON ////////////////////////////
 
-inline FactorImu::FactorImu(const FeatureImuPtr    &_ftr_ptr,
+inline FactorImu3d::FactorImu3d(const FeatureImu3dPtr    &_ftr_ptr,
                             const CaptureImuPtr    &_cap_origin_ptr,
                             const ProcessorBasePtr &_processor_ptr,
                             bool                    _apply_loss_function,
                             FactorStatus            _status)
-    : FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>(  // ...
-          "FactorImu",
+    : FactorAutodiff<FactorImu3d, 9, 3, 4, 3, 6, 3, 4, 3>(  // ...
+          "FactorImu3d",
           TOP_MOTION,
           _ftr_ptr->getMeasurement(),
           _ftr_ptr->getMeasurementSquareRootInformationUpper(),
@@ -188,7 +188,7 @@ inline FactorImu::FactorImu(const FeatureImuPtr    &_ftr_ptr,
 }
 
 template <typename T>
-inline bool FactorImu::operator()(const T *const _p1,
+inline bool FactorImu3d::operator()(const T *const _p1,
                                   const T *const _q1,
                                   const T *const _v1,
                                   const T *const _b1,
@@ -218,7 +218,7 @@ inline bool FactorImu::operator()(const T *const _p1,
 }
 
 template <typename D1, typename D2, typename D3>
-inline bool FactorImu::residual(const Eigen::MatrixBase<D1>     &_p1,
+inline bool FactorImu3d::residual(const Eigen::MatrixBase<D1>     &_p1,
                                 const Eigen::QuaternionBase<D2> &_q1,
                                 const Eigen::MatrixBase<D1>     &_v1,
                                 const Eigen::MatrixBase<D1>     &_ab1,
@@ -236,7 +236,7 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1>     &_p1,
      * 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:
+     *  We use the following functions from imu_3d_tools.h:
      *    D  = betweenStates(x1,x2,dt) : obtain a Delta from two states separated
      * dt=t2-t1 D2 = plus(D1,T)              : blocl-plus operator,  D2 = D1 <+> T
      *    T  = diff(D1,D2)             : block-minus operator, T  = D2 <-> D1
@@ -427,18 +427,18 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1>     &_p1,
     return true;
 }
 
-inline Eigen::VectorXd FactorImu::expectation() const
+inline Eigen::VectorXd FactorImu3d::expectation() const
 {
     // There are only 2 factored frames: origin and current
     auto frm_previous = getFramesFactored().front().lock();
     auto frm_current  = getFramesFactored().back().lock();
 
-    // get information on current_frame in the FactorImu
+    // get information on current_frame in the FactorImu3d
     const Eigen::Vector3d    frame_current_pos = (frm_current->getP()->getState());
     const Eigen::Quaterniond frame_current_ori(frm_current->getO()->getState().data());
     const Eigen::Vector3d    frame_current_vel = (frm_current->getV()->getState());
 
-    // get info on previous frame in the FactorImu
+    // get info on previous frame in the FactorImu3d
     const Eigen::Vector3d    frame_previous_pos = (frm_previous->getP()->getState());
     const Eigen::Quaterniond frame_previous_ori(frm_previous->getO()->getState().data());
     const Eigen::Vector3d    frame_previous_vel = (frm_previous->getV()->getState());
@@ -464,7 +464,7 @@ inline Eigen::VectorXd FactorImu::expectation() const
 }
 
 template <typename D1, typename D2, typename D3, typename D4>
-inline void FactorImu::expectation(const Eigen::MatrixBase<D1>     &_p1,
+inline void FactorImu3d::expectation(const Eigen::MatrixBase<D1>     &_p1,
                                    const Eigen::QuaternionBase<D2> &_q1,
                                    const Eigen::MatrixBase<D1>     &_v1,
                                    const Eigen::MatrixBase<D1>     &_p2,
diff --git a/include/imu/feature/feature_imu2d.h b/include/imu/feature/feature_imu_2d.h
similarity index 100%
rename from include/imu/feature/feature_imu2d.h
rename to include/imu/feature/feature_imu_2d.h
diff --git a/include/imu/feature/feature_imu.h b/include/imu/feature/feature_imu_3d.h
similarity index 85%
rename from include/imu/feature/feature_imu.h
rename to include/imu/feature/feature_imu_3d.h
index 6a01399971f1ea2a52aca52114ade6ffe85d1032..621e05949b00b421b22732fc22616334ff27037a 100644
--- a/include/imu/feature/feature_imu.h
+++ b/include/imu/feature/feature_imu_3d.h
@@ -32,9 +32,9 @@ namespace wolf
 {
 
 // WOLF_PTR_TYPEDEFS(CaptureImu);
-WOLF_PTR_TYPEDEFS(FeatureImu);
+WOLF_PTR_TYPEDEFS(FeatureImu3d);
 
-class FeatureImu : public FeatureBase
+class FeatureImu3d : public FeatureBase
 {
   public:
     /** \brief Constructor from and measures
@@ -46,7 +46,7 @@ class FeatureImu : public FeatureBase
      * \param gyro_bias gyroscope bias of origin frame
      * \param _cap_imu_ptr pointer to parent CaptureMotion
      */
-    FeatureImu(const Eigen::VectorXd&             _delta_preintegrated,
+    FeatureImu3d(const Eigen::VectorXd&             _delta_preintegrated,
                const Eigen::MatrixXd&             _delta_preintegrated_covariance,
                const Eigen::Vector6d&             _bias,
                const Eigen::Matrix<double, 9, 6>& _dD_db_jacobians,
@@ -56,9 +56,9 @@ class FeatureImu : public FeatureBase
      *
      * \param _cap_imu_ptr pointer to parent CaptureMotion
      */
-    FeatureImu(CaptureMotionPtr _cap_imu_ptr);
+    FeatureImu3d(CaptureMotionPtr _cap_imu_ptr);
 
-    ~FeatureImu() override;
+    ~FeatureImu3d() override;
 
     const Eigen::Vector3d&             getAccBiasPreint() const;
     const Eigen::Vector3d&             getGyroBiasPreint() const;
@@ -75,17 +75,17 @@ class FeatureImu : public FeatureBase
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 };
 
-inline const Eigen::Vector3d& FeatureImu::getAccBiasPreint() const
+inline const Eigen::Vector3d& FeatureImu3d::getAccBiasPreint() const
 {
     return acc_bias_preint_;
 }
 
-inline const Eigen::Vector3d& FeatureImu::getGyroBiasPreint() const
+inline const Eigen::Vector3d& FeatureImu3d::getGyroBiasPreint() const
 {
     return gyro_bias_preint_;
 }
 
-inline const Eigen::Matrix<double, 9, 6>& FeatureImu::getJacobianBias() const
+inline const Eigen::Matrix<double, 9, 6>& FeatureImu3d::getJacobianBias() const
 {
     return jacobian_bias_;
 }
diff --git a/include/imu/math/imu2d_tools.h b/include/imu/math/imu_2d_tools.h
similarity index 99%
rename from include/imu/math/imu2d_tools.h
rename to include/imu/math/imu_2d_tools.h
index d4733c902ecdff831783a5a8fa457291551b19d6..db832b39dcabcfdd8eff74e722267cfc5181c0a8 100644
--- a/include/imu/math/imu2d_tools.h
+++ b/include/imu/math/imu_2d_tools.h
@@ -30,7 +30,7 @@
 #include <cstdio>
 
 /*
- * Most functions in this file are the 2d versions of the functions in imu_tools.h
+ * Most functions in this file are the 2d versions of the functions in imu_3d_tools.h
  * They relate manipulations of Delta motion magnitudes used for Imu pre-integration.
  *
  * The Delta is defined as
diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_3d_tools.h
similarity index 100%
rename from include/imu/math/imu_tools.h
rename to include/imu/math/imu_3d_tools.h
diff --git a/include/imu/processor/processor_imu2d.h b/include/imu/processor/processor_imu_2d.h
similarity index 98%
rename from include/imu/processor/processor_imu2d.h
rename to include/imu/processor/processor_imu_2d.h
index 3f466dce71f34073598446426a0845bc7d3e8a2c..e6a14c16c00028f314cba6694cfb12bc477ac806 100644
--- a/include/imu/processor/processor_imu2d.h
+++ b/include/imu/processor/processor_imu_2d.h
@@ -24,7 +24,7 @@
 #include "imu/common/imu.h"
 #include "imu/sensor/sensor_imu.h"
 #include "imu/capture/capture_imu.h"
-#include "imu/feature/feature_imu.h"
+#include "imu/feature/feature_imu_2d.h"
 #include <core/processor/processor_motion.h>
 
 namespace wolf
@@ -82,7 +82,7 @@ class ProcessorImu2d : public ProcessorMotion
     virtual void     emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
 
   protected:
-    SensorImu2dPtr          sensor_imu2d_;
+    SensorImu2dPtr          sensor_imu_2d_;
     Matrix3d                imu_drift_cov_;
 };
 
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu_3d.h
similarity index 94%
rename from include/imu/processor/processor_imu.h
rename to include/imu/processor/processor_imu_3d.h
index f565acb9af4f8437f067fd5fa212c239e0f6103e..74a06972d50ec7b70c3559387fdfdbafbc08cf05 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu_3d.h
@@ -24,7 +24,7 @@
 #include "imu/common/imu.h"
 #include "imu/sensor/sensor_imu.h"
 #include "imu/capture/capture_imu.h"
-#include "imu/feature/feature_imu.h"
+#include "imu/feature/feature_imu_3d.h"
 
 // Wolf core
 #include <core/processor/processor_motion.h>
@@ -32,7 +32,7 @@
 namespace wolf
 {
 
-WOLF_PTR_TYPEDEFS(ProcessorImu);
+WOLF_PTR_TYPEDEFS(ProcessorImu3d);
 
 enum BootstrapMethod
 {
@@ -42,12 +42,12 @@ enum BootstrapMethod
 };
 
 // class
-class ProcessorImu : public ProcessorMotion
+class ProcessorImu3d : public ProcessorMotion
 {
   public:
-    ProcessorImu(const YAML::Node& _params);
-    ~ProcessorImu() override;
-    WOLF_PROCESSOR_CREATE(ProcessorImu);
+    ProcessorImu3d(const YAML::Node& _params);
+    ~ProcessorImu3d() override;
+    WOLF_PROCESSOR_CREATE(ProcessorImu3d);
     void configure(SensorBasePtr _sensor) override;
 
     void preProcess() override;
@@ -130,7 +130,7 @@ class ProcessorImu : public ProcessorMotion
 namespace wolf
 {
 
-inline Eigen::VectorXd ProcessorImu::deltaZero() const
+inline Eigen::VectorXd ProcessorImu3d::deltaZero() const
 {
     return (Eigen::VectorXd(10) << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0).finished();  // p, q, v
 }
diff --git a/include/imu/sensor/sensor_compass.h b/include/imu/sensor/sensor_compass.h
index 2475df34a91e9358eff40906efbb6e78f9a6eb00..4478e8ac073213d6522149e633091fb14a98a991 100644
--- a/include/imu/sensor/sensor_compass.h
+++ b/include/imu/sensor/sensor_compass.h
@@ -44,6 +44,11 @@ class SensorCompass : public SensorBase
     ~SensorCompass() override;
 
     Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
+
+    double getStdNoise() const
+    {
+      return stdev_noise_;
+    }
 };
 
 } /* namespace wolf */
\ No newline at end of file
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index 0f864ae0e840eeb2e806cf52da1fbfa5a095fec0..8eeed7084b6128698602ef6e2206d028b453e602 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/include/imu/sensor/sensor_imu.h
@@ -31,7 +31,7 @@ template <unsigned int DIM>
 class SensorImu : public SensorBase
 {
   protected:
-    double w_noise_;  ///< standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
+    double w_noise_;  ///< standard deviation of Gyroscope noise (same for all the axis) in rad/sec/sqrt(s)
     double a_noise_;  ///< standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
 
     // Is the 2D plane orthogonal to gravity? (only for 2D cases)
@@ -40,10 +40,17 @@ class SensorImu : public SensorBase
   public:
     SensorImu(const YAML::Node& _params) : SensorBase("SensorImu" + toString(DIM) + "d", DIM, _params)
     {
+        static_assert(DIM == 2 or DIM == 3);
+
         w_noise_ = _params["w_noise"].as<double>();
         a_noise_ = _params["a_noise"].as<double>();
 
-        if (_params["orthogonal_gravity"]) orthogonal_gravity_ = _params["orthogonal_gravity"].as<bool>();
+        if (DIM == 2)
+        {
+            orthogonal_gravity_ = _params["orthogonal_gravity"].as<bool>();
+
+            if (orthogonal_gravity_) removeStateBlock('G');
+        }
     };
 
     WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorImu, DIM);
diff --git a/schema/processor/ProcessorImu.schema b/schema/processor/ProcessorImu3d.schema
similarity index 78%
rename from schema/processor/ProcessorImu.schema
rename to schema/processor/ProcessorImu3d.schema
index e2a89a2fbb6818beaf97f0a7859d3a68742b09e0..7fde31268239e7aba98aba319474f762962ae374 100644
--- a/schema/processor/ProcessorImu.schema
+++ b/schema/processor/ProcessorImu3d.schema
@@ -1,16 +1,16 @@
 follow: ProcessorMotion.schema
 bootstrap:
-  enable:
+  enabled:
     _mandatory: true
     _type: bool
     _doc: If any boostrap mechanism is enabled for the processor.
   method:
-    _mandatory: $enable
+    _mandatory: $enabled
     _type: string
     _options: ["STATIC", "G", "V0_G"]
     _doc: "Boostrap method (only if enable==true)"
   averaging_length:
-    _mandatory: "$enable and (method=='STATIC' or method=='G')"
+    _mandatory: $enabled
     _type: double
     _doc: Averaging length used by the bootstrap mechanism.
 
diff --git a/schema/sensor/SensorImu2d.schema b/schema/sensor/SensorImu2d.schema
index 111c038d5f37df848cd2a1a635d6afa8b8612016..2dc3995e20565685b47ce1173cc71c50ab8947a8 100644
--- a/schema/sensor/SensorImu2d.schema
+++ b/schema/sensor/SensorImu2d.schema
@@ -10,46 +10,53 @@ a_noise:
   _type: double
   _doc: standard deviation of the accelerometer measurements (square root of the covariance matrix diagonal).
 
+orthogonal_gravity:
+  _mandatory: true
+  _type: bool
+  _doc: If the gravity is orthogonal to Z axis (i.e. it is measured by accelerometers X and Y)
+
 states:
   keys:
     _mandatory: false
     _type: string
-    _value: POI
-    _doc: "State keys: PO extrinsics, I for biases"
+    _value: POIG
+    _doc: "State keys: PO extrinsics, I for biases, G for gravity projection to X and Y axis"
   P:
     follow: SpecStateSensorP2d.schema
     state:
+      _mandatory: false
       _type: Vector2d
       _value: [0,0]
       _doc: For the moment, IMU only implemented in the origin.
   O:
     follow: SpecStateSensorO2d.schema
     state:
+      _mandatory: false
       _type: Vector1d
       _value: [0]
       _doc: For the moment, IMU only implemented in the origin.
   I:
-    follow: SpecStateSensor.schema
-    type:
-      _type: string
-      _mandatory: false
-      _value: StateParams3
-      _doc: The derived type of the StateBlock for IMU biases
+    follow: SpecStateSensorParams3.schema
     state:
-      _type: Vector3d
       _mandatory: false
-      _default: [0,0,0]
-      _doc: The initial values of the IMU biases
+      _type: Vector3d
+      _default: [0, 0, 0]
+      _doc: State default initial value.
+  G: 
+    follow: SpecStateSensorParams2.schema
     dynamic:
+      _mandatory: false
       _type: bool
-      _mandatory: true
-      _doc: If the bias are dynamic, i.e. they change along time.
-    noise_std:
-      _type: Vector3d
+      _default: false
+      _doc: The projection of G over XY map axis is assumed static by default (i.e. constant slope).
+    state:
       _mandatory: false
-      _doc: (only if mode==factor) A vector containing the stdev values of the noise of the prior factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
-    drift_std:
-      _type: Vector3d
+      _type: Vector2d
+      _default: [0, 0]
+      _doc: State default initial value.
+    mode:
+      _type: string
       _mandatory: false
-      _doc: (only if dynamic==true) A vector containing the stdev values of the noise of the drift factor , i.e. the sqrt of the diagonal elements of the covariance matrix.
-
+      _default: initial_guess
+      _options: ["fix", "factor", "initial_guess"]
+      _doc: The prior mode can be 'factor' to add an absolute factor (requires 'noise_std'), 'fix' to set the values constant or 'initial_guess' to just set the values
diff --git a/schema/sensor/SensorImu3d.schema b/schema/sensor/SensorImu3d.schema
index 2599ac37541b21bb0fa21c80af64302e8f962dda..d31c0dfe02fba3d4bee1b2d3bf9cb2c030c95ed5 100644
--- a/schema/sensor/SensorImu3d.schema
+++ b/schema/sensor/SensorImu3d.schema
@@ -19,12 +19,14 @@ states:
   P:
     follow: SpecStateSensorP3d.schema
     state:
+      _mandatory: false
       _type: Vector3d
       _value: [0,0,0]
       _doc: For the moment, IMU only implemented in the origin.
   O:
     follow: SpecStateSensorO3d.schema
     state:
+      _mandatory: false
       _type: Vector4d
       _value: [0,0,0,1]
       _doc: For the moment, IMU only implemented in the origin.
diff --git a/src/capture/capture_imu.cpp b/src/capture/capture_imu.cpp
index eabab021f926c2830bd90627983aee4ea7196d36..60aab0e146b85dc9abaf2a138049169c04537c92 100644
--- a/src/capture/capture_imu.cpp
+++ b/src/capture/capture_imu.cpp
@@ -71,7 +71,7 @@ CaptureImu::CaptureImu(const TimeStamp&       _init_ts,
               : nullptr)
 {
     assert((_bias.size() == 3) or (_bias.size() == 6));
-    WOLF_WARN_COND(_sensor_ptr->isStateBlockDynamic('I'), "Sensor bias was provided but bias is static in sensor. Bias discarded.");
+    WOLF_WARN_COND(not _sensor_ptr->isStateBlockDynamic('I'), "Sensor bias was provided but bias is static in sensor. Bias discarded.");
 }
 
 CaptureImu::~CaptureImu()
diff --git a/src/feature/feature_imu2d.cpp b/src/feature/feature_imu_2d.cpp
similarity index 97%
rename from src/feature/feature_imu2d.cpp
rename to src/feature/feature_imu_2d.cpp
index ef19eef6842e16e7795cb3916ea41c534f66547e..29ac6b8bf4bc578a705d062fcd3e7127ba79e6f3 100644
--- a/src/feature/feature_imu2d.cpp
+++ b/src/feature/feature_imu_2d.cpp
@@ -18,7 +18,7 @@
 // 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/>.
 
-#include "imu/feature/feature_imu2d.h"
+#include "imu/feature/feature_imu_2d.h"
 
 namespace wolf {
 
diff --git a/src/feature/feature_imu.cpp b/src/feature/feature_imu_3d.cpp
similarity index 80%
rename from src/feature/feature_imu.cpp
rename to src/feature/feature_imu_3d.cpp
index 71fa2db99ecd6096dfcc96efbf29d312c0d569cc..1232cbe6ff00117d2896ebeca5cde84140268d74 100644
--- a/src/feature/feature_imu.cpp
+++ b/src/feature/feature_imu_3d.cpp
@@ -18,31 +18,31 @@
 // 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/>.
 
-#include "imu/feature/feature_imu.h"
+#include "imu/feature/feature_imu_3d.h"
 
 namespace wolf {
 
-FeatureImu::FeatureImu(const Eigen::VectorXd& _delta_preintegrated,
+FeatureImu3d::FeatureImu3d(const Eigen::VectorXd& _delta_preintegrated,
                        const Eigen::MatrixXd& _delta_preintegrated_covariance,
                        const Eigen::Vector6d& _bias,
                        const Eigen::Matrix<double,9,6>& _dD_db_jacobians,
                        CaptureMotionPtr _cap_imu_ptr) :
-    FeatureBase("FeatureImu", _delta_preintegrated, _delta_preintegrated_covariance),
+    FeatureBase("FeatureImu3d", _delta_preintegrated, _delta_preintegrated_covariance),
     acc_bias_preint_(_bias.head<3>()),
     gyro_bias_preint_(_bias.tail<3>()),
     jacobian_bias_(_dD_db_jacobians)
 {
 }
 
-FeatureImu::FeatureImu(CaptureMotionPtr _cap_imu_ptr):
-        FeatureBase("FeatureImu", _cap_imu_ptr->getDeltaPreint(), _cap_imu_ptr->getDeltaPreintCov()),
+FeatureImu3d::FeatureImu3d(CaptureMotionPtr _cap_imu_ptr):
+        FeatureBase("FeatureImu3d", _cap_imu_ptr->getDeltaPreint(), _cap_imu_ptr->getDeltaPreintCov()),
         acc_bias_preint_ (_cap_imu_ptr->getCalibrationPreint().head<3>()),
         gyro_bias_preint_(_cap_imu_ptr->getCalibrationPreint().tail<3>()),
         jacobian_bias_(_cap_imu_ptr->getJacobianCalib())
 {
 }
 
-FeatureImu::~FeatureImu()
+FeatureImu3d::~FeatureImu3d()
 {
     //
 }
diff --git a/src/processor/processor_compass.cpp b/src/processor/processor_compass.cpp
index a6022a3faf6e266d58bb27dedf9d10f35f5889e5..ba38774a9abea9315ce063a964633c12c3718bbd 100644
--- a/src/processor/processor_compass.cpp
+++ b/src/processor/processor_compass.cpp
@@ -93,5 +93,6 @@ void ProcessorCompass::processMatch(FrameBasePtr _frame, CaptureBasePtr _capture
                                          applyLossFunction());
 }
 
+// Register in the FactoryProcessor
 WOLF_REGISTER_PROCESSOR(ProcessorCompass)
 }
diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu_2d.cpp
similarity index 97%
rename from src/processor/processor_imu2d.cpp
rename to src/processor/processor_imu_2d.cpp
index 883889f1ba5524878484a4a98c331742833c492a..9c1b4e29aafc906daf7bbb2d63f0e2a8f0552cf1 100644
--- a/src/processor/processor_imu2d.cpp
+++ b/src/processor/processor_imu_2d.cpp
@@ -19,11 +19,11 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 // imu
-#include "imu/processor/processor_imu2d.h"
+#include "imu/processor/processor_imu_2d.h"
 #include "imu/sensor/sensor_imu.h"
-#include "imu/factor/factor_imu2d.h"
-#include "imu/factor/factor_imu2d_with_gravity.h"
-#include "imu/math/imu2d_tools.h"
+#include "imu/factor/factor_imu_2d.h"
+#include "imu/factor/factor_imu_2d_with_gravity.h"
+#include "imu/math/imu_2d_tools.h"
 
 // wolf
 #include <core/composite/vector_composite.h>
@@ -285,7 +285,7 @@ void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
                    _dt,
                    _delta_preint_plus_delta,
                    _jacobian_delta_preint,
-                   _jacobian_delta);  // jacobians tested in imu2d_tools
+                   _jacobian_delta);  // jacobians tested in imu_2d_tools
 }
 
 Eigen::VectorXd ProcessorImu2d::correctDelta(const Eigen::VectorXd& delta_preint,
@@ -294,12 +294,6 @@ Eigen::VectorXd ProcessorImu2d::correctDelta(const Eigen::VectorXd& delta_preint
     return imu2d::plus(delta_preint, delta_step);
 }
 
-}  // namespace wolf
-
-// Register in the FactorySensor
-#include "core/processor/factory_processor.h"
-
-namespace wolf
-{
+// Register in the FactoryProcessor
 WOLF_REGISTER_PROCESSOR(ProcessorImu2d)
 }
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu_3d.cpp
similarity index 81%
rename from src/processor/processor_imu.cpp
rename to src/processor/processor_imu_3d.cpp
index 86d54557d36a17687cc716d26f5429c114a8d6a9..c123587ca68cf498b7cfc2857f71c1014048af06 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu_3d.cpp
@@ -19,9 +19,9 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 // imu
-#include "imu/processor/processor_imu.h"
-#include "imu/factor/factor_imu.h"
-#include "imu/math/imu_tools.h"
+#include "imu/processor/processor_imu_3d.h"
+#include "imu/factor/factor_imu_3d.h"
+#include "imu/math/imu_3d_tools.h"
 
 // wolf
 #include <core/composite/vector_composite.h>
@@ -30,8 +30,8 @@
 namespace wolf
 {
 
-ProcessorImu::ProcessorImu(const YAML::Node& _params)
-    : ProcessorMotion("ProcessorImu",
+ProcessorImu3d::ProcessorImu3d(const YAML::Node& _params)
+    : ProcessorMotion("ProcessorImu3d",
                       {{'P', "StatePoint3d"}, {'O', "StateQuaternion"}, {'V', "StateVector3d"}},
                       3,
                       10,
@@ -41,7 +41,8 @@ ProcessorImu::ProcessorImu(const YAML::Node& _params)
                       6,
                       _params)
 {
-    bootstrapping_ = _params["bootstrap"]["enable"].as<bool>();
+    WOLF_DEBUG("in ProcessorImu3d");
+    bootstrapping_ = _params["bootstrap"]["enabled"].as<bool>();
     bootstrap_factor_list_.clear();
 
     if (bootstrapping_)
@@ -65,18 +66,18 @@ ProcessorImu::ProcessorImu(const YAML::Node& _params)
     }
 }
 
-ProcessorImu::~ProcessorImu()
+ProcessorImu3d::~ProcessorImu3d()
 {
     //
 }
-void ProcessorImu::preProcess()
+void ProcessorImu3d::preProcess()
 {
     auto cap_ptr = std::dynamic_pointer_cast<CaptureImu>(incoming_ptr_);
     assert(
         cap_ptr != nullptr &&
         ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureImu").c_str());
 }
-bool ProcessorImu::voteForKeyFrame() const
+bool ProcessorImu3d::voteForKeyFrame() const
 {
     // time span
     if (getBuffer().back().ts_ - getBuffer().front().ts_ >= getMaxTimeSpan() - Constants::EPS)
@@ -101,14 +102,14 @@ bool ProcessorImu::voteForKeyFrame() const
     return false;
 }
 
-CaptureMotionPtr ProcessorImu::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)
+CaptureMotionPtr ProcessorImu3d::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)
 {
     auto cap_motion = std::static_pointer_cast<CaptureMotion>(
         CaptureBase::emplace<CaptureImu>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin));
@@ -118,7 +119,7 @@ CaptureMotionPtr ProcessorImu::emplaceCapture(const FrameBasePtr&   _frame_own,
     return cap_motion;
 }
 
-VectorXd ProcessorImu::getCalibration(const CaptureBaseConstPtr _capture) const
+VectorXd ProcessorImu3d::getCalibration(const CaptureBaseConstPtr _capture) const
 {
     if (_capture)
         return _capture->getStateBlock('I')->getState();
@@ -126,22 +127,22 @@ VectorXd ProcessorImu::getCalibration(const CaptureBaseConstPtr _capture) const
         return getSensor()->getStateBlockDynamic('I')->getState();
 }
 
-void ProcessorImu::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
+void ProcessorImu3d::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration)
 {
     _capture->getSensorIntrinsic()->setState(_calibration);
 }
 
-void ProcessorImu::configure(SensorBasePtr _sensor)
+void ProcessorImu3d::configure(SensorBasePtr _sensor)
 {
     imu_drift_cov_ = _sensor->getDriftCov('I');
 }
 
-void ProcessorImu::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
+void ProcessorImu3d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
 {
     // 1. Feature and factor Imu
     //---------------------------
 
-    auto ftr_imu = FeatureBase::emplace<FeatureImu>(
+    auto ftr_imu = FeatureBase::emplace<FeatureImu3d>(
         _capture_own,
         _capture_own->getBuffer().back().delta_integr_,
         _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
@@ -150,7 +151,7 @@ void ProcessorImu::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, Cap
 
     CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
 
-    auto fac_imu = FactorBase::emplace<FactorImu>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), applyLossFunction());
+    auto fac_imu = FactorBase::emplace<FactorImu3d>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), applyLossFunction());
 
     if (bootstrapping_)
     {
@@ -194,13 +195,13 @@ void ProcessorImu::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, Cap
     }
 }
 
-void ProcessorImu::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
+void ProcessorImu3d::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!");
 
@@ -226,7 +227,7 @@ void ProcessorImu::computeCurrentDelta(const Eigen::VectorXd& _data,
      */
 
     // create delta
-    imu::body2delta(_data - _calib, _dt, _delta, jac_delta_data);  // Jacobians tested in imu_tools
+    imu::body2delta(_data - _calib, _dt, _delta, jac_delta_data);  // Jacobians tested in imu_3d_tools
 
     // compute delta_cov
     _delta_cov = jac_delta_data * _data_cov * jac_delta_data.transpose();
@@ -235,10 +236,10 @@ void ProcessorImu::computeCurrentDelta(const Eigen::VectorXd& _data,
     _jac_delta_calib = -jac_delta_data;
 }
 
-void ProcessorImu::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
-                                  const Eigen::VectorXd& _delta,
-                                  const double           _dt,
-                                  Eigen::VectorXd&       _delta_preint_plus_delta) const
+void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+                                    const Eigen::VectorXd& _delta,
+                                    const double           _dt,
+                                    Eigen::VectorXd&       _delta_preint_plus_delta) const
 {
     /* MATHS according to Sola-16
      * Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2    = Dp + Dv*dt + Dq*dp   if  dp = 1/2*(a-a_b)*dt^2
@@ -250,10 +251,10 @@ void ProcessorImu::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
     _delta_preint_plus_delta = imu::compose(_delta_preint, _delta, _dt);
 }
 
-void ProcessorImu::statePlusDelta(const VectorComposite& _x,
-                                  const Eigen::VectorXd& _delta,
-                                  const double           _dt,
-                                  VectorComposite&       _x_plus_delta) const
+void ProcessorImu3d::statePlusDelta(const VectorComposite& _x,
+                                    const Eigen::VectorXd& _delta,
+                                    const double           _dt,
+                                    VectorComposite&       _x_plus_delta) const
 {
     assert(_delta.size() == 10 && "Wrong _delta vector size");
     assert(_dt >= 0 && "Time interval _dt is negative!");
@@ -269,12 +270,12 @@ void ProcessorImu::statePlusDelta(const VectorComposite& _x,
     _x_plus_delta = imu::composeOverState(_x, delta, _dt);
 }
 
-void ProcessorImu::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
+void ProcessorImu3d::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
 {
     /*
      * Expression of the delta integration step, D' = D (+) d:
@@ -304,16 +305,16 @@ void ProcessorImu::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
                  _dt,
                  _delta_preint_plus_delta,
                  _jacobian_delta_preint,
-                 _jacobian_delta);  // jacobians tested in imu_tools
+                 _jacobian_delta);  // jacobians tested in imu_3d_tools
 }
 
-Eigen::VectorXd ProcessorImu::correctDelta(const Eigen::VectorXd& delta_preint,
-                                           const Eigen::VectorXd& delta_step) const
+Eigen::VectorXd ProcessorImu3d::correctDelta(const Eigen::VectorXd& delta_preint,
+                                             const Eigen::VectorXd& delta_step) const
 {
     return imu::plus(delta_preint, delta_step);
 }
 
-void ProcessorImu::bootstrap()
+void ProcessorImu3d::bootstrap()
 {
     // TODO bootstrap strategies.
     // See Sola-22 "Imu bootstrap strategies" https://www.overleaf.com/project/629e276e7f68b0c2bfa469ac
@@ -442,12 +443,12 @@ void ProcessorImu::bootstrap()
     }
 }
 
-void ProcessorImu::bootstrapEnable(bool _bootstrap_enable)
+void ProcessorImu3d::bootstrapEnable(bool _bootstrap_enable)
 {
     bootstrapping_ = _bootstrap_enable;
 };
 
-CaptureBasePtr ProcessorImu::bootstrapOrigin() const
+CaptureBasePtr ProcessorImu3d::bootstrapOrigin() const
 {
     if (bootstrap_factor_list_.empty())
         return origin_ptr_;
@@ -456,7 +457,7 @@ CaptureBasePtr ProcessorImu::bootstrapOrigin() const
             ->getOriginCapture();
 }
 
-VectorXd ProcessorImu::bootstrapDelta() const
+VectorXd ProcessorImu3d::bootstrapDelta() const
 {
     // Compute total integrated delta during bootstrap period
     // first, integrate all deltas in previous factors
@@ -465,7 +466,7 @@ VectorXd ProcessorImu::bootstrapDelta() const
     for (const auto& fac : bootstrap_factor_list_)
     // here, we take advantage of the list of IMU factors to recover all deltas
     {
-        if (std::dynamic_pointer_cast<FactorImu>(fac) != nullptr)
+        if (std::dynamic_pointer_cast<FactorImu3d>(fac) != nullptr)
         {
             dt = fac->getCapture()->getTimeStamp() - fac->getCapturesFactored().front().lock()->getTimeStamp();
             const auto& delta = fac->getFeature()->getMeasurement();  // In FeatImu, delta = measurement
@@ -480,7 +481,7 @@ VectorXd ProcessorImu::bootstrapDelta() const
     return delta_int;
 }
 
-bool ProcessorImu::recomputeStates() const
+bool ProcessorImu3d::recomputeStates() const
 {
     const auto& mp = getProblem()->getMotionProviderMap();
     if (not mp.empty() and
@@ -490,7 +491,7 @@ bool ProcessorImu::recomputeStates() const
         WOLF_DEBUG("Recomputing IMU keyframe states...");
         for (const auto& fac : bootstrap_factor_list_)
         {
-            if (std::dynamic_pointer_cast<FactorImu>(fac) != nullptr)
+            if (std::dynamic_pointer_cast<FactorImu3d>(fac) != nullptr)
             {
                 const auto& ftr        = fac->getFeature();
                 const auto& cap        = std::static_pointer_cast<CaptureMotion>(ftr->getCapture());
@@ -510,12 +511,6 @@ bool ProcessorImu::recomputeStates() const
         return false;
 }
 
-}  // namespace wolf
-
 // Register in the FactoryProcessor
-#include "core/processor/factory_processor.h"
-
-namespace wolf
-{
-WOLF_REGISTER_PROCESSOR(ProcessorImu)
-}
+WOLF_REGISTER_PROCESSOR(ProcessorImu3d)
+}  // namespace wolf
diff --git a/src/sensor/sensor_compass.cpp b/src/sensor/sensor_compass.cpp
index 389cc01b50743165bbfed6cd6467c33e2ad8b8a6..c2bd3a2a973c1b5988f3bd83e5912f14397e79ae 100644
--- a/src/sensor/sensor_compass.cpp
+++ b/src/sensor/sensor_compass.cpp
@@ -37,5 +37,6 @@ Eigen::MatrixXd SensorCompass::computeNoiseCov(const Eigen::VectorXd& _data) con
     return Eigen::Vector3d::Constant(pow(stdev_noise_, 2)).asDiagonal();
 }
 
+// Register in the FactorySensor
 WOLF_REGISTER_SENSOR(SensorCompass);
 }  // namespace wolf
diff --git a/src/sensor/sensor_imu.cpp b/src/sensor/sensor_imu.cpp
index 3edd6210369a9f793773f91c05deecf6bcee6bfc..79a82a1150a19308f70cc039c2c3c5acf1ea2b8b 100644
--- a/src/sensor/sensor_imu.cpp
+++ b/src/sensor/sensor_imu.cpp
@@ -23,6 +23,7 @@
 namespace wolf
 {
 
+// Register in the FactorySensor
 WOLF_REGISTER_SENSOR(SensorImu2d);
 WOLF_REGISTER_SENSOR(SensorImu3d);
 
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index ad1848367592e3d19cec69be50e90b1910ee15d2..6ac75d86079d86af8c31b9a578f60d2ac6ef7ba0 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -11,38 +11,38 @@ wolf_add_gtest(gtest_example gtest_example.cpp)           #
 
 wolf_add_gtest(gtest_factor_compass_3d gtest_factor_compass_3d.cpp)
 
-# Has been excluded from tests for god knows how long, so thing is broken.
-# Maybe call an archeologist to fix this thing?
-# wolf_add_gtest(gtest_factor_imu gtest_factor_imu.cpp)
+wolf_add_gtest(gtest_factor_imu_2d_with_gravity gtest_factor_imu_2d_with_gravity.cpp)
 
-wolf_add_gtest(gtest_factor_imu2d gtest_factor_imu2d.cpp)
+wolf_add_gtest(gtest_factor_imu_2d gtest_factor_imu_2d.cpp)
 
-# wolf_add_gtest(gtest_factor_imu2d_with_gravity gtest_factor_imu2d_with_gravity.cpp)
+# Has been excluded from tests for god knows how long, so thing is broken.
+# Maybe call an archeologist to fix this thing?
+# wolf_add_gtest(gtest_factor_imu_3d gtest_factor_imu_3d.cpp)
 
-# wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
+wolf_add_gtest(gtest_feature_imu_3d gtest_feature_imu_3d.cpp)
 
-# wolf_add_gtest(gtest_imu_mocap_fusion gtest_imu_mocap_fusion.cpp)
+wolf_add_gtest(gtest_imu_2d_static_init gtest_imu_2d_static_init.cpp)
 
-# wolf_add_gtest(gtest_imu_static_init gtest_imu_static_init.cpp)
+# wolf_add_gtest(gtest_imu_2d_tools gtest_imu_2d_tools.cpp)
 
-# wolf_add_gtest(gtest_imu_tools gtest_imu_tools.cpp)
+# wolf_add_gtest(gtest_imu_3d_mocap_fusion gtest_imu_3d_mocap_fusion.cpp)
 
-# wolf_add_gtest(gtest_imu gtest_imu.cpp)
+# wolf_add_gtest(gtest_imu_3d_static_init gtest_imu_3d_static_init.cpp)
 
-# wolf_add_gtest(gtest_imu2d_static_init gtest_imu2d_static_init.cpp)
+# wolf_add_gtest(gtest_imu_3d_tools gtest_imu_3d_tools.cpp)
 
-# wolf_add_gtest(gtest_imu2d_tools gtest_imu2d_tools.cpp)
+# wolf_add_gtest(gtest_imu_3d gtest_imu_3d.cpp)
 
-# wolf_add_gtest(gtest_processor_imu_jacobians gtest_processor_imu_jacobians.cpp)
+# wolf_add_gtest(gtest_processor_imu_3d_jacobians gtest_processor_imu_3d_jacobians.cpp)
 
-# wolf_add_gtest(gtest_processor_imu gtest_processor_imu.cpp)
+# wolf_add_gtest(gtest_processor_imu_3d gtest_processor_imu_3d.cpp)
 
-# wolf_add_gtest(gtest_processor_imu2d_with_gravity gtest_processor_imu2d_with_gravity.cpp)
+# wolf_add_gtest(gtest_processor_imu_2d_with_gravity gtest_processor_imu_2d_with_gravity.cpp)
 
-# wolf_add_gtest(gtest_processor_imu2d gtest_processor_imu2d.cpp)
+# wolf_add_gtest(gtest_processor_imu_2d gtest_processor_imu_2d.cpp)
 
 # wolf_add_gtest(gtest_processor_motion_intrinsics_update gtest_processor_motion_intrinsics_update.cpp)
 
 # wolf_add_gtest(gtest_schema gtest_schema.cpp)
 
-# wolf_add_gtest(gtest_sensor_compass gtest_sensor_compass.cpp)
+wolf_add_gtest(gtest_sensor_compass gtest_sensor_compass.cpp)
diff --git a/test/gtest_factor_imu2d_with_gravity.cpp b/test/gtest_factor_imu2d_with_gravity.cpp
deleted file mode 100644
index 637e14d110c501542163e0d86fbc61f2684b56c5..0000000000000000000000000000000000000000
--- a/test/gtest_factor_imu2d_with_gravity.cpp
+++ /dev/null
@@ -1,638 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// 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: http://www.iri.upc.edu/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/>.
-
-#include <core/ceres_wrapper/solver_ceres.h>
-#include <core/utils/utils_gtest.h>
-
-#include "imu/factor/factor_imu2d_with_gravity.h"
-#include "imu/internal/config.h"
-#include "imu/math/imu2d_tools.h"
-#include "imu/sensor/sensor_imu.h"
-
-using namespace Eigen;
-using namespace wolf;
-
-std::string imu_dir         = _WOLF_IMU_CODE_DIR;
-std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
-int         N_TESTS         = 10;
-
-class FactorImu2dwithGravity_test : public testing::Test
-{
-  public:
-    Matrix6d       data_cov;
-    Matrix5d       delta_cov;
-    MatrixXd       jacobian_bias = MatrixXd(5, 3);
-    Vector3d       b0_preint;
-    ProblemPtr     problem_ptr;
-    SolverCeresPtr solver;
-    FrameBasePtr   frm0, frm1;
-    SensorBasePtr  sensor;
-    CaptureImuPtr  cap0, cap1;
-
-    virtual void SetUp()
-    {
-        // Input odometry data and covariance
-        data_cov  = 0.1 * Matrix6d::Identity();
-        delta_cov = 0.1 * Matrix5d::Identity();
-
-        // Jacobian of the bias
-        jacobian_bias << 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0;
-        // preintegration bias
-        b0_preint = Vector3d::Zero();
-
-        // Problem and solver
-        problem_ptr                                   = Problem::create("POV", 2);
-        solver                                        = std::make_shared<SolverCeres>(problem_ptr);
-        solver->getSolverOptions().function_tolerance = 1e-9;
-        solver->getSolverOptions().gradient_tolerance = 1e-9;
-
-        // Two frames
-        frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "POV", Vector5d::Zero());
-        frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero());
-
-        // Imu2d sensor
-        sensor = problem_ptr->installSensor(imu_dir + "/test/yaml/sensor_imu2d_with_gravity.yaml", {imu_dir});
-
-        // capture from frm1 to frm0
-        cap0 =
-            CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
-        cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
-    }
-
-    virtual void TearDown() {}
-};
-
-TEST_F(FactorImu2dwithGravity_test, check_tree)
-{
-    ASSERT_TRUE(problem_ptr->check(0));
-}
-
-TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_f1)
-{
-    for (int i = 0; i < N_TESTS; i++)
-    {
-        // WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
-        // Random delta
-        Vector5d delta = Vector5d::Random() * 10;  // delta *= 0;
-        delta(2)       = pi2pi(delta(2));
-
-        // Random initial pose
-        Vector5d x0 = Vector5d::Random() * 10;  // x0 *= 0;
-        x0(2)       = pi2pi(x0(2));
-
-        // Random gravity
-        Vector2d g = Vector2d::Random() * 5;
-        // Vector2d g = Vector2d::Zero();
-
-        // x1 groundtruth
-        Vector5d x1;
-        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
-        // Set states
-        frm0->setState(x0, "POV");
-        frm1->setState(x1, "POV");
-        sensor->getStateBlock('G')->setState(g);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
-        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
-        // Fix frm0 and biases, perturb frm1
-        frm0->fix();
-        cap0->fix();
-        frm1->unfix();
-        cap1->fix();
-        sensor->getStateBlock('G')->fix();
-        frm1->perturb(0.01);
-
-        // solve
-        std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
-        // std::cout << report << std::endl;
-
-        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
-        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
-        // WOLF_INFO(frm1->getStateVector("POV"));
-
-        // remove feature (and factor) for the next loop
-        fea1->remove();
-    }
-}
-
-TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_f0)
-{
-    for (int i = 0; i < N_TESTS; i++)
-    {
-        // Random delta
-        Vector5d delta = Vector5d::Random() * 10;  // delta *= 0;
-        delta(2)       = pi2pi(delta(2));
-
-        // Random initial pose
-        Vector5d x0 = Vector5d::Random() * 10;  // x0 *= 0;
-        x0(2)       = pi2pi(x0(2));
-
-        // Random gravity
-        Vector2d g = Vector2d::Random() * 5;
-        // Vector2d g = Vector2d::Zero();
-
-        // x1 groundtruth
-        Vector5d x1;
-        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
-        // Set states
-        frm0->setState(x0, "POV");
-        frm1->setState(x1, "POV");
-        sensor->getStateBlock('G')->setState(g);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
-        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
-        // Fix frm1 and biases, perturb frm0
-        frm0->unfix();
-        cap0->fix();
-        frm1->fix();
-        cap1->fix();
-        sensor->getStateBlock('G')->fix();
-        frm0->perturb(0.01);
-
-        // solve
-        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
-        ASSERT_POSE2d_APPROX(frm0->getStateVector("POV"), x0, 1e-6);
-        // WOLF_INFO(frm1->getStateVector());
-
-        // remove feature (and factor) for the next loop
-        fea1->remove();
-    }
-}
-
-TEST_F(FactorImu2dWithGravity, bias_zero_solve_b1)
-{
-    for (int i = 0; i < N_TESTS; i++)
-    {
-        // Random delta
-        Vector5d delta = Vector5d::Random() * 10;  // delta *= 0;
-        delta(2)       = pi2pi(delta(2));
-
-        // Random initial pose
-        Vector5d x0 = Vector5d::Random() * 10;  // x0 *= 0;
-        x0(2)       = pi2pi(x0(2));
-
-        // Random gravity
-        Vector2d g = Vector2d::Random() * 5;
-        // Vector2d g = Vector2d::Zero();
-
-        // x1 groundtruth
-        Vector5d x1;
-        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
-        // 0 Initial bias
-        Vector3d b0 = Vector3d::Zero();
-
-        // Set states
-        frm0->setState(x0, "POV");
-        frm1->setState(x1, "POV");
-        cap0->getStateBlock('I')->setState(b0);
-        cap1->getStateBlock('I')->setState(b0);
-        sensor->getStateBlock('G')->setState(g);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
-        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
-        // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
-        frm0->fix();
-        cap0->fix();
-        frm1->fix();
-        sensor->getStateBlock('G')->fix();
-        cap1->unfix();
-        cap1->perturb(0.01);
-
-        // solve  to estimate bias at cap1
-        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
-        ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
-        // WOLF_INFO(cap1->getStateVector());
-
-        // remove feature (and factor) for the next loop
-        fea1->remove();
-    }
-}
-
-TEST_F(FactorImu2dWithGravity, solve_b0)
-{
-    for (int i = 0; i < 50; i++)
-    {
-        // Random delta
-        Vector5d delta_biased = Vector5d::Random() * 10;
-        delta_biased(2)       = pi2pi(delta_biased(2));
-
-        // Random initial pose
-        Vector5d x0 = Vector5d::Random() * 10;
-        x0(2)       = pi2pi(x0(2));
-
-        // Random gravity
-        Vector2d g = Vector2d::Random() * 5;
-        // Vector2d g = Vector2d::Zero();
-
-        // Random Initial bias
-        Vector3d b0 = Vector3d::Random();
-
-        // Corrected delta
-        Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
-        Vector5d delta      = imu2d::plus(delta_biased, delta_step);
-
-        // x1 groundtruth
-        Vector5d x1;
-        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
-        // Set states
-        frm0->setState(x0, "POV");
-        frm1->setState(x1, "POV");
-        cap0->getStateBlock('I')->setState(b0);
-        cap1->getStateBlock('I')->setState(b0);
-        sensor->getStateBlock('G')->setState(g);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
-        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
-        // Fix frm1, perturb frm0
-        frm0->fix();
-        cap0->unfix();
-        frm1->fix();
-        cap1->fix();
-        sensor->getStateBlock('G')->fix();
-        cap0->perturb(0.1);
-
-        // solve
-        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
-        ASSERT_MATRIX_APPROX(cap0->getStateVector("I"), b0, 1e-6);
-        // WOLF_INFO(cap0->getStateVector("I"));
-
-        // remove feature (and factor) for the next loop
-        fea1->remove();
-    }
-}
-
-TEST_F(FactorImu2dwithGravity_test, solve_b1)
-{
-    for (int i = 0; i < N_TESTS; i++)
-    {
-        // Random delta
-        Vector5d delta = Vector5d::Random() * 10;  // delta *= 0;
-        delta(2)       = pi2pi(delta(2));
-
-        // Random initial pose
-        Vector5d x0 = Vector5d::Random() * 10;  // x0 *= 0;
-        x0(2)       = pi2pi(x0(2));
-
-        // Random gravity
-        Vector2d g = Vector2d::Random() * 5;
-        // Vector2d g = Vector2d::Zero();
-
-        // x1 groundtruth
-        Vector5d x1;
-        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
-        // 0 Initial bias
-        Vector3d b0 = Vector3d::Random();
-
-        // Set states
-        frm0->setState(x0, "POV");
-        frm1->setState(x1, "POV");
-        cap0->getStateBlock('I')->setState(b0);
-        cap1->getStateBlock('I')->setState(b0);
-        sensor->getStateBlock('G')->setState(g);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
-        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
-        // Fix frm0 and frm1, unfix bias, perturb cap1
-        frm0->fix();
-        cap0->fix();
-        frm1->fix();
-        sensor->getStateBlock('G')->fix();
-        cap1->unfix();
-        cap1->perturb(0.01);
-
-        // solve  to estimate bias at cap1
-        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
-        ASSERT_MATRIX_APPROX(cap1->getStateVector("I"), b0, 1e-6);
-        // WOLF_INFO(cap1->getStateVector("I"));
-
-        // remove feature (and factor) for the next loop
-        fea1->remove();
-    }
-}
-
-TEST_F(FactorImu2dwithGravity_test, solve_f0)
-{
-    for (int i = 0; i < N_TESTS; i++)
-    {
-        // Random delta
-        Vector5d delta_biased = Vector5d::Random() * 10;
-        delta_biased(2)       = pi2pi(delta_biased(2));
-
-        // Random initial pose
-        Vector5d x0 = Vector5d::Random() * 10;
-        x0(2)       = pi2pi(x0(2));
-
-        // Random gravity
-        Vector2d g = Vector2d::Random() * 5;
-        // Vector2d g = Vector2d::Zero();
-
-        // Random Initial bias
-        Vector3d b0 = Vector3d::Random();
-
-        // Corrected delta
-        Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
-        Vector5d delta      = imu2d::plus(delta_biased, delta_step);
-
-        // x1 groundtruth
-        Vector5d x1;
-        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
-        // Set states
-        frm0->setState(x0, "POV");
-        frm1->setState(x1, "POV");
-        cap0->getStateBlock('I')->setState(b0);
-        cap1->getStateBlock('I')->setState(b0);
-        sensor->getStateBlock('G')->setState(g);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
-        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
-        // Fix frm1, perturb frm0
-        frm0->unfix();
-        cap0->fix();
-        frm1->fix();
-        cap1->fix();
-        sensor->getStateBlock('G')->fix();
-        frm0->perturb(0.1);
-
-        // solve
-        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
-        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
-        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
-        // WOLF_INFO(frm0->getStateVector("POV"));
-
-        // remove feature (and factor) for the next loop
-        fea1->remove();
-    }
-}
-
-TEST_F(FactorImu2dwithGravity_test, solve_f1)
-{
-    for (int i = 0; i < N_TESTS; i++)
-    {
-        // Random delta
-        Vector5d delta_biased = Vector5d::Random() * 10;
-        delta_biased(2)       = pi2pi(delta_biased(2));
-
-        // Random initial pose
-        Vector5d x0 = Vector5d::Random() * 10;
-        x0(2)       = pi2pi(x0(2));
-
-        // Random gravity
-        Vector2d g = Vector2d::Random() * 5;
-        // Vector2d g = Vector2d::Zero();
-
-        // Random Initial bias
-        Vector3d b0 = Vector3d::Random();
-
-        // Corrected delta
-        Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
-        Vector5d delta      = imu2d::plus(delta_biased, delta_step);
-
-        // x1 groundtruth
-        Vector5d x1;
-        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
-        // Set states
-        frm0->setState(x0, "POV");
-        frm1->setState(x1, "POV");
-        cap0->getStateBlock('I')->setState(b0);
-        cap1->getStateBlock('I')->setState(b0);
-        sensor->getStateBlock('G')->setState(g);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
-        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
-        // Fix frm1, perturb frm0
-        frm0->fix();
-        cap0->fix();
-        frm1->unfix();
-        cap1->fix();
-        sensor->getStateBlock('G')->fix();
-        frm1->perturb(0.1);
-
-        // solve
-        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
-        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
-        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
-        // WOLF_INFO(frm0->getStateVector("POV"));
-
-        // remove feature (and factor) for the next loop
-        fea1->remove();
-    }
-}
-
-TEST_F(FactorImu2dwithGravity_test, solve_f1_b1)
-{
-    for (int i = 0; i < N_TESTS; i++)
-    {
-        // Random delta
-        Vector5d delta_biased = Vector5d::Random() * 10;
-        delta_biased(2)       = pi2pi(delta_biased(2));
-
-        // Random initial pose
-        Vector5d x0 = Vector5d::Random() * 10;
-        x0(2)       = pi2pi(x0(2));
-
-        // Random gravity
-        Vector2d g = Vector2d::Random() * 5;
-        // Vector2d g = Vector2d::Zero();
-
-        // Random Initial bias
-        Vector3d b0 = Vector3d::Random();
-
-        // Corrected delta
-        Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
-        Vector5d delta      = imu2d::plus(delta_biased, delta_step);
-
-        // x1 groundtruth
-        Vector5d x1;
-        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
-        // Set states
-        frm0->setState(x0, "POV");
-        frm1->setState(x1, "POV");
-        cap0->getStateBlock('I')->setState(b0);
-        cap1->getStateBlock('I')->setState(b0);
-        sensor->getStateBlock('G')->setState(g);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
-        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
-        // Fix frm1, perturb frm0
-        frm0->fix();
-        cap0->fix();
-        sensor->getStateBlock('G')->fix();
-        frm1->unfix();
-        cap1->unfix();
-        frm1->perturb(0.1);
-        cap1->perturb(0.1);
-
-        // solve
-        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
-        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
-        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
-        ASSERT_MATRIX_APPROX(cap1->getStateVector("I"), b0, 1e-6);
-        // WOLF_INFO(frm0->getStateVector("POV"));
-
-        // remove feature (and factor) for the next loop
-        fea1->remove();
-    }
-}
-
-TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_G)
-{
-    for (int i = 0; i < N_TESTS; i++)
-    {
-        // WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
-        // Random delta
-        Vector5d delta = Vector5d::Random() * 10;  // delta *= 0;
-        delta(2)       = pi2pi(delta(2));
-
-        // Random initial pose
-        Vector5d x0 = Vector5d::Random() * 10;  // x0 *= 0;
-        x0(2)       = pi2pi(x0(2));
-
-        // Random gravity
-        // Vector2d g = Vector2d::Random()*5;
-        Vector2d g = Vector2d::Zero();
-
-        // Zero bias
-        Vector3d b0 = Vector3d::Zero();
-
-        // x1 groundtruth
-        Vector5d x1;
-        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
-        // Set states
-        frm0->setState(x0, "POV");
-        frm1->setState(x1, "POV");
-        cap0->getStateBlock('I')->setState(b0);
-        cap1->getStateBlock('I')->setState(b0);
-        sensor->getStateBlock('G')->setState(g);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
-        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
-        // Fix frames and biases, perturb g
-        frm0->fix();
-        cap0->fix();
-        frm1->fix();
-        cap1->fix();
-        sensor->getStateBlock('G')->unfix();
-        sensor->getStateBlock('G')->perturb(1);
-
-        // solve
-        std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
-
-        ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
-        // WOLF_INFO(frm1->getStateVector("POV"));
-
-        // remove feature (and factor) for the next loop
-        fea1->remove();
-    }
-}
-TEST_F(FactorImu2dwithGravity_test, solve_G)
-{
-    for (int i = 0; i < N_TESTS; i++)
-    {
-        // Random delta
-        Vector5d delta_biased = Vector5d::Random() * 10;
-        delta_biased(2)       = pi2pi(delta_biased(2));
-
-        // Random initial pose
-        Vector5d x0 = Vector5d::Random() * 10;
-        x0(2)       = pi2pi(x0(2));
-
-        // Random gravity
-        Vector2d g = Vector2d::Random() * 5;
-        // Vector2d g = Vector2d::Zero();
-
-        // Random Initial bias
-        Vector3d b0 = Vector3d::Random();
-
-        // Corrected delta
-        Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
-        Vector5d delta      = imu2d::plus(delta_biased, delta_step);
-
-        // x1 groundtruth
-        Vector5d x1;
-        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
-        // Set states
-        frm0->setState(x0, "POV");
-        frm1->setState(x1, "POV");
-        cap0->getStateBlock('I')->setState(b0);
-        cap1->getStateBlock('I')->setState(b0);
-        sensor->getStateBlock('G')->setState(g);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
-        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
-        // Fix frames and captures, perturb g
-        frm0->fix();
-        cap0->fix();
-        frm1->fix();
-        cap1->fix();
-        sensor->getStateBlock('G')->unfix();
-        sensor->getStateBlock('G')->perturb(1);
-
-        // solve
-        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
-        ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
-        // WOLF_INFO(cap0->getStateVector("POV"));
-
-        // remove feature (and factor) for the next loop
-        fea1->remove();
-    }
-}
-
-int main(int argc, char **argv)
-{
-    testing::InitGoogleTest(&argc, argv);
-    return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_factor_imu_2d.cpp b/test/gtest_factor_imu_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8aa7762dfa35a7741c47e964d6f0f6fb6c417bd9
--- /dev/null
+++ b/test/gtest_factor_imu_2d.cpp
@@ -0,0 +1,341 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/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/>.
+
+#include "imu/common/imu.h"
+
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+
+#include "imu/factor/factor_imu_2d.h"
+#include "imu/math/imu_2d_tools.h"
+#include "imu/sensor/sensor_imu.h"
+
+using namespace Eigen;
+using namespace wolf;
+
+std::string imu_dir         = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
+int         N_TESTS         = 10;
+
+class FactorImu2d_test : public testing::Test
+{
+  public:
+    Matrix6d         data_cov;
+    Matrix5d         delta_cov;
+    MatrixXd         jacobian_bias = MatrixXd(5, 3);
+    Vector3d         b0_preint;
+    ProblemPtr       problem;
+    SolverManagerPtr solver;
+    FrameBasePtr     frm0, frm1;
+    SensorBasePtr    sensor;
+    CaptureImuPtr    cap0, cap1;
+
+    void SetUp() override
+    {
+        // Input odometry data and covariance
+        data_cov  = 0.1 * Matrix6d::Identity();
+        delta_cov = 0.1 * Matrix5d::Identity();
+
+        // Jacobian of the bias
+        jacobian_bias << 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0;
+        // preintegration bias
+        b0_preint = Vector3d::Zero();
+
+        // Problem and solver
+        problem = Problem::create("POV", 2);
+        solver = FactorySolverFile::create(
+            "SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
+
+        // Two frames
+        frm0 = problem->emplaceFrame(TimeStamp(0), "POV", Vector5d::Zero());
+        frm1 = problem->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero());
+
+        // Imu2d sensor
+        sensor = problem->installSensor(imu_dir + "/test/yaml/sensor_imu_2d.yaml", {imu_dir});
+
+        // capture from frm1 to frm0
+        cap0 =
+            CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
+        cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
+    }
+};
+
+TEST_F(FactorImu2d_test, check_tree)
+{
+    ASSERT_TRUE(problem->check(0));
+}
+
+TEST_F(FactorImu2d_test, bias_zero_solve_f1)
+{
+    for (int i = 0; i < N_TESTS; i++)
+    {
+        // Random delta
+        Vector5d delta = Vector5d::Random() * 10;  // delta *= 0;
+        delta(2)       = pi2pi(delta(2));
+
+        // Random initial pose
+        Vector5d x0 = Vector5d::Random() * 10;  // x0 *= 0;
+        x0(2)       = pi2pi(x0(2));
+
+        // x1 groundtruth
+        Vector5d x1;
+        x1 = imu2d::compose(x0, delta, 1);
+
+        // Set states
+        frm0->setState(x0, "POV");
+        frm1->setState(x1, "POV");
+
+        // feature & factor with delta measurement
+        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+        FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+        // Fix frm0 and biases, perturb frm1
+        frm0->fix();
+        cap0->fix();
+        frm1->unfix();
+        cap1->fix();
+        frm1->perturb(0.01);
+
+        // solve
+        problem->print(4, 1, 1, 1);
+        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+        problem->print(4, 1, 1, 1);
+
+        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+        // WOLF_INFO(frm1->getStateVector("POV"));
+
+        // remove feature (and factor) for the next loop
+        fea1->remove();
+    }
+}
+
+TEST_F(FactorImu2d_test, bias_zero_solve_f0)
+{
+    for (int i = 0; i < N_TESTS; i++)
+    {
+        // Random delta
+        Vector5d delta = Vector5d::Random() * 10;  // delta *= 0;
+        delta(2)       = pi2pi(delta(2));
+
+        // Random initial pose
+        Vector5d x0 = Vector5d::Random() * 10;  // x0 *= 0;
+        x0(2)       = pi2pi(x0(2));
+
+        // x1 groundtruth
+        Vector5d x1;
+        x1 = imu2d::compose(x0, delta, 1);
+
+        // Set states
+        frm0->setState(x0, "POV");
+        frm1->setState(x1, "POV");
+
+        // feature & factor with delta measurement
+        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+        FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+        // Fix frm0 and biases, perturb frm1
+        frm0->unfix();
+        cap0->fix();
+        frm1->fix();
+        cap1->fix();
+        frm0->perturb(0.01);
+
+        // solve
+        problem->print(4, 1, 1, 1);
+        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+        problem->print(4, 1, 1, 1);
+
+        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+        // WOLF_INFO(frm1->getStateVector("POV"));
+
+        // remove feature (and factor) for the next loop
+        fea1->remove();
+    }
+}
+
+TEST_F(FactorImu2d_test, solve_b0)
+{
+    for (int i = 0; i < N_TESTS; i++)
+    {
+        // Random delta
+        Vector5d delta_biased = Vector5d::Random() * 10;
+        delta_biased(2)       = pi2pi(delta_biased(2));
+
+        // Random initial pose
+        Vector5d x0 = Vector5d::Random() * 10;
+        x0(2)       = pi2pi(x0(2));
+
+        // Random Initial bias
+        Vector3d b0 = Vector3d::Random();
+
+        // Corrected delta
+        Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+        Vector5d delta      = imu2d::plus(delta_biased, delta_step);
+
+        // x1 groundtruth
+        Vector5d x1;
+        x1 = imu2d::compose(x0, delta, 1);
+
+        // Set states
+        frm0->setState(x0, "POV");
+        frm1->setState(x1, "POV");
+        cap0->getStateBlock('I')->setState(b0);
+        cap1->getStateBlock('I')->setState(b0);
+
+        // feature & factor with delta measurement
+        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+        FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+        // Fix frm1, perturb frm0
+        frm0->fix();
+        cap0->unfix();
+        frm1->fix();
+        cap1->fix();
+        cap0->perturb(0.1);
+
+        // solve
+        problem->print(4, 1, 1, 1);
+        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+        problem->print(4, 1, 1, 1);
+
+        EXPECT_POSE2d_APPROX(cap0->getStateVector("I"), b0, 1e-6);
+        // WOLF_INFO(cap0->getStateVector("I"));
+
+        // remove feature (and factor) for the next loop
+        fea1->remove();
+    }
+}
+
+TEST_F(FactorImu2d_test, solve_f0)
+{
+    for (int i = 0; i < N_TESTS; i++)
+    {
+        // Random delta
+        Vector5d delta_biased = Vector5d::Random() * 10;
+        delta_biased(2)       = pi2pi(delta_biased(2));
+
+        // Random initial pose
+        Vector5d x0 = Vector5d::Random() * 10;
+        x0(2)       = pi2pi(x0(2));
+
+        // Random Initial bias
+        Vector3d b0 = Vector3d::Random();
+
+        // Corrected delta
+        Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+        Vector5d delta      = imu2d::plus(delta_biased, delta_step);
+
+        // x1 groundtruth
+        Vector5d x1;
+        x1 = imu2d::compose(x0, delta, 1);
+
+        // Set states
+        frm0->setState(x0, "POV");
+        frm1->setState(x1, "POV");
+        cap0->getStateBlock('I')->setState(b0);
+        cap1->getStateBlock('I')->setState(b0);
+
+        // feature & factor with delta measurement
+        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+        FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+        // Fix frm1, perturb frm0
+        frm0->unfix();
+        cap0->fix();
+        frm1->fix();
+        cap1->fix();
+        frm0->perturb(0.1);
+
+        // solve
+        problem->print(4, 1, 1, 1);
+        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+        problem->print(4, 1, 1, 1);
+
+        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+        // WOLF_INFO(frm0->getStateVector("POV"));
+
+        // remove feature (and factor) for the next loop
+        fea1->remove();
+    }
+}
+
+TEST_F(FactorImu2d_test, solve_f1)
+{
+    for (int i = 0; i < N_TESTS; i++)
+    {
+        // Random delta
+        Vector5d delta_biased = Vector5d::Random() * 10;
+        delta_biased(2)       = pi2pi(delta_biased(2));
+
+        // Random initial pose
+        Vector5d x0 = Vector5d::Random() * 10;
+        x0(2)       = pi2pi(x0(2));
+
+        // Random Initial bias
+        Vector3d b0 = Vector3d::Random();
+
+        // Corrected delta
+        Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+        Vector5d delta      = imu2d::plus(delta_biased, delta_step);
+
+        // x1 groundtruth
+        Vector5d x1;
+        x1 = imu2d::compose(x0, delta, 1);
+
+        // Set states
+        frm0->setState(x0, "POV");
+        frm1->setState(x1, "POV");
+        cap0->getStateBlock('I')->setState(b0);
+        cap1->getStateBlock('I')->setState(b0);
+
+        // feature & factor with delta measurement
+        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+        FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+        // Fix frm1, perturb frm0
+        frm0->fix();
+        cap0->fix();
+        frm1->unfix();
+        cap1->fix();
+        frm1->perturb(0.1);
+
+        // solve
+        problem->print(4, 1, 1, 1);
+        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+        problem->print(4, 1, 1, 1);
+
+        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+        // WOLF_INFO(frm0->getStateVector("POV"));
+
+        // remove feature (and factor) for the next loop
+        fea1->remove();
+    }
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    // ::testing::GTEST_FLAG(filter) = "FactorImu2d_test.solve_f1_b1";  // Test only this one
+    return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu_2d_with_gravity.cpp
similarity index 70%
rename from test/gtest_factor_imu2d.cpp
rename to test/gtest_factor_imu_2d_with_gravity.cpp
index eda9d97a5bb0684031c60f3925ec185e7c669812..f8f5e80b08bf5bed42cf1bb68c84ae01540d0d71 100644
--- a/test/gtest_factor_imu2d.cpp
+++ b/test/gtest_factor_imu_2d_with_gravity.cpp
@@ -1,8 +1,7 @@
 // WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
+// 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: http://www.iri.upc.edu/wolf
 // WOLF is free software: you can redistribute it and/or modify
@@ -18,13 +17,14 @@
 // 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/>.
 
+#include "imu/common/imu.h"
+
 #include <core/ceres_wrapper/solver_ceres.h>
 #include <core/utils/utils_gtest.h>
 
-#include "imu/factor/factor_imu2d.h"
-#include "imu/math/imu2d_tools.h"
+#include "imu/factor/factor_imu_2d_with_gravity.h"
+#include "imu/math/imu_2d_tools.h"
 #include "imu/sensor/sensor_imu.h"
-#include "imu/internal/config.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -33,7 +33,7 @@ std::string imu_dir         = _WOLF_IMU_CODE_DIR;
 std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
 int         N_TESTS         = 10;
 
-class FactorImu2d_test : public testing::Test
+class FactorImu2dWithGravityTest : public testing::Test
 {
   public:
     Matrix6d         data_cov;
@@ -46,7 +46,7 @@ class FactorImu2d_test : public testing::Test
     SensorBasePtr    sensor;
     CaptureImuPtr    cap0, cap1;
 
-    virtual void SetUp()
+    void SetUp() override
     {
         // Input odometry data and covariance
         data_cov  = 0.1 * Matrix6d::Identity();
@@ -59,8 +59,7 @@ class FactorImu2d_test : public testing::Test
 
         // Problem and solver
         problem = Problem::create("POV", 2);
-
-        solver = FactorySolverFile::create(
+        solver  = FactorySolverFile::create(
             "SolverCeres", problem, imu_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir});
 
         // Two frames
@@ -68,26 +67,25 @@ class FactorImu2d_test : public testing::Test
         frm1 = problem->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero());
 
         // Imu2d sensor
-        sensor = problem->installSensor(imu_dir + "/test/yaml/sensor_imu2d.yaml", {imu_dir});
+        sensor = problem->installSensor(imu_dir + "/test/yaml/sensor_imu_2d_with_gravity.yaml", {imu_dir});
 
         // capture from frm1 to frm0
         cap0 =
             CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
         cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
     }
-
-    virtual void TearDown() {}
 };
 
-TEST_F(FactorImu2d_test, check_tree)
+TEST_F(FactorImu2dWithGravityTest, check_tree)
 {
     ASSERT_TRUE(problem->check(0));
 }
 
-TEST_F(FactorImu2d_test, bias_zero_solve_f1)
+TEST_F(FactorImu2dWithGravityTest, bias_zero_solve_f1)
 {
     for (int i = 0; i < N_TESTS; i++)
     {
+        // WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
         // Random delta
         Vector5d delta = Vector5d::Random() * 10;  // delta *= 0;
         delta(2)       = pi2pi(delta(2));
@@ -96,29 +94,34 @@ TEST_F(FactorImu2d_test, bias_zero_solve_f1)
         Vector5d x0 = Vector5d::Random() * 10;  // x0 *= 0;
         x0(2)       = pi2pi(x0(2));
 
+        // Random gravity
+        Vector2d g = Vector2d::Random() * 5;
+        // Vector2d g = Vector2d::Zero();
+
         // x1 groundtruth
         Vector5d x1;
-        x1 = imu2d::compose(x0, delta, 1);
+        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
 
         // Set states
         frm0->setState(x0, "POV");
         frm1->setState(x1, "POV");
+        sensor->getStateBlock('G')->setState(g);
 
         // feature & factor with delta measurement
         auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
-        FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
 
         // Fix frm0 and biases, perturb frm1
         frm0->fix();
         cap0->fix();
         frm1->unfix();
         cap1->fix();
+        sensor->getStateBlock('G')->fix();
         frm1->perturb(0.01);
 
         // solve
-        problem->print(4, 1, 1, 1);
-        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-        problem->print(4, 1, 1, 1);
+        std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+        // std::cout << report << std::endl;
 
         ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
         ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
@@ -129,7 +132,7 @@ TEST_F(FactorImu2d_test, bias_zero_solve_f1)
     }
 }
 
-TEST_F(FactorImu2d_test, bias_zero_solve_f0)
+TEST_F(FactorImu2dWithGravityTest, bias_zero_solve_f0)
 {
     for (int i = 0; i < N_TESTS; i++)
     {
@@ -141,91 +144,46 @@ TEST_F(FactorImu2d_test, bias_zero_solve_f0)
         Vector5d x0 = Vector5d::Random() * 10;  // x0 *= 0;
         x0(2)       = pi2pi(x0(2));
 
+        // Random gravity
+        Vector2d g = Vector2d::Random() * 5;
+        // Vector2d g = Vector2d::Zero();
+
         // x1 groundtruth
         Vector5d x1;
-        x1 = imu2d::compose(x0, delta, 1);
+        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
 
         // Set states
         frm0->setState(x0, "POV");
         frm1->setState(x1, "POV");
+        sensor->getStateBlock('G')->setState(g);
 
         // feature & factor with delta measurement
         auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
-        FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
 
-        // Fix frm0 and biases, perturb frm1
+        // Fix frm1 and biases, perturb frm0
         frm0->unfix();
         cap0->fix();
         frm1->fix();
         cap1->fix();
+        sensor->getStateBlock('G')->fix();
         frm0->perturb(0.01);
 
         // solve
-        problem->print(4, 1, 1, 1);
-        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-        problem->print(4, 1, 1, 1);
-
-        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
-        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
-        // WOLF_INFO(frm1->getStateVector("POV"));
-
-        // remove feature (and factor) for the next loop
-        fea1->remove();
-    }
-}
-
-TEST_F(FactorImu2d_test, bias_zero_solve_b1)
-{
-    for (int i = 0; i < N_TESTS; i++)
-    {
-        // Random delta
-        Vector5d delta = Vector5d::Random() * 10;  // delta *= 0;
-        delta(2)       = pi2pi(delta(2));
-
-        // Random initial pose
-        Vector5d x0 = Vector5d::Random() * 10;  // x0 *= 0;
-        x0(2)       = pi2pi(x0(2));
-
-        // x1 groundtruth
-        Vector5d x1;
-        x1 = imu2d::compose(x0, delta, 1);
-
-        // 0 Initial bias
-        Vector3d b0 = Vector3d::Zero();
-
-        // Set states
-        frm0->setState(x0, "POV");
-        frm1->setState(x1, "POV");
-        cap0->getStateBlock('I')->setState(b0);
-        cap1->getStateBlock('I')->setState(b0);
-
-        // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
-        FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
-        // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
-        frm0->fix();
-        cap0->fix();
-        frm1->fix();
-        cap1->unfix();
-        cap1->perturb(0.01);
-
-        // solve  to estimate bias at cap1
-        problem->print(4, 1, 1, 1);
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-        problem->print(4, 1, 1, 1);
 
-        EXPECT_POSE2d_APPROX(cap1->getStateVector("I"), b0, 1e-6);
-        // WOLF_INFO(cap1->getStateVector("I"));
+        ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm0->getStateVector("V"), x0.tail(2), 1e-6);
+        // WOLF_INFO(frm1->getStateVector());
 
         // remove feature (and factor) for the next loop
         fea1->remove();
     }
 }
 
-TEST_F(FactorImu2d_test, solve_b0)
+TEST_F(FactorImu2dWithGravityTest, solve_b0)
 {
-    for (int i = 0; i < N_TESTS; i++)
+    for (int i = 0; i < 50; i++)
     {
         // Random delta
         Vector5d delta_biased = Vector5d::Random() * 10;
@@ -235,6 +193,10 @@ TEST_F(FactorImu2d_test, solve_b0)
         Vector5d x0 = Vector5d::Random() * 10;
         x0(2)       = pi2pi(x0(2));
 
+        // Random gravity
+        Vector2d g = Vector2d::Random() * 5;
+        // Vector2d g = Vector2d::Zero();
+
         // Random Initial bias
         Vector3d b0 = Vector3d::Random();
 
@@ -244,31 +206,31 @@ TEST_F(FactorImu2d_test, solve_b0)
 
         // x1 groundtruth
         Vector5d x1;
-        x1 = imu2d::compose(x0, delta, 1);
+        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
 
         // Set states
         frm0->setState(x0, "POV");
         frm1->setState(x1, "POV");
         cap0->getStateBlock('I')->setState(b0);
         cap1->getStateBlock('I')->setState(b0);
+        sensor->getStateBlock('G')->setState(g);
 
         // feature & factor with delta measurement
         auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
-        FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
 
         // Fix frm1, perturb frm0
         frm0->fix();
         cap0->unfix();
         frm1->fix();
         cap1->fix();
+        sensor->getStateBlock('G')->fix();
         cap0->perturb(0.1);
 
         // solve
-        problem->print(4, 1, 1, 1);
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-        problem->print(4, 1, 1, 1);
 
-        EXPECT_POSE2d_APPROX(cap0->getStateVector("I"), b0, 1e-6);
+        ASSERT_MATRIX_APPROX(cap0->getStateVector("I"), b0, 1e-6);
         // WOLF_INFO(cap0->getStateVector("I"));
 
         // remove feature (and factor) for the next loop
@@ -276,56 +238,65 @@ TEST_F(FactorImu2d_test, solve_b0)
     }
 }
 
-TEST_F(FactorImu2d_test, solve_b1)
+TEST_F(FactorImu2dWithGravityTest, solve_f0)
 {
     for (int i = 0; i < N_TESTS; i++)
     {
         // Random delta
-        Vector5d delta = Vector5d::Random() * 10;  // delta *= 0;
-        delta(2)       = pi2pi(delta(2));
+        Vector5d delta_biased = Vector5d::Random() * 10;
+        delta_biased(2)       = pi2pi(delta_biased(2));
 
         // Random initial pose
-        Vector5d x0 = Vector5d::Random() * 10;  // x0 *= 0;
+        Vector5d x0 = Vector5d::Random() * 10;
         x0(2)       = pi2pi(x0(2));
 
-        // x1 groundtruth
-        Vector5d x1;
-        x1 = imu2d::compose(x0, delta, 1);
+        // Random gravity
+        Vector2d g = Vector2d::Random() * 5;
+        // Vector2d g = Vector2d::Zero();
 
-        // 0 Initial bias
+        // Random Initial bias
         Vector3d b0 = Vector3d::Random();
 
+        // Corrected delta
+        Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+        Vector5d delta      = imu2d::plus(delta_biased, delta_step);
+
+        // x1 groundtruth
+        Vector5d x1;
+        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
         // Set states
         frm0->setState(x0, "POV");
         frm1->setState(x1, "POV");
         cap0->getStateBlock('I')->setState(b0);
         cap1->getStateBlock('I')->setState(b0);
+        sensor->getStateBlock('G')->setState(g);
 
         // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
-        FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
 
-        // Fix frm0 and frm1, unfix bias, perturb cap1
-        frm0->fix();
+        // Fix frm1, perturb frm0
+        frm0->unfix();
         cap0->fix();
         frm1->fix();
-        cap1->unfix();
-        cap1->perturb(0.01);
+        cap1->fix();
+        sensor->getStateBlock('G')->fix();
+        frm0->perturb(0.1);
 
-        // solve  to estimate bias at cap1
-        problem->print(4, 1, 1, 1);
+        // solve
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-        problem->print(4, 1, 1, 1);
 
-        EXPECT_POSE2d_APPROX(cap1->getStateVector("I"), b0, 1e-6);
-        // WOLF_INFO(cap1->getStateVector("I"));
+        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+        // WOLF_INFO(frm0->getStateVector("POV"));
 
         // remove feature (and factor) for the next loop
         fea1->remove();
     }
 }
 
-TEST_F(FactorImu2d_test, solve_f0)
+TEST_F(FactorImu2dWithGravityTest, solve_f1)
 {
     for (int i = 0; i < N_TESTS; i++)
     {
@@ -337,6 +308,10 @@ TEST_F(FactorImu2d_test, solve_f0)
         Vector5d x0 = Vector5d::Random() * 10;
         x0(2)       = pi2pi(x0(2));
 
+        // Random gravity
+        Vector2d g = Vector2d::Random() * 5;
+        // Vector2d g = Vector2d::Zero();
+
         // Random Initial bias
         Vector3d b0 = Vector3d::Random();
 
@@ -346,29 +321,29 @@ TEST_F(FactorImu2d_test, solve_f0)
 
         // x1 groundtruth
         Vector5d x1;
-        x1 = imu2d::compose(x0, delta, 1);
+        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
 
         // Set states
         frm0->setState(x0, "POV");
         frm1->setState(x1, "POV");
         cap0->getStateBlock('I')->setState(b0);
         cap1->getStateBlock('I')->setState(b0);
+        sensor->getStateBlock('G')->setState(g);
 
         // feature & factor with delta measurement
         auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
-        FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
 
         // Fix frm1, perturb frm0
-        frm0->unfix();
+        frm0->fix();
         cap0->fix();
-        frm1->fix();
+        frm1->unfix();
         cap1->fix();
-        frm0->perturb(0.1);
+        sensor->getStateBlock('G')->fix();
+        frm1->perturb(0.1);
 
         // solve
-        problem->print(4, 1, 1, 1);
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-        problem->print(4, 1, 1, 1);
 
         ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
         ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
@@ -379,61 +354,61 @@ TEST_F(FactorImu2d_test, solve_f0)
     }
 }
 
-TEST_F(FactorImu2d_test, solve_f1)
+TEST_F(FactorImu2dWithGravityTest, bias_zero_solve_G)
 {
     for (int i = 0; i < N_TESTS; i++)
     {
+        // WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
         // Random delta
-        Vector5d delta_biased = Vector5d::Random() * 10;
-        delta_biased(2)       = pi2pi(delta_biased(2));
+        Vector5d delta = Vector5d::Random() * 10;  // delta *= 0;
+        delta(2)       = pi2pi(delta(2));
 
         // Random initial pose
-        Vector5d x0 = Vector5d::Random() * 10;
+        Vector5d x0 = Vector5d::Random() * 10;  // x0 *= 0;
         x0(2)       = pi2pi(x0(2));
 
-        // Random Initial bias
-        Vector3d b0 = Vector3d::Random();
+        // Random gravity
+        // Vector2d g = Vector2d::Random()*5;
+        Vector2d g = Vector2d::Zero();
 
-        // Corrected delta
-        Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
-        Vector5d delta      = imu2d::plus(delta_biased, delta_step);
+        // Zero bias
+        Vector3d b0 = Vector3d::Zero();
 
         // x1 groundtruth
         Vector5d x1;
-        x1 = imu2d::compose(x0, delta, 1);
+        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
 
         // Set states
         frm0->setState(x0, "POV");
         frm1->setState(x1, "POV");
         cap0->getStateBlock('I')->setState(b0);
         cap1->getStateBlock('I')->setState(b0);
+        sensor->getStateBlock('G')->setState(g);
 
         // feature & factor with delta measurement
-        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
-        FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+        auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
 
-        // Fix frm1, perturb frm0
+        // Fix frames and biases, perturb g
         frm0->fix();
         cap0->fix();
-        frm1->unfix();
+        frm1->fix();
         cap1->fix();
-        frm1->perturb(0.1);
+        sensor->getStateBlock('G')->unfix();
+        sensor->getStateBlock('G')->perturb(1);
 
         // solve
-        problem->print(4, 1, 1, 1);
-        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-        problem->print(4, 1, 1, 1);
+        std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
 
-        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
-        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
-        // WOLF_INFO(frm0->getStateVector("POV"));
+        ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
+        // WOLF_INFO(frm1->getStateVector("POV"));
 
         // remove feature (and factor) for the next loop
         fea1->remove();
     }
 }
 
-TEST_F(FactorImu2d_test, solve_f1_b1)
+TEST_F(FactorImu2dWithGravityTest, solve_G)
 {
     for (int i = 0; i < N_TESTS; i++)
     {
@@ -445,6 +420,10 @@ TEST_F(FactorImu2d_test, solve_f1_b1)
         Vector5d x0 = Vector5d::Random() * 10;
         x0(2)       = pi2pi(x0(2));
 
+        // Random gravity
+        Vector2d g = Vector2d::Random() * 5;
+        // Vector2d g = Vector2d::Zero();
+
         // Random Initial bias
         Vector3d b0 = Vector3d::Random();
 
@@ -454,35 +433,32 @@ TEST_F(FactorImu2d_test, solve_f1_b1)
 
         // x1 groundtruth
         Vector5d x1;
-        x1 = imu2d::compose(x0, delta, 1);
+        x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
 
         // Set states
         frm0->setState(x0, "POV");
         frm1->setState(x1, "POV");
         cap0->getStateBlock('I')->setState(b0);
         cap1->getStateBlock('I')->setState(b0);
+        sensor->getStateBlock('G')->setState(g);
 
         // feature & factor with delta measurement
         auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
-        FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+        FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
 
-        // Fix frm1, perturb frm0
+        // Fix frames and captures, perturb g
         frm0->fix();
         cap0->fix();
-        frm1->unfix();
-        cap1->unfix();
-        frm1->perturb(0.1);
-        cap1->perturb(0.1);
+        frm1->fix();
+        cap1->fix();
+        sensor->getStateBlock('G')->unfix();
+        sensor->getStateBlock('G')->perturb(1);
 
         // solve
-        problem->print(4, 1, 1, 1);
         std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-        problem->print(4, 1, 1, 1);
 
-        ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
-        ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
-        EXPECT_MATRIX_APPROX(cap1->getStateVector("I"), b0, 1e-6);
-        // WOLF_INFO(frm0->getStateVector("POV"));
+        ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
+        // WOLF_INFO(cap0->getStateVector("POV"));
 
         // remove feature (and factor) for the next loop
         fea1->remove();
@@ -492,6 +468,5 @@ TEST_F(FactorImu2d_test, solve_f1_b1)
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
-    // ::testing::GTEST_FLAG(filter) = "FactorImu2d_test.solve_f1_b1";  // Test only this one
     return RUN_ALL_TESTS();
 }
diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu_3d.cpp
similarity index 99%
rename from test/gtest_factor_imu.cpp
rename to test/gtest_factor_imu_3d.cpp
index 63ae694c045eef311d02126415958fd3c6367b7c..82580f1696e2abfd4ac878386537c552a0b55c45 100644
--- a/test/gtest_factor_imu.cpp
+++ b/test/gtest_factor_imu_3d.cpp
@@ -25,7 +25,7 @@
 // Imu
 #include "imu/internal/config.h"
 #include "imu/capture/capture_imu.h"
-#include "imu/processor/processor_imu.h"
+#include "imu/processor/processor_imu_3d.h"
 #include "imu/sensor/sensor_imu.h"
 
 //Wolf
diff --git a/test/gtest_feature_imu.cpp b/test/gtest_feature_imu.cpp
deleted file mode 100644
index c69f596b0ff637f8fb0a659831e39bf7b7bc4feb..0000000000000000000000000000000000000000
--- a/test/gtest_feature_imu.cpp
+++ /dev/null
@@ -1,186 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/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/>.
-
-//Wolf
-#include "imu/capture/capture_imu.h"
-#include "imu/processor/processor_imu.h"
-#include "imu/sensor/sensor_imu.h"
-#include "core/common/wolf.h"
-#include "core/problem/problem.h"
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/utils/utils_gtest.h"
-#include "core/utils/logging.h"
-
-using namespace wolf;
-using namespace Eigen;
-using std::make_shared;
-using std::static_pointer_cast;
-
-class FeatureImu_test : public testing::Test
-{
-
-    public: //These can be accessed in fixtures
-        ProblemPtr problem;
-        TimeStamp ts;
-        CaptureImuPtr imu_ptr;
-        VectorXd state_vec;
-        VectorXd delta_preint;
-        Matrix<double,9,9> delta_preint_cov;
-        FeatureImuPtr feat_imu;
-        FrameBasePtr last_frame;
-        FrameBasePtr origin_frame;
-        MatrixXd dD_db_jacobians;
-        ProcessorMotionPtr processor_motion_ptr_;
-
-    //a new of this will be created for each new test
-    void SetUp() override
-    {
-
-    // Wolf problem
-        problem = Problem::create("POV", 3);
-        SpecSensorComposite imu_priors{{'P',SpecStateSensor("P",Vector3d::Zero())},
-                                       {'O',SpecStateSensor("O",(Vector4d() << 0,0,0,1).finished())},
-                                       {'I',SpecStateSensor("StateBlock",Vector6d::Zero(),"factor",Vector6d::Ones(),true)}};
-        ParamsSensorImuPtr sen_imu_params = make_shared<ParamsSensorImu>();
-        SensorBasePtr sensor_ptr = SensorBase::emplace<SensorImu3d>(problem->getHardware(), sen_imu_params, imu_priors);
-        ParamsProcessorImuPtr prc_imu_params = make_shared<ParamsProcessorImu>();
-        prc_imu_params->max_time_span   = 0.5;
-        prc_imu_params->max_buff_length = 10;
-        prc_imu_params->dist_traveled   = 5;
-        prc_imu_params->angle_turned    = 0.5;
-        processor_motion_ptr_ = ProcessorBase::emplace<ProcessorImu>(sensor_ptr, prc_imu_params);
-
-    // Time and data variables
-        TimeStamp t;
-        Vector6d data_=Vector6d::Zero();
-
-        t.set(0);
-
-    // Set the origin
-        // VectorComposite x0("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-        // VectorComposite s0("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-        // origin_frame = problem->setPriorFactor(x0, s0, t);
-        SpecComposite problem_prior{{'P', SpecState("StatePoint3d",  Vector3d::Zero(), "factor", Vector3d(1,1,1))},
-                                    {'O', SpecState("StateQuaternion",Quaterniond::Identity().coeffs(), "factor", Vector3d(1,1,1))},
-                                    {'V', SpecState("StateVector3d", Vector3d::Zero(), "factor", Vector3d(1,1,1))}};
-        origin_frame = problem->setPrior(problem_prior, t);
-        processor_motion_ptr_->setOrigin(origin_frame);
-
-    // Emplace one capture to store the Imu data arriving from (sensor / callback / file / etc.)
-    // give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
-        imu_ptr = CaptureBase::emplace<CaptureImu>(origin_frame,
-                                                   t,
-                                                   sensor_ptr,
-                                                   data_,
-                                                   Matrix6d::Identity(),
-                                                   Vector6d::Zero().eval());
-
-    //process data
-        data_ << 2, 0, 9.8, 0, 0, 0;
-        t.set(0.1);
-        // Expected state after one integration
-        //x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
-    // assign data to capture
-        imu_ptr->setData(data_);
-        imu_ptr->setTimeStamp(t);
-    // process data in capture
-        sensor_ptr->process(imu_ptr);
-
-    //emplace Frame
-        ts          = problem->getTimeStamp();
-        state_vec   = problem->getState().vector("POV");
-        last_frame  = problem->emplaceFrame(ts, "POV", state_vec);
-
-    //emplace a feature
-        delta_preint            = processor_motion_ptr_->getMotion().delta_integr_;
-        delta_preint_cov        = processor_motion_ptr_->getMotion().delta_integr_cov_ + MatrixXd::Identity(9,9)*1e-08;
-        VectorXd calib_preint   = processor_motion_ptr_->getLast()->getCalibrationPreint();
-        dD_db_jacobians         = processor_motion_ptr_->getMotion().jacobian_calib_;
-        feat_imu                = FeatureBase::emplace<FeatureImu>(imu_ptr,
-                                                                   delta_preint,
-                                                                   delta_preint_cov,
-                                                                   calib_preint,
-                                                                   dD_db_jacobians,
-                                                                   imu_ptr) ;
-    }
-
-    void TearDown() override
-    {
-        // code here will be called just after the test completes
-        // ok to through exceptions from here if need be
-        /*
-            You can do deallocation of resources in TearDown or the destructor routine.
-                However, if you want exception handling you must do it only in the TearDown code because throwing an exception
-                from the destructor results in undefined behavior.
-            The Google assertion macros may throw exceptions in platforms where they are enabled in future releases.
-                Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
-        */
-    }
-};
-
-TEST_F(FeatureImu_test, check_frame)
-{
-    // set variables
-    FrameBasePtr left_frame = feat_imu->getFrame();
-    TimeStamp t;
-    left_frame->getTimeStamp(t);
-    origin_frame->getTimeStamp(ts);
-
-    ASSERT_NEAR(t.get(), ts.get(), Constants::EPS_SMALL) << "t != ts \t t=" << t << "\t ts=" << ts << std::endl;
-
-    StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr;
-    origin_pptr = origin_frame->getP();
-    origin_optr = origin_frame->getO();
-    origin_vptr = origin_frame->getV();
-    left_pptr = left_frame->getP();
-    left_optr = left_frame->getO();
-    left_vptr = left_frame->getV();
-
-    ASSERT_MATRIX_APPROX(origin_pptr->getState(), left_pptr->getState(), Constants::EPS_SMALL);
-    Map<const Quaterniond> origin_Quat(origin_optr->getState().data()), left_Quat(left_optr->getState().data());
-    ASSERT_QUATERNION_APPROX(origin_Quat, left_Quat, Constants::EPS_SMALL);
-    ASSERT_MATRIX_APPROX(origin_vptr->getState(), left_vptr->getState(), Constants::EPS_SMALL);
-
-    ASSERT_EQ(origin_frame->id(), left_frame->id());
-}
-
-TEST_F(FeatureImu_test, access_members)
-{
-    VectorXd delta(10);
-    //dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2; dz = 0.5*9.8*0.1^2 = 0.049; dvz = 9.8*0.1 = 0.98
-    delta << 0.01,0,0.049, 0,0,0,1, 0.2,0,0.98;
-    ASSERT_MATRIX_APPROX(feat_imu->getMeasurement().transpose(), delta.transpose(), Constants::EPS);
-}
-
-//TEST_F(FeatureImu_test, addFactor)
-//{
-//    FrameBasePtr frm_imu = static_pointer_cast<FrameBase>(last_frame);
-//    auto cap_imu = last_frame->getCaptureList().back();
-//    FactorImuPtr factor_imu = make_shared<FactorImu>(feat_imu, static_pointer_cast<CaptureImu>(cap_imu), processor_ptr_);
-//    feat_imu->addFactor(factor_imu);
-//    origin_frame->addConstrainedBy(factor_imu);
-//}
-
-int main(int argc, char **argv)
-{
-  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_feature_imu_3d.cpp b/test/gtest_feature_imu_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ad881c79d868b4aaeaa5c5f774422aa2bb0a27d5
--- /dev/null
+++ b/test/gtest_feature_imu_3d.cpp
@@ -0,0 +1,173 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/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/>.
+
+// Wolf
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu_3d.h"
+#include "imu/sensor/sensor_imu.h"
+
+#include "core/common/wolf.h"
+#include "core/problem/problem.h"
+#include "core/state_block/state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+using namespace wolf;
+using namespace Eigen;
+using std::make_shared;
+using std::static_pointer_cast;
+
+std::string imu_dir         = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
+
+class FeatureImu3d_test : public testing::Test
+{
+  public:
+    ProblemPtr           problem;
+    TimeStamp            ts;
+    CaptureImuPtr        capture_imu;
+    VectorXd             state_vec;
+    VectorXd             delta_preint;
+    Matrix<double, 9, 9> delta_preint_cov;
+    FeatureImu3dPtr      feat_imu;
+    FrameBasePtr         last_frame;
+    FrameBasePtr         origin_frame;
+    MatrixXd             dD_db_jacobians;
+    ProcessorMotionPtr   processor;
+
+    // a new of this will be created for each new test
+    void SetUp() override
+    {
+        // // Wolf problem
+        // problem = Problem::create("POV", 3);
+        // SpecSensorComposite imu_priors{
+        //     {'P', SpecStateSensor("P", Vector3d::Zero())},
+        //     {'O', SpecStateSensor("O", (Vector4d() << 0, 0, 0, 1).finished())},
+        //     {'I', SpecStateSensor("StateBlock", Vector6d::Zero(), "factor", Vector6d::Ones(), true)}};
+        // ParamsSensorImuPtr sen_imu_params = make_shared<ParamsSensorImu>();
+        // SensorBasePtr      sensor_ptr =
+        //     SensorBase::emplace<SensorImu3d>(problem->getHardware(), sen_imu_params, imu_priors);
+        // ParamsProcessorImuPtr prc_imu_params = make_shared<ParamsProcessorImu>();
+        // prc_imu_params->max_time_span        = 0.5;
+        // prc_imu_params->max_buff_length      = 10;
+        // prc_imu_params->dist_traveled        = 5;
+        // prc_imu_params->angle_turned         = 0.5;
+        // processor                = ProcessorBase::emplace<ProcessorImu3d>(sensor_ptr, prc_imu_params);
+
+        // // Time and data variables
+        // TimeStamp t;
+        // Vector6d  data_ = Vector6d::Zero();
+
+        // t.set(0);
+
+        // // Set the origin
+        // // VectorComposite x0("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
+        // // VectorComposite s0("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+        // // origin_frame = problem->setPriorFactor(x0, s0, t);
+        // SpecComposite problem_prior{
+        //     {'P', SpecState("StatePoint3d", Vector3d::Zero(), "factor", Vector3d(1, 1, 1))},
+        //     {'O', SpecState("StateQuaternion", Quaterniond::Identity().coeffs(), "factor", Vector3d(1, 1, 1))},
+        //     {'V', SpecState("StateVector3d", Vector3d::Zero(), "factor", Vector3d(1, 1, 1))}};
+        // origin_frame = problem->setPrior(problem_prior, t);
+        // processor->setOrigin(origin_frame);
+
+        // Wolf problem
+        problem = Problem::autoSetup(imu_dir + "/test/yaml/problem_3d_gtest_feature_imu.yaml", {imu_dir});
+
+        // sensor imu
+        auto sensor = problem->findSensor("cool sensor imu");
+
+        // processor imu
+        auto processor = std::static_pointer_cast<ProcessorMotion>(sensor->getProcessorList().front());
+
+        // Set the origin at 0
+        origin_frame = problem->applyPriorOptions(TimeStamp(0));
+        processor->setOrigin(origin_frame);
+
+        // Emplace one capture to store the Imu data arriving from (sensor / callback / file / etc.)
+        // give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
+        Vector6d data_;
+        data_ << 2, 0, 9.8, 0, 0, 0;
+        capture_imu = CaptureBase::emplace<CaptureImu>(
+            origin_frame, TimeStamp(0.1), sensor, data_, Matrix6d::Identity(), Vector6d::Zero().eval());
+
+        // Expected state after one integration
+        // x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s
+        // dx = 0.5*2*0.1^2 = 0.01
+        // dvx = 2*0.1 = 0.2
+
+        // process data in capture
+        sensor->process(capture_imu);
+
+        // emplace Frame
+        ts         = problem->getTimeStamp();
+        state_vec  = problem->getState().vector("POV");
+        last_frame = problem->emplaceFrame(ts, "POV", state_vec);
+
+        // emplace a feature
+        delta_preint          = processor->getMotion().delta_integr_;
+        delta_preint_cov      = processor->getMotion().delta_integr_cov_ + MatrixXd::Identity(9, 9) * 1e-08;
+        VectorXd calib_preint = processor->getLast()->getCalibrationPreint();
+        dD_db_jacobians       = processor->getMotion().jacobian_calib_;
+        feat_imu              = FeatureBase::emplace<FeatureImu3d>(
+            capture_imu, delta_preint, delta_preint_cov, calib_preint, dD_db_jacobians, capture_imu);
+    }
+};
+
+TEST_F(FeatureImu3d_test, check_frame)
+{
+    // set variables
+    FrameBasePtr left_frame = feat_imu->getFrame();
+    TimeStamp    t;
+    left_frame->getTimeStamp(t);
+    origin_frame->getTimeStamp(ts);
+
+    ASSERT_NEAR(t.get(), ts.get(), Constants::EPS_SMALL) << "t != ts \t t=" << t << "\t ts=" << ts << std::endl;
+
+    StateBlockPtr origin_p, origin_o, origin_v, left_p, left_o, left_v;
+    origin_p = origin_frame->getP();
+    origin_o = origin_frame->getO();
+    origin_v = origin_frame->getV();
+    left_p   = left_frame->getP();
+    left_o   = left_frame->getO();
+    left_v   = left_frame->getV();
+
+    ASSERT_MATRIX_APPROX(origin_p->getState(), left_p->getState(), Constants::EPS_SMALL);
+    Map<const Quaterniond> origin_Quat(origin_o->getState().data()), left_Quat(left_o->getState().data());
+    ASSERT_QUATERNION_APPROX(origin_Quat, left_Quat, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(origin_v->getState(), left_v->getState(), Constants::EPS_SMALL);
+
+    ASSERT_EQ(origin_frame->id(), left_frame->id());
+}
+
+TEST_F(FeatureImu3d_test, access_members)
+{
+    VectorXd delta(10);
+    // dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2; dz = 0.5*9.8*0.1^2 = 0.049; dvz = 9.8*0.1 = 0.98
+    delta << 0.01, 0, 0.049, 0, 0, 0, 1, 0.2, 0, 0.98;
+    ASSERT_MATRIX_APPROX(feat_imu->getMeasurement().transpose(), delta.transpose(), Constants::EPS);
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp
deleted file mode 100644
index 19a2f77040d6efea26fd3958a2756bd933a7e837..0000000000000000000000000000000000000000
--- a/test/gtest_imu2d_static_init.cpp
+++ /dev/null
@@ -1,541 +0,0 @@
-// WOLF - Copyright (C) 2020,2021,2022,2023
-// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
-// Joan Vallvé Navarro (jvallve@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF: http://www.iri.upc.edu/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/>.
-
-#include <fstream>
-#include <core/utils/utils_gtest.h>
-#include "imu/internal/config.h"
-
-#include "imu/factor/factor_imu2d.h"
-#include "imu/math/imu2d_tools.h"
-#include "imu/sensor/sensor_imu.h"
-#include "imu/processor/processor_imu2d.h"
-#include "core/capture/capture_void.h"
-#include <core/ceres_wrapper/solver_ceres.h>
-#include <core/factor/factor_relative_pose_2d.h>
-
-using namespace Eigen;
-using namespace wolf;
-using std::make_shared;
-
-std::string wolf_root = _WOLF_IMU_CODE_DIR;
-
-class ProcessorImu2dStaticInitTest : public testing::Test
-{
-    public: //These can be accessed in fixtures
-        ProblemPtr problem_ptr_;
-        SensorBasePtr sensor_ptr_;
-        ProcessorMotionPtr processor_motion_;
-        TimeStamp t;
-        TimeStamp t0;
-        double dt;
-        Vector6d data;
-        Matrix6d data_cov;
-        FrameBasePtr KF0_;
-        FrameBasePtr first_frame_;
-        FrameBasePtr last_frame_;
-        SolverCeresPtr solver_;
-
-    //a new of this will be created for each new test
-    void SetUp() override
-    {
-        WOLF_INFO("Doing setup...");
-
-        // Wolf problem
-        problem_ptr_ = Problem::create("POV", 2);
-        sensor_ptr_ = problem_ptr_->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d.yaml", {wolf_root});
-        processor_motion_ = std::static_pointer_cast<ProcessorMotion>(problem_ptr_->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu2d_static_init.yaml", {wolf_root}));
-
-        // Time and data variables
-        dt = 0.1;
-        t0.set(0);
-        t = t0;
-        data = Vector6d::Random();
-        data_cov = Matrix6d::Identity();
-        last_frame_ = nullptr;
-        first_frame_ = nullptr;
-
-        // Create the solver
-        solver_ = make_shared<SolverCeres>(problem_ptr_);
-        
-        // Set the origin
-        // VectorComposite x_origin("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()});
-        // VectorComposite std_origin("POV", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones(), 0.001*Vector2d::Ones()});
-        // //KF0_ = problem_ptr_->setPriorFix(x_origin, t0);
-        // KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0);
-        SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d::Zero(), "factor", Vector2d::Constant(0.001))},
-                                    {'O', SpecState("StateAngle",    Vector1d::Zero(), "factor", Vector1d::Constant(0.001))},
-                                    {'V', SpecState("StateVector2d", Vector2d::Zero(), "factor", Vector2d::Constant(0.001))}};
-        KF0_ = problem_ptr_->setPrior(problem_prior, t0);
-    }
-
-    void TestStatic(const Vector3d& body_magnitudes, const Vector3d& bias_groundtruth, const Vector3d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
-    {
-      //Data
-      data.head(2) = body_magnitudes.head(2);
-      data(5) = body_magnitudes(2);
-      data.head(2) += bias_groundtruth.head(2);
-      data(5) += bias_groundtruth(2);
-
-      //Set bias initial guess
-      sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
-      processor_motion_->setOrigin(KF0_);
-
-#if WRITE_CSV_FILE
-      std::fstream file_est; 
-      file_est.open("./est2d-"+test_name+".csv", std::fstream::out);
-      //    std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
-      std::string header_est;
-      if(testing_var == 0) header_est = "t;px;vx;o;bax_est;bg_est;bax_preint;bg_preint\n";
-      if(testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n";
-      //if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
-      if(testing_var == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n";
-      file_est << header_est;
-#endif
-
-
-      int n_frames = 0;
-      for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
-        WOLF_INFO("\n------------------------------------------------------------------------");
-
-        //Create and process capture  
-        auto C = make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
-        C->process();
-
-        auto state = problem_ptr_->getState();
-        auto bias_est = sensor_ptr_->getIntrinsic()->getState();
-        auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
-
-#if WRITE_CSV_FILE
-        // pre-solve print to CSV
-        if(testing_var == 3){
-        file_est << std::fixed << t << ";"
-            << state['P'].norm() << ";"
-            << state['V'].norm() << ";"
-            << state['O'] << ";"
-            << bias_est.head(2).norm() << ";"
-            << bias_est(2) << ";"
-            << bias_preint.head(2).norm() << ";"
-            << bias_preint(2) << "\n";
-        }
-        else
-        {
-        file_est << std::fixed << t << ";"
-            << state['P'](testing_var) << ";"
-            << state['V'](testing_var) << ";"
-            << state['O'] << ";"
-            << bias_est(testing_var) << ";"
-            << bias_est(2) << ";"
-            << bias_preint(testing_var) << ";"
-            << bias_preint(2) << "\n";
-
-        }
-#endif
-
-        // new frame
-        if (last_frame_ != processor_motion_->getOrigin()->getFrame())
-        {
-          n_frames++;
-          last_frame_ = processor_motion_->getOrigin()->getFrame();
-
-          // impose static
-          last_frame_->getP()->setState(KF0_->getP()->getState());
-          last_frame_->getO()->setState(KF0_->getO()->getState());
-          last_frame_->getV()->setZero();
-
-          //Fix frame
-          last_frame_->fix();
-
-          //Solve
-          if(n_frames > 0)
-          {
-            std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
-            //WOLF_INFO("Solver Report: ", report);
-
-            state = problem_ptr_->getState();
-            bias_est = sensor_ptr_->getIntrinsic()->getState();
-            bias_preint = processor_motion_->getLast()->getCalibrationPreint();
-            
-            WOLF_INFO("The current problem is:");
-            problem_ptr_->print(4);
-
-#if WRITE_CSV_FILE
-            // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
-            if(testing_var == 3){
-              file_est << std::fixed << t+dt/2 << ";"
-                << state['P'].norm() << ";"
-                << state['V'].norm() << ";"
-                << state['O'] << ";"
-                << bias_est.head(2).norm() << ";"
-                << bias_est(2) << ";"
-                << bias_preint.head(2).norm() << ";"
-                << bias_preint(2) << "\n";
-            }
-            else
-            {
-              file_est << std::fixed << t+dt/2 << ";"
-                << state['P'](testing_var) << ";"
-                << state['V'](testing_var) << ";"
-                << state['O'] << ";"
-                << bias_est(testing_var) << ";"
-                << bias_est(2) << ";"
-                << bias_preint(testing_var) << ";"
-                << bias_preint(2) << "\n";
-
-            }
-#endif
-          }
-
-        }
-
-
-        WOLF_INFO("Number of frames is ", n_frames);
-        WOLF_INFO("The state is: ", state);
-        WOLF_INFO("The estimated bias is: ", bias_est.transpose());
-        WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
-        if(small_tol)
-        {
-          if(n_frames == 2)
-          {
-            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
-            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
-          }
-          else if (n_frames > 2)
-          {
-            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
-            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
-            EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
-          }
-        }
-        else
-        {
-          if(n_frames == 2)
-          {
-            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
-            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
-          }
-          else if (n_frames > 2)
-          {
-            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
-            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
-            EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
-          }
-        }
-      }
-
-#if WRITE_CSV_FILE
-      file_est.close();
-#endif
-
-    }
-
-    void TestStaticZeroOdom(const Vector3d& body_magnitudes, const Vector3d& bias_groundtruth, const Vector3d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
-    {
-      //Data
-      data.head(2) = body_magnitudes.head(2);
-      data(5) = body_magnitudes(2);
-      data.head(2) += bias_groundtruth.head(2);
-      data(5) += bias_groundtruth(2);
-
-      //Set bias initial guess
-      sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
-      processor_motion_->setOrigin(KF0_);
-
-#if WRITE_CSV_FILE
-      std::fstream file_est; 
-      file_est.open("./est2dzeroodom-"+test_name+".csv", std::fstream::out);
-      //    std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
-      std::string header_est;
-      if(testing_var == 0) header_est = "t;px;vx;o;bax_est;bg_est;bax_preint;bg_preint\n";
-      if(testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n";
-      //if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
-      if(testing_var == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n";
-      file_est << header_est;
-#endif
-
-      int n_frames = 0;
-      for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
-        WOLF_INFO("\n------------------------------------------------------------------------");
-
-        //Create and process capture  
-        auto C = make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
-        C->process();
-
-        auto state = problem_ptr_->getState();
-        auto bias_est = sensor_ptr_->getIntrinsic()->getState();
-        auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
-
-#if WRITE_CSV_FILE
-        // pre-solve print to CSV
-        if(testing_var == 3){
-        file_est << std::fixed << t << ";"
-            << state['P'].norm() << ";"
-            << state['V'].norm() << ";"
-            << state['O'] << ";"
-            << bias_est.head(2).norm() << ";"
-            << bias_est(2) << ";"
-            << bias_preint.head(2).norm() << ";"
-            << bias_preint(2) << "\n";
-        }
-        else
-        {
-        file_est << std::fixed << t << ";"
-            << state['P'](testing_var) << ";"
-            << state['V'](testing_var) << ";"
-            << state['O'] << ";"
-            << bias_est(testing_var) << ";"
-            << bias_est(2) << ";"
-            << bias_preint(testing_var) << ";"
-            << bias_preint(2) << "\n";
-
-        }
-#endif
-
-        // new frame
-        if (last_frame_ != processor_motion_->getOrigin()->getFrame())
-        {
-          n_frames++;
-          last_frame_ = processor_motion_->getOrigin()->getFrame();
-          
-          // impose zero odometry
-          processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateKeys()));
-
-          // add zero displacement and rotation capture & feature & factor with all previous frames
-          assert(sensor_ptr_->getProblem());
-          for (auto frm_pair : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap())
-          {
-            if (frm_pair.second == last_frame_)
-              break;
-            auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
-
-            auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
-                "FeatureZeroOdom",
-                Vector3d::Zero(),
-                MatrixXd::Identity(3,3) * 0.01);
-
-            FactorBase::emplace<FactorRelativePose2d>(feature_zero,
-                feature_zero,
-                frm_pair.second,
-                nullptr,
-                false,
-                TOP_MOTION);
-
-          }
-
-          // impose static
-          last_frame_->getV()->setZero();
-
-          //Fix frame
-          last_frame_->getV()->fix();
-
-          //Solve
-          if(n_frames > 0)
-          {
-            std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
-            //WOLF_INFO("Solver Report: ", report);
-
-            state = problem_ptr_->getState();
-            bias_est = sensor_ptr_->getIntrinsic()->getState();
-            bias_preint = processor_motion_->getLast()->getCalibrationPreint();
-            
-            WOLF_INFO("The current problem is:");
-            problem_ptr_->print(4);
-
-#if WRITE_CSV_FILE
-            // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
-            if(testing_var == 3){
-              file_est << std::fixed << t+dt/2 << ";"
-                << state['P'].norm() << ";"
-                << state['V'].norm() << ";"
-                << state['O'] << ";"
-                << bias_est.head(2).norm() << ";"
-                << bias_est(2) << ";"
-                << bias_preint.head(2).norm() << ";"
-                << bias_preint(2) << "\n";
-            }
-            else
-            {
-              file_est << std::fixed << t+dt/2 << ";"
-                << state['P'](testing_var) << ";"
-                << state['V'](testing_var) << ";"
-                << state['O'] << ";"
-                << bias_est(testing_var) << ";"
-                << bias_est(2) << ";"
-                << bias_preint(testing_var) << ";"
-                << bias_preint(2) << "\n";
-
-            }
-#endif
-          }
-
-        }
-
-
-
-        WOLF_INFO("Number of frames is ", n_frames);
-        WOLF_INFO("The state is: ", state);
-        WOLF_INFO("The estimated bias is: ", bias_est.transpose());
-        WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
-        if(small_tol)
-        {
-          if(n_frames == 2)
-          {
-            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
-            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
-          }
-          else if (n_frames > 2)
-          {
-            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
-            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
-            EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
-          }
-        }
-        else
-        {
-          if(n_frames == 2)
-          {
-            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
-            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
-          }
-          else if (n_frames > 2)
-          {
-            EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
-            EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
-            EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
-          }
-        }
-      }
-
-#if WRITE_CSV_FILE
-      file_est.close();
-#endif
-
-    }
-};
-
-
-TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero)
-{
-   Vector3d body_magnitudes = Vector3d::Zero();
-   Vector3d bias_groundtruth = (Vector3d() << 0.42, 0, 0).finished(); 
-   Vector3d bias_initial_guess = Vector3d::Zero();
-
-   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true); 
-}
-
-TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX)
-{
-   Vector3d body_magnitudes = Vector3d::Zero();
-   Vector3d bias_groundtruth = Vector3d::Zero();
-   Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished(); 
-
-   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); 
-}
-
-TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero)
-{
-   Vector3d body_magnitudes = Vector3d::Zero();
-   Vector3d bias_groundtruth = (Vector3d() << 0, 0, 0.01).finished(); 
-   Vector3d bias_initial_guess = Vector3d::Zero();
-
-   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); 
-}
-
-TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX)
-{
-   Vector3d body_magnitudes = Vector3d::Zero();
-   Vector3d bias_groundtruth = Vector3d::Zero();
-   Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished(); 
-
-   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); 
-}
-
-TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero_zeroodom)
-{
-   Vector3d body_magnitudes = Vector3d::Zero();
-   Vector3d bias_groundtruth = (Vector3d() << 0.42, 0, 0).finished(); 
-   Vector3d bias_initial_guess = Vector3d::Zero();
-
-   TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true); 
-}
-
-TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX_zeroodom)
-{
-   Vector3d body_magnitudes = Vector3d::Zero();
-   Vector3d bias_groundtruth = Vector3d::Zero();
-   Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished(); 
-
-   TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); 
-}
-
-TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero_zeroodom)
-{
-   Vector3d body_magnitudes = Vector3d::Zero();
-   Vector3d bias_groundtruth = (Vector3d() <<  0, 0, 0.01).finished(); 
-   Vector3d bias_initial_guess = Vector3d::Zero();
-
-   TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); 
-}
-
-TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX_zeroodom)
-{
-   Vector3d body_magnitudes = Vector3d::Zero();
-   Vector3d bias_groundtruth = Vector3d::Zero();
-   Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished(); 
-
-   TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); 
-}
-
-
-TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_a_random)
-{
-   Vector3d body_magnitudes = Vector3d::Zero();
-   Vector3d bias_groundtruth = Vector3d::Zero();
-   Vector3d bias_initial_guess = Vector3d::Random()*100;
-   bias_initial_guess(2) = 0;
-
-   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true); 
-}
-
-TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_random)
-{
-   Vector3d body_magnitudes = Vector3d::Zero();
-   Vector3d bias_groundtruth = Vector3d::Zero();
-   Vector3d bias_initial_guess = Vector3d::Random()*0.01;
-
-   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false); 
-}
-
-TEST_F(ProcessorImu2dStaticInitTest, realistic_test)
-{
-   Vector3d body_magnitudes = Vector3d::Zero();
-   Vector3d bias_groundtruth = (Vector3d() << -0.529550648247,
-                                               0.278316717683,
-                                              -0.00122491355676).finished();
-   Vector3d bias_initial_guess = Vector3d::Zero();
-
-   TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false); 
-}
-
-int main(int argc, char **argv)
-{
-    testing::InitGoogleTest(&argc, argv);
-    //    ::testing::GTEST_FLAG(filter) = "FactorImu2d.no_bias_fixed"; // Test only this one
-    return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_imu_2d_static_init.cpp b/test/gtest_imu_2d_static_init.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..39abfbdd217e46987a1294ddf5e183b29221e678
--- /dev/null
+++ b/test/gtest_imu_2d_static_init.cpp
@@ -0,0 +1,506 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/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/>.
+
+#include <fstream>
+#include <core/utils/utils_gtest.h>
+#include "imu/internal/config.h"
+
+#include "imu/factor/factor_imu_2d.h"
+#include "imu/math/imu_2d_tools.h"
+#include "imu/sensor/sensor_imu.h"
+#include "imu/processor/processor_imu_2d.h"
+#include "core/capture/capture_void.h"
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/factor/factor_relative_pose_2d.h>
+
+using namespace Eigen;
+using namespace wolf;
+using std::make_shared;
+
+std::string imu_dir         = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
+
+class ProcessorImu2dStaticInitTest : public testing::Test
+{
+  public:  // These can be accessed in fixtures
+    ProblemPtr         problem_ptr_;
+    SensorBasePtr      sensor_ptr_;
+    ProcessorMotionPtr processor_motion_;
+    TimeStamp          t;
+    TimeStamp          t0;
+    double             dt;
+    Vector6d           data;
+    Matrix6d           data_cov;
+    FrameBasePtr       KF0_;
+    FrameBasePtr       first_frame_;
+    FrameBasePtr       last_frame_;
+    SolverCeresPtr     solver_;
+
+    // a new of this will be created for each new test
+    void SetUp() override
+    {
+        WOLF_INFO("Doing setup...");
+
+        // Wolf problem
+        problem_ptr_ = Problem::create("POV", 2);
+        sensor_ptr_ =
+            problem_ptr_->installSensor(imu_dir + "/test/yaml/sensor_imu_2d.yaml", {imu_dir, wolf_schema_dir});
+        processor_motion_ = std::static_pointer_cast<ProcessorMotion>(problem_ptr_->installProcessor(
+            imu_dir + "/test/yaml/processor_imu_2d_static_init.yaml", {imu_dir, wolf_schema_dir}));
+
+        // Time and data variables
+        dt = 0.1;
+        t0.set(0);
+        t            = t0;
+        data         = Vector6d::Random();
+        data_cov     = Matrix6d::Identity();
+        last_frame_  = nullptr;
+        first_frame_ = nullptr;
+
+        // Create the solver
+        solver_ = make_shared<SolverCeres>(problem_ptr_);
+
+        // Set the origin
+        // VectorComposite x_origin("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()});
+        // VectorComposite std_origin("POV", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones(), 0.001*Vector2d::Ones()});
+        // //KF0_ = problem_ptr_->setPriorFix(x_origin, t0);
+        // KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0);
+        SpecComposite problem_prior{
+            {'P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(0.001))},
+            {'O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(0.001))},
+            {'V', SpecState("StateVector2d", Vector2d::Zero(), "factor", Vector2d::Constant(0.001))}};
+        KF0_ = problem_ptr_->setPrior(problem_prior, t0);
+    }
+
+    void TestStatic(const Vector3d& body_magnitudes,
+                    const Vector3d& bias_groundtruth,
+                    const Vector3d& bias_initial_guess,
+                    const string&   test_name,
+                    int             testing_var,
+                    bool            small_tol)
+    {
+        // Data
+        data.head(2) = body_magnitudes.head(2);
+        data(5)      = body_magnitudes(2);
+        data.head(2) += bias_groundtruth.head(2);
+        data(5) += bias_groundtruth(2);
+
+        // Set bias initial guess
+        sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+        processor_motion_->setOrigin(KF0_);
+
+#if WRITE_CSV_FILE
+        std::fstream file_est;
+        file_est.open("./est2d-" + test_name + ".csv", std::fstream::out);
+        //    std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
+        std::string header_est;
+        if (testing_var == 0) header_est = "t;px;vx;o;bax_est;bg_est;bax_preint;bg_preint\n";
+        if (testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n";
+        // if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
+        if (testing_var == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n";
+        file_est << header_est;
+#endif
+
+        int n_frames = 0;
+        for (t = t0; t <= t0 + 9.9 + dt / 2; t += dt)
+        {
+            WOLF_INFO("\n------------------------------------------------------------------------");
+
+            // Create and process capture
+            auto C = make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+            C->process();
+
+            auto state       = problem_ptr_->getState();
+            auto bias_est    = sensor_ptr_->getIntrinsic()->getState();
+            auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+#if WRITE_CSV_FILE
+            // pre-solve print to CSV
+            if (testing_var == 3)
+            {
+                file_est << std::fixed << t << ";" << state['P'].norm() << ";" << state['V'].norm() << ";"
+                         << state['O'] << ";" << bias_est.head(2).norm() << ";" << bias_est(2) << ";"
+                         << bias_preint.head(2).norm() << ";" << bias_preint(2) << "\n";
+            }
+            else
+            {
+                file_est << std::fixed << t << ";" << state['P'](testing_var) << ";" << state['V'](testing_var) << ";"
+                         << state['O'] << ";" << bias_est(testing_var) << ";" << bias_est(2) << ";"
+                         << bias_preint(testing_var) << ";" << bias_preint(2) << "\n";
+            }
+#endif
+
+            // new frame
+            if (last_frame_ != processor_motion_->getOrigin()->getFrame())
+            {
+                n_frames++;
+                last_frame_ = processor_motion_->getOrigin()->getFrame();
+
+                // impose static
+                last_frame_->getP()->setState(KF0_->getP()->getState());
+                last_frame_->getO()->setState(KF0_->getO()->getState());
+                last_frame_->getV()->setZero();
+
+                // Fix frame
+                last_frame_->fix();
+
+                // Solve
+                if (n_frames > 0)
+                {
+                    std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+                    // WOLF_INFO("Solver Report: ", report);
+
+                    state       = problem_ptr_->getState();
+                    bias_est    = sensor_ptr_->getIntrinsic()->getState();
+                    bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+                    WOLF_INFO("The current problem is:");
+                    problem_ptr_->print(4);
+
+#if WRITE_CSV_FILE
+                    // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
+                    if (testing_var == 3)
+                    {
+                        file_est << std::fixed << t + dt / 2 << ";" << state['P'].norm() << ";" << state['V'].norm()
+                                 << ";" << state['O'] << ";" << bias_est.head(2).norm() << ";" << bias_est(2) << ";"
+                                 << bias_preint.head(2).norm() << ";" << bias_preint(2) << "\n";
+                    }
+                    else
+                    {
+                        file_est << std::fixed << t + dt / 2 << ";" << state['P'](testing_var) << ";"
+                                 << state['V'](testing_var) << ";" << state['O'] << ";" << bias_est(testing_var) << ";"
+                                 << bias_est(2) << ";" << bias_preint(testing_var) << ";" << bias_preint(2) << "\n";
+                    }
+#endif
+                }
+            }
+
+            WOLF_INFO("Number of frames is ", n_frames);
+            WOLF_INFO("The state is: ", state);
+            WOLF_INFO("The estimated bias is: ", bias_est.transpose());
+            WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
+            if (small_tol)
+            {
+                if (n_frames == 2)
+                {
+                    EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+                    EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+                }
+                else if (n_frames > 2)
+                {
+                    EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+                    EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+                    EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+                }
+            }
+            else
+            {
+                if (n_frames == 2)
+                {
+                    EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+                    EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+                }
+                else if (n_frames > 2)
+                {
+                    EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+                    EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+                    EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+                }
+            }
+        }
+
+#if WRITE_CSV_FILE
+        file_est.close();
+#endif
+    }
+
+    void TestStaticZeroOdom(const Vector3d&    body_magnitudes,
+                            const Vector3d&    bias_groundtruth,
+                            const Vector3d&    bias_initial_guess,
+                            const std::string& test_name,
+                            int                testing_var,
+                            bool               small_tol)
+    {
+        // Data
+        data.head(2) = body_magnitudes.head(2);
+        data(5)      = body_magnitudes(2);
+        data.head(2) += bias_groundtruth.head(2);
+        data(5) += bias_groundtruth(2);
+
+        // Set bias initial guess
+        sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+        processor_motion_->setOrigin(KF0_);
+
+#if WRITE_CSV_FILE
+        std::fstream file_est;
+        file_est.open("./est2dzeroodom-" + test_name + ".csv", std::fstream::out);
+        //    std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
+        std::string header_est;
+        if (testing_var == 0) header_est = "t;px;vx;o;bax_est;bg_est;bax_preint;bg_preint\n";
+        if (testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n";
+        // if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
+        if (testing_var == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n";
+        file_est << header_est;
+#endif
+
+        int n_frames = 0;
+        for (t = t0; t <= t0 + 9.9 + dt / 2; t += dt)
+        {
+            WOLF_INFO("\n------------------------------------------------------------------------");
+
+            // Create and process capture
+            auto C = make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+            C->process();
+
+            auto state       = problem_ptr_->getState();
+            auto bias_est    = sensor_ptr_->getIntrinsic()->getState();
+            auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+#if WRITE_CSV_FILE
+            // pre-solve print to CSV
+            if (testing_var == 3)
+            {
+                file_est << std::fixed << t << ";" << state['P'].norm() << ";" << state['V'].norm() << ";"
+                         << state['O'] << ";" << bias_est.head(2).norm() << ";" << bias_est(2) << ";"
+                         << bias_preint.head(2).norm() << ";" << bias_preint(2) << "\n";
+            }
+            else
+            {
+                file_est << std::fixed << t << ";" << state['P'](testing_var) << ";" << state['V'](testing_var) << ";"
+                         << state['O'] << ";" << bias_est(testing_var) << ";" << bias_est(2) << ";"
+                         << bias_preint(testing_var) << ";" << bias_preint(2) << "\n";
+            }
+#endif
+
+            // new frame
+            if (last_frame_ != processor_motion_->getOrigin()->getFrame())
+            {
+                n_frames++;
+                last_frame_ = processor_motion_->getOrigin()->getFrame();
+
+                // impose zero odometry
+                processor_motion_->setOdometry(
+                    sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateKeys()));
+
+                // add zero displacement and rotation capture & feature & factor with all previous frames
+                assert(sensor_ptr_->getProblem());
+                for (auto frm_pair : sensor_ptr_->getProblem()->getTrajectory()->getFrameMap())
+                {
+                    if (frm_pair.second == last_frame_) break;
+                    auto capture_zero =
+                        CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
+
+                    auto feature_zero = FeatureBase::emplace<FeatureBase>(
+                        capture_zero, "FeatureZeroOdom", Vector3d::Zero(), MatrixXd::Identity(3, 3) * 0.01);
+
+                    FactorBase::emplace<FactorRelativePose2d>(
+                        feature_zero, feature_zero, frm_pair.second, nullptr, false, TOP_MOTION);
+                }
+
+                // impose static
+                last_frame_->getV()->setZero();
+
+                // Fix frame
+                last_frame_->getV()->fix();
+
+                // Solve
+                if (n_frames > 0)
+                {
+                    std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+                    // WOLF_INFO("Solver Report: ", report);
+
+                    state       = problem_ptr_->getState();
+                    bias_est    = sensor_ptr_->getIntrinsic()->getState();
+                    bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+
+                    WOLF_INFO("The current problem is:");
+                    problem_ptr_->print(4);
+
+#if WRITE_CSV_FILE
+                    // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
+                    if (testing_var == 3)
+                    {
+                        file_est << std::fixed << t + dt / 2 << ";" << state['P'].norm() << ";" << state['V'].norm()
+                                 << ";" << state['O'] << ";" << bias_est.head(2).norm() << ";" << bias_est(2) << ";"
+                                 << bias_preint.head(2).norm() << ";" << bias_preint(2) << "\n";
+                    }
+                    else
+                    {
+                        file_est << std::fixed << t + dt / 2 << ";" << state['P'](testing_var) << ";"
+                                 << state['V'](testing_var) << ";" << state['O'] << ";" << bias_est(testing_var) << ";"
+                                 << bias_est(2) << ";" << bias_preint(testing_var) << ";" << bias_preint(2) << "\n";
+                    }
+#endif
+                }
+            }
+
+            WOLF_INFO("Number of frames is ", n_frames);
+            WOLF_INFO("The state is: ", state);
+            WOLF_INFO("The estimated bias is: ", bias_est.transpose());
+            WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
+            if (small_tol)
+            {
+                if (n_frames == 2)
+                {
+                    EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+                    EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+                }
+                else if (n_frames > 2)
+                {
+                    EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+                    EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+                    EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+                }
+            }
+            else
+            {
+                if (n_frames == 2)
+                {
+                    EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+                    EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+                }
+                else if (n_frames > 2)
+                {
+                    EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+                    EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+                    EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+                }
+            }
+        }
+
+#if WRITE_CSV_FILE
+        file_est.close();
+#endif
+    }
+};
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero)
+{
+    Vector3d body_magnitudes    = Vector3d::Zero();
+    Vector3d bias_groundtruth   = (Vector3d() << 0.42, 0, 0).finished();
+    Vector3d bias_initial_guess = Vector3d::Zero();
+
+    TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX)
+{
+    Vector3d body_magnitudes    = Vector3d::Zero();
+    Vector3d bias_groundtruth   = Vector3d::Zero();
+    Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished();
+
+    TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero)
+{
+    Vector3d body_magnitudes    = Vector3d::Zero();
+    Vector3d bias_groundtruth   = (Vector3d() << 0, 0, 0.01).finished();
+    Vector3d bias_initial_guess = Vector3d::Zero();
+
+    TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX)
+{
+    Vector3d body_magnitudes    = Vector3d::Zero();
+    Vector3d bias_groundtruth   = Vector3d::Zero();
+    Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished();
+
+    TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero_zeroodom)
+{
+    Vector3d body_magnitudes    = Vector3d::Zero();
+    Vector3d bias_groundtruth   = (Vector3d() << 0.42, 0, 0).finished();
+    Vector3d bias_initial_guess = Vector3d::Zero();
+
+    TestStaticZeroOdom(
+        body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX_zeroodom)
+{
+    Vector3d body_magnitudes    = Vector3d::Zero();
+    Vector3d bias_groundtruth   = Vector3d::Zero();
+    Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished();
+
+    TestStaticZeroOdom(
+        body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero_zeroodom)
+{
+    Vector3d body_magnitudes    = Vector3d::Zero();
+    Vector3d bias_groundtruth   = (Vector3d() << 0, 0, 0.01).finished();
+    Vector3d bias_initial_guess = Vector3d::Zero();
+
+    TestStaticZeroOdom(
+        body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX_zeroodom)
+{
+    Vector3d body_magnitudes    = Vector3d::Zero();
+    Vector3d bias_groundtruth   = Vector3d::Zero();
+    Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished();
+
+    TestStaticZeroOdom(
+        body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_a_random)
+{
+    Vector3d body_magnitudes    = Vector3d::Zero();
+    Vector3d bias_groundtruth   = Vector3d::Zero();
+    Vector3d bias_initial_guess = Vector3d::Random() * 100;
+    bias_initial_guess(2)       = 0;
+
+    TestStatic(
+        body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_random)
+{
+    Vector3d body_magnitudes    = Vector3d::Zero();
+    Vector3d bias_groundtruth   = Vector3d::Zero();
+    Vector3d bias_initial_guess = Vector3d::Random() * 0.01;
+
+    TestStatic(
+        body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, realistic_test)
+{
+    Vector3d body_magnitudes    = Vector3d::Zero();
+    Vector3d bias_groundtruth   = (Vector3d() << -0.529550648247, 0.278316717683, -0.00122491355676).finished();
+    Vector3d bias_initial_guess = Vector3d::Zero();
+
+    TestStatic(
+        body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false);
+}
+
+int main(int argc, char** argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    //    ::testing::GTEST_FLAG(filter) = "FactorImu2d.no_bias_fixed"; // Test only this one
+    return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_imu2d_tools.cpp b/test/gtest_imu_2d_tools.cpp
similarity index 98%
rename from test/gtest_imu2d_tools.cpp
rename to test/gtest_imu_2d_tools.cpp
index d05cc6093eda505c7e9bd911b62d5896c92c1b43..4ed316f43458b77cd4bb5aae15c9a3b43ebf4cde 100644
--- a/test/gtest_imu2d_tools.cpp
+++ b/test/gtest_imu_2d_tools.cpp
@@ -18,13 +18,13 @@
 // 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/>.
 /*
- * gtest_imu2d_tools.cpp
+ * gtest_imu_2d_tools.cpp
  *
  *  Created on: Nov 17, 2020
  *      Author: igeer
  */
 
-#include "imu/math/imu2d_tools.h"
+#include "imu/math/imu_2d_tools.h"
 #include <core/utils/utils_gtest.h>
 
 using namespace Eigen;
@@ -164,7 +164,7 @@ TEST(Imu2d_tools, compose_between_with_state)
     ASSERT_MATRIX_APPROX(betweenStates(x1, composeOverState(x1, d, dt), dt), d, 1e-10);
 }
 
-TEST(imu2d_tools, betweencomposeOverStateWithGravity_D000)
+TEST(imu_2d_tools, betweencomposeOverStateWithGravity_D000)
 {
     VectorXd x0(5), x1(5), d(5), g(2);
     double dt = 0.1;
@@ -181,7 +181,7 @@ TEST(imu2d_tools, betweencomposeOverStateWithGravity_D000)
 
 }
 
-TEST(imu2d_tools, betweencomposeOverStateWithGravity_D0X0)
+TEST(imu_2d_tools, betweencomposeOverStateWithGravity_D0X0)
 {
     VectorXd x0(5), x1(5), d(5), g(2);
     double dt = 0.1;
@@ -199,7 +199,7 @@ TEST(imu2d_tools, betweencomposeOverStateWithGravity_D0X0)
 
 }
 
-TEST(imu2d_tools, betweencomposeOverStateWithGravity_DXXX)
+TEST(imu_2d_tools, betweencomposeOverStateWithGravity_DXXX)
 {
     VectorXd x0(5), x1(5), d(5), g(2);
     double dt = 0.1;
diff --git a/test/gtest_imu.cpp b/test/gtest_imu_3d.cpp
similarity index 98%
rename from test/gtest_imu.cpp
rename to test/gtest_imu_3d.cpp
index 07e6f8e1d57d1a222ac3056439b23e351d46f539..1bec04ae8133bdffcb3ad01e2431d876dacf6612 100644
--- a/test/gtest_imu.cpp
+++ b/test/gtest_imu_3d.cpp
@@ -17,18 +17,12 @@
 //
 // 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/>.
-/*
- * gtest_Imu.cpp
- *
- *  Created on: Nov 14, 2017
- *      Author: jsola
- */
 
 //Wolf
 
-#include "imu/processor/processor_imu.h"
+#include "imu/processor/processor_imu_3d.h"
 #include "imu/sensor/sensor_imu.h"
-#include "imu/math/imu_tools.h"
+#include "imu/math/imu_3d_tools.h"
 #include "imu/internal/config.h"
 
 #include <core/utils/utils_gtest.h>
@@ -88,12 +82,12 @@ class Process_Factor_Imu : public testing::Test
 
         // Deltas and states (exact, integrated, corrected, solved, etc)
         VectorXd        D_exact,         x1_exact;          // exact or ground truth
-        VectorXd        D_preint_imu,    x1_preint_imu;     // preintegrated with imu_tools
-        VectorXd        D_corrected_imu, x1_corrected_imu;  // corrected with imu_tools
+        VectorXd        D_preint_imu,    x1_preint_imu;     // preintegrated with imu_3d_tools
+        VectorXd        D_corrected_imu, x1_corrected_imu;  // corrected with imu_3d_tools
         VectorXd        D_preint,        x1_preint;         // preintegrated with processor_imu
         VectorXd        D_corrected,     x1_corrected;      // corrected with processor_imu
         VectorXd        D_optim,         x1_optim;          // optimized using factor_imu
-        VectorXd        D_optim_imu,     x1_optim_imu;      // corrected with imu_tools using optimized bias
+        VectorXd        D_optim_imu,     x1_optim_imu;      // corrected with imu_3d_tools using optimized bias
         VectorXd                         x0_optim;          // optimized using factor_imu
 
         // Trajectory buffer of correction Jacobians
@@ -428,7 +422,7 @@ class Process_Factor_Imu : public testing::Test
             // pre-integrate
             MotionBuffer Buf_preint_imu = integrateDeltaTrajectory(q0, motion, bias_real, bias_preint, dt, J_D_bias);
 
-            // Build trajectories preintegrated and corrected with imu_tools
+            // Build trajectories preintegrated and corrected with imu_3d_tools
             col = 0;
             Dt = 0;
             for (auto m : Buf_preint_imu)
@@ -541,7 +535,7 @@ class Process_Factor_Imu : public testing::Test
             D_optim         = getDeltaCorrected(CM_1, bias_0);
             x1_optim        = KF_1->getStateVector("POV");
 
-            // with imu_tools
+            // with imu_3d_tools
             step            = J_D_bias * (bias_0 - bias_preint);
             D_optim_imu     = imu::plus(D_preint, step);
             x1_optim_imu    = imu::composeOverState(x0_optim, D_optim_imu, DT);
@@ -564,11 +558,11 @@ class Process_Factor_Imu : public testing::Test
 
             WOLF_TRACE("D_exact       : ", D_exact            .transpose() ); // exact delta integrated, with absolutely no bias
             WOLF_TRACE("D_preint      : ", D_preint           .transpose() ); // pre-integrated delta using processor
-            WOLF_TRACE("D_preint_imu  : ", D_preint_imu       .transpose() ); // pre-integrated delta using imu_tools
+            WOLF_TRACE("D_preint_imu  : ", D_preint_imu       .transpose() ); // pre-integrated delta using imu_3d_tools
             WOLF_TRACE("D_correct     : ", D_corrected        .transpose() ); // corrected delta using processor
-            WOLF_TRACE("D_correct_imu : ", D_corrected_imu    .transpose() ); // corrected delta using imu_tools
+            WOLF_TRACE("D_correct_imu : ", D_corrected_imu    .transpose() ); // corrected delta using imu_3d_tools
             WOLF_TRACE("D_optim       : ", D_optim            .transpose() ); // corrected delta using optimum bias after solving using processor
-            WOLF_TRACE("D_optim_imu   : ", D_optim_imu        .transpose() ); // corrected delta using optimum bias after solving using imu_tools
+            WOLF_TRACE("D_optim_imu   : ", D_optim_imu        .transpose() ); // corrected delta using optimum bias after solving using imu_3d_tools
 
             WOLF_TRACE("bias real     : ", bias_real          .transpose() ); // real bias
             WOLF_TRACE("bias preint   : ", bias_preint        .transpose() ); // bias used during pre-integration
@@ -578,13 +572,13 @@ class Process_Factor_Imu : public testing::Test
             // states at the end of integration, i.e., at KF_1
             WOLF_TRACE("X1_exact      : ", x1_exact           .transpose() ); // exact state
             WOLF_TRACE("X1_preint     : ", x1_preint          .transpose() ); // state using delta preintegrated by processor
-            WOLF_TRACE("X1_preint_imu : ", x1_preint_imu      .transpose() ); // state using delta preintegrated by imu_tools
+            WOLF_TRACE("X1_preint_imu : ", x1_preint_imu      .transpose() ); // state using delta preintegrated by imu_3d_tools
             WOLF_TRACE("X1_correct    : ", x1_corrected       .transpose() ); // corrected state vy processor
-            WOLF_TRACE("X1_correct_imu: ", x1_corrected_imu   .transpose() ); // corrected state by imu_tools
+            WOLF_TRACE("X1_correct_imu: ", x1_corrected_imu   .transpose() ); // corrected state by imu_3d_tools
             WOLF_TRACE("X1_optim      : ", x1_optim           .transpose() ); // optimal state after solving using processor
-            WOLF_TRACE("X1_optim_imu  : ", x1_optim_imu       .transpose() ); // optimal state after solving using imu_tools
-            WOLF_TRACE("err1_optim    : ", (x1_optim-x1_exact).transpose() ); // error of optimal state corrected by imu_tools w.r.t. exact state
-            WOLF_TRACE("err1_optim_imu: ", (x1_optim_imu-x1_exact).transpose() ); // error of optimal state corrected by imu_tools w.r.t. exact state
+            WOLF_TRACE("X1_optim_imu  : ", x1_optim_imu       .transpose() ); // optimal state after solving using imu_3d_tools
+            WOLF_TRACE("err1_optim    : ", (x1_optim-x1_exact).transpose() ); // error of optimal state corrected by imu_3d_tools w.r.t. exact state
+            WOLF_TRACE("err1_optim_imu: ", (x1_optim_imu-x1_exact).transpose() ); // error of optimal state corrected by imu_3d_tools w.r.t. exact state
             WOLF_TRACE("X0_optim      : ", x0_optim           .transpose() ); // optimal state after solving using processor
         }
 
diff --git a/test/gtest_imu_mocap_fusion.cpp b/test/gtest_imu_3d_mocap_fusion.cpp
similarity index 99%
rename from test/gtest_imu_mocap_fusion.cpp
rename to test/gtest_imu_3d_mocap_fusion.cpp
index e2316366e4896726ae2b2f386f3c5c848db02a3b..dd13a7c4929e2b151f1d44a890c74d3093b97ae5 100644
--- a/test/gtest_imu_mocap_fusion.cpp
+++ b/test/gtest_imu_3d_mocap_fusion.cpp
@@ -32,7 +32,7 @@
 #include "imu/internal/config.h"
 #include "imu/capture/capture_imu.h"
 #include "imu/sensor/sensor_imu.h"
-#include "imu/processor/processor_imu.h"
+#include "imu/processor/processor_imu_3d.h"
 
 #include "Eigen/Dense"
 #include <Eigen/SparseQR>
diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_3d_static_init.cpp
similarity index 99%
rename from test/gtest_imu_static_init.cpp
rename to test/gtest_imu_3d_static_init.cpp
index 9de7e9868582f8f6c8cfcbe509dbdcc82ee407a4..648f42a6b9622567ac905876a5637506fd0f3a9e 100644
--- a/test/gtest_imu_static_init.cpp
+++ b/test/gtest_imu_3d_static_init.cpp
@@ -30,7 +30,7 @@
 #include "imu/internal/config.h"
 
 #include "imu/factor/factor_imu.h"
-#include "imu/math/imu_tools.h"
+#include "imu/math/imu_3d_tools.h"
 #include "imu/sensor/sensor_imu.h"
 #include "core/capture/capture_void.h"
 #include <core/factor/factor_relative_pose_3d.h>
diff --git a/test/gtest_imu_tools.cpp b/test/gtest_imu_3d_tools.cpp
similarity index 99%
rename from test/gtest_imu_tools.cpp
rename to test/gtest_imu_3d_tools.cpp
index 0d4dc734c5116949226189998e3b504f63035ff2..206848d6061c8fadb2bf3aae8b81968ded3ed3d0 100644
--- a/test/gtest_imu_tools.cpp
+++ b/test/gtest_imu_3d_tools.cpp
@@ -18,13 +18,13 @@
 // 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/>.
 /*
- * gtest_imu_tools.cpp
+ * gtest_imu_3d_tools.cpp
  *
  *  Created on: Jul 29, 2017
  *      Author: jsola
  */
 
-#include "imu/math/imu_tools.h"
+#include "imu/math/imu_3d_tools.h"
 #include <core/utils/utils_gtest.h>
 
 using namespace Eigen;
diff --git a/test/gtest_processor_imu2d.cpp b/test/gtest_processor_imu_2d.cpp
similarity index 97%
rename from test/gtest_processor_imu2d.cpp
rename to test/gtest_processor_imu_2d.cpp
index 5da98fafeff1cfe67f78d8c049c4f63603a415a5..7cd9f3336d73047fb32333c809eff18f052870fc 100644
--- a/test/gtest_processor_imu2d.cpp
+++ b/test/gtest_processor_imu_2d.cpp
@@ -20,7 +20,7 @@
 
 #include "imu/internal/config.h"
 #include "imu/capture/capture_imu.h"
-#include "imu/processor/processor_imu2d.h"
+#include "imu/processor/processor_imu_2d.h"
 #include "imu/sensor/sensor_imu.h"
 
 #include <core/utils/utils_gtest.h>
@@ -58,8 +58,8 @@ class ProcessorImu2dTest : public testing::Test
     {
         // Wolf problem
         problem = Problem::create("POV", 2);
-        sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d.yaml", {wolf_root});
-        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu2d.yaml", {wolf_root});
+        sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu_2d.yaml", {wolf_root});
+        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu_2d.yaml", {wolf_root});
         processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
 
         // Time and data variables
@@ -94,7 +94,7 @@ TEST(ProcessorImu2d_constructors, ALL)
     std::string wolf_root = _WOLF_IMU_CODE_DIR;
     ProblemPtr problem = Problem::create("POV", 2);
     std::cout << "creating sensor_ptr" << std::endl;
-    SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d.yaml", {wolf_root});
+    SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu_2d.yaml", {wolf_root});
     std::cout << "created sensor_ptr" << std::endl;
     ParamsProcessorImu2dPtr params_default = std::make_shared<ParamsProcessorImu2d>();
     // ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Imu pre-integrator", sensor_ptr, params_default);
@@ -105,7 +105,7 @@ TEST(ProcessorImu2d_constructors, ALL)
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getAngleTurned(),   params_default->angle_turned);
 
     //Factory constructor with yaml
-    processor_ptr = problem->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu2d.yaml", {wolf_root});
+    processor_ptr = problem->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu_2d.yaml", {wolf_root});
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxTimeSpan(),   2.0);
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxBuffLength(), 20000);
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getDistTraveled(),  2.0);
diff --git a/test/gtest_processor_imu2d_with_gravity.cpp b/test/gtest_processor_imu_2d_with_gravity.cpp
similarity index 98%
rename from test/gtest_processor_imu2d_with_gravity.cpp
rename to test/gtest_processor_imu_2d_with_gravity.cpp
index 5067c87ffe6d2a2062d2f65fb983eefcf5182d3c..91dd896c12925059b283282ea8107b5261193914 100644
--- a/test/gtest_processor_imu2d_with_gravity.cpp
+++ b/test/gtest_processor_imu_2d_with_gravity.cpp
@@ -20,7 +20,7 @@
 
 #include "imu/internal/config.h"
 #include "imu/capture/capture_imu.h"
-#include "imu/processor/processor_imu2d.h"
+#include "imu/processor/processor_imu_2d.h"
 #include "imu/sensor/sensor_imu.h"
 
 // #include "core/common/wolf.h"
@@ -66,8 +66,8 @@ class ProcessorImu2dTest : public testing::Test
 
         // Wolf problem
         problem = Problem::create("POV", 2);
-        sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d_with_gravity.yaml", {wolf_root});
-        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu2d.yaml", {wolf_root});
+        sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu_2d_with_gravity.yaml", {wolf_root});
+        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu_2d.yaml", {wolf_root});
         processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
 
         // Time and data variables
diff --git a/test/gtest_processor_imu.cpp b/test/gtest_processor_imu_3d.cpp
similarity index 97%
rename from test/gtest_processor_imu.cpp
rename to test/gtest_processor_imu_3d.cpp
index 12d32c2b225e6a2411291ddce32ba1ac0e487f7c..62c22ee1002ee0594d1ce31cf92d6b54172b8ee9 100644
--- a/test/gtest_processor_imu.cpp
+++ b/test/gtest_processor_imu_3d.cpp
@@ -20,7 +20,7 @@
 
 #include "imu/internal/config.h"
 #include "imu/capture/capture_imu.h"
-#include "imu/processor/processor_imu.h"
+#include "imu/processor/processor_imu_3d.h"
 #include "imu/sensor/sensor_imu.h"
 
 #include <core/utils/utils_gtest.h>
@@ -51,7 +51,7 @@ SpecComposite problem_prior_{{'P', SpecState("StatePoint3d",
                                              "factor",
                                              Vector3d::Ones())}};
 
-class ProcessorImut : public testing::Test
+class ProcessorImu3dTest : public testing::Test
 {
 
     public: //These can be accessed in fixtures
@@ -74,7 +74,7 @@ class ProcessorImut : public testing::Test
         // Wolf problem
         problem = Problem::create("POV", 3);
         sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
-        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
+        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu3d", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
         processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
 
         // Time and data variables
@@ -103,7 +103,7 @@ class ProcessorImut : public testing::Test
     }
 };
 
-TEST(ProcessorImu_constructors, ALL)
+TEST(ProcessorImu3d_constructors, ALL)
 {
     //constructor with ProcessorImuParamsPtr argument only
     ParamsProcessorImuPtr param_ptr = std::make_shared<ParamsProcessorImu>();
@@ -137,7 +137,7 @@ TEST(ProcessorImu_constructors, ALL)
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getAngleTurned(),   0.2);
 }
 
-TEST(ProcessorImu, voteForKeyFrame)
+TEST(ProcessorImu3d, voteForKeyFrame)
 {
     // Wolf problem
     ProblemPtr problem = Problem::create("POV", 3);
@@ -212,7 +212,7 @@ TEST(ProcessorImu, voteForKeyFrame)
 }
 
 
-TEST_F(ProcessorImut, acc_x)
+TEST_F(ProcessorImu3dTest, acc_x)
 {
     t.set(0); // clock in 0,1 ms ticks
     // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
@@ -243,7 +243,7 @@ TEST_F(ProcessorImut, acc_x)
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS_SMALL);
 }
 
-TEST_F(ProcessorImut, acc_y)
+TEST_F(ProcessorImu3dTest, acc_y)
 {
     // last part of this test fails with precision Constants::EPS_SMALL beacause error is in 1e-12
     // difference hier is that we integrate over 1ms periods
@@ -290,7 +290,7 @@ TEST_F(ProcessorImut, acc_y)
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS);
 }
 
-TEST_F(ProcessorImut, acc_z)
+TEST_F(ProcessorImu3dTest, acc_z)
 {
     t.set(0); // clock in 0,1 ms ticks
     // x0 << 0,0,0,  0,0,0,1,  0,0,0;
@@ -334,7 +334,7 @@ TEST_F(ProcessorImut, acc_z)
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS);
 }
 
-TEST_F(ProcessorImut, check_Covariance)
+TEST_F(ProcessorImu3dTest, check_Covariance)
 {
     t.set(0); // clock in 0,1 ms ticks
     // x0 << 0,0,0,  0,0,0,1,  0,0,0;
@@ -361,7 +361,7 @@ TEST_F(ProcessorImut, check_Covariance)
 //    ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXd::Zero(9,9), Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
 }
 
-TEST_F(ProcessorImut, gyro_x)
+TEST_F(ProcessorImu3dTest, gyro_x)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -411,7 +411,7 @@ TEST_F(ProcessorImut, gyro_x)
     ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
-TEST_F(ProcessorImut, gyro_x_biasedAbx)
+TEST_F(ProcessorImu3dTest, gyro_x_biasedAbx)
 {
     // time
     double dt(0.001);
@@ -479,7 +479,7 @@ TEST_F(ProcessorImut, gyro_x_biasedAbx)
 
 }
 
-TEST_F(ProcessorImut, gyro_xy_biasedAbxy)
+TEST_F(ProcessorImu3dTest, gyro_xy_biasedAbxy)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -538,7 +538,7 @@ TEST_F(ProcessorImut, gyro_xy_biasedAbxy)
 //    "\n expected state is : \n" << x.transpose() << std::endl;
 }
 
-TEST_F(ProcessorImut, gyro_z)
+TEST_F(ProcessorImu3dTest, gyro_z)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -584,7 +584,7 @@ TEST_F(ProcessorImut, gyro_z)
     ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
-TEST_F(ProcessorImut, gyro_xyz)
+TEST_F(ProcessorImu3dTest, gyro_xyz)
 {
     t.set(0); // clock in 0,1 ms ticks
     // x0 << 0,0,0,  0,0,0,1,  0,0,0;
@@ -677,7 +677,7 @@ TEST_F(ProcessorImut, gyro_xyz)
     ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7,3), Constants::EPS);
 }
 
-TEST_F(ProcessorImut, gyro_z_ConstVelocity)
+TEST_F(ProcessorImu3dTest, gyro_z_ConstVelocity)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -723,7 +723,7 @@ TEST_F(ProcessorImut, gyro_z_ConstVelocity)
     ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
-TEST_F(ProcessorImut, gyro_x_ConstVelocity)
+TEST_F(ProcessorImu3dTest, gyro_x_ConstVelocity)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -774,7 +774,7 @@ TEST_F(ProcessorImut, gyro_x_ConstVelocity)
     ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
-TEST_F(ProcessorImut, gyro_xy_ConstVelocity)
+TEST_F(ProcessorImu3dTest, gyro_xy_ConstVelocity)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -825,7 +825,7 @@ TEST_F(ProcessorImut, gyro_xy_ConstVelocity)
     ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
-TEST_F(ProcessorImut, gyro_y_ConstVelocity)
+TEST_F(ProcessorImu3dTest, gyro_y_ConstVelocity)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -876,7 +876,7 @@ TEST_F(ProcessorImut, gyro_y_ConstVelocity)
     ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
-TEST_F(ProcessorImut, gyro_xyz_ConstVelocity)
+TEST_F(ProcessorImu3dTest, gyro_xyz_ConstVelocity)
 {
     t.set(0); // clock in 0,1 ms ticks
     // x0 << 0,0,0,  0,0,0,1,  2,0,0;
@@ -971,7 +971,7 @@ TEST_F(ProcessorImut, gyro_xyz_ConstVelocity)
 
 }
 
-TEST_F(ProcessorImut, gyro_x_acc_x)
+TEST_F(ProcessorImu3dTest, gyro_x_acc_x)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -1027,7 +1027,7 @@ TEST_F(ProcessorImut, gyro_x_acc_x)
     ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
-TEST_F(ProcessorImut, gyro_y_acc_y)
+TEST_F(ProcessorImu3dTest, gyro_y_acc_y)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -1083,7 +1083,7 @@ TEST_F(ProcessorImut, gyro_y_acc_y)
     ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
-TEST_F(ProcessorImut, gyro_z_acc_z)
+TEST_F(ProcessorImu3dTest, gyro_z_acc_z)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
@@ -1142,7 +1142,7 @@ TEST_F(ProcessorImut, gyro_z_acc_z)
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
-  //::testing::GTEST_FLAG(filter) = "ProcessorImut.check_Covariance";
+  //::testing::GTEST_FLAG(filter) = "ProcessorImu3dTest.check_Covariance";
   return RUN_ALL_TESTS();
 }
 
diff --git a/test/gtest_processor_imu_jacobians.cpp b/test/gtest_processor_imu_3d_jacobians.cpp
similarity index 96%
rename from test/gtest_processor_imu_jacobians.cpp
rename to test/gtest_processor_imu_3d_jacobians.cpp
index f9bda2fc33585e5ee3740099559968cad02246d0..ef429cf072840049e14d53d86e846ccc3d0ae6e0 100644
--- a/test/gtest_processor_imu_jacobians.cpp
+++ b/test/gtest_processor_imu_3d_jacobians.cpp
@@ -21,7 +21,7 @@
  //Wolf
 #include "imu/capture/capture_imu.h"
 #include "imu/sensor/sensor_imu.h"
-#include "processor_imu_tester.h"
+#include "processor_imu_3d_tester.h"
 #include "core/common/wolf.h"
 #include "core/problem/problem.h"
 #include "core/state_block/state_block.h"
@@ -41,7 +41,7 @@
 using namespace wolf;
 
 // A new one of these is created for each test
-class ProcessorImuJacobians : public testing::Test
+class ProcessorImu3dJacobians : public testing::Test
 {
     public:
         TimeStamp t;
@@ -78,7 +78,7 @@ class ProcessorImuJacobians : public testing::Test
         Eigen::VectorXd Imu_extrinsics(7);
         Imu_extrinsics << 0,0,0, 0,0,0,1; // Imu pose in the robot
 
-        ProcessorImuTester processor_imu;
+        ProcessorImu3dTester processor_imu;
         ddelta_bias = 0.00000001;
         dt = 0.001;
 
@@ -123,7 +123,7 @@ class ProcessorImuJacobians : public testing::Test
     }
 };
 
-class ProcessorImuJacobians_Dq : public testing::Test
+class ProcessorImu3dJacobians_Dq : public testing::Test
 {
     public:
         TimeStamp t;
@@ -157,7 +157,7 @@ class ProcessorImuJacobians_Dq : public testing::Test
         Eigen::VectorXd Imu_extrinsics(7);
         Imu_extrinsics << 0,0,0, 0,0,0,1; // Imu pose in the robot
 
-        ProcessorImuTester processor_imu;
+        ProcessorImu3dTester processor_imu;
         ddelta_bias2 = 0.0001;
         dt = 0.001;
 
@@ -228,7 +228,7 @@ class ProcessorImuJacobians_Dq : public testing::Test
         Then at first step, dR.tr() = Id, dDq_dwb = 0_{3x3}, which boils down to dDq_dwb_ = Jr(wdt)*dt 
 */
 
-TEST_F(ProcessorImuJacobians, dDp_dab)
+TEST_F(ProcessorImu3dJacobians, dDp_dab)
 {
     using namespace wolf;
     Eigen::Matrix3d dDp_dab;
@@ -240,7 +240,7 @@ TEST_F(ProcessorImuJacobians, dDp_dab)
      "\ndDp_dab_a - dDp_dab_ : \n" << bias_jac.dDp_dab_ - dDp_dab << std::endl;
 }
 
-TEST_F(ProcessorImuJacobians, dDv_dab)
+TEST_F(ProcessorImu3dJacobians, dDv_dab)
 {
     using namespace wolf;
     Eigen::Matrix3d dDv_dab;
@@ -252,7 +252,7 @@ TEST_F(ProcessorImuJacobians, dDv_dab)
      "\ndDv_dab_a - dDv_dab_ : \n" << bias_jac.dDv_dab_ - dDv_dab << std::endl;
 }
 
-TEST_F(ProcessorImuJacobians, dDp_dwb)
+TEST_F(ProcessorImu3dJacobians, dDp_dwb)
 {
     using namespace wolf;
     Eigen::Matrix3d dDp_dwb;
@@ -264,7 +264,7 @@ TEST_F(ProcessorImuJacobians, dDp_dwb)
      "\ndDp_dwb_a - dDv_dab_ : \n" << bias_jac.dDp_dwb_ - dDp_dwb << std::endl;
 }
 
-TEST_F(ProcessorImuJacobians, dDv_dwb_)
+TEST_F(ProcessorImu3dJacobians, dDv_dwb_)
 {
     using namespace wolf;
     Eigen::Matrix3d dDv_dwb;
@@ -276,7 +276,7 @@ TEST_F(ProcessorImuJacobians, dDv_dwb_)
      "\ndDv_dwb_a - dDv_dwb_ : \n" << bias_jac.dDv_dwb_ - dDv_dwb << std::endl;
 }
 
-TEST_F(ProcessorImuJacobians, dDq_dab)
+TEST_F(ProcessorImu3dJacobians, dDq_dab)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
@@ -291,7 +291,7 @@ TEST_F(ProcessorImuJacobians, dDq_dab)
     ASSERT_TRUE(dDq_dab.isZero(wolf::Constants::EPS)) << "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
 }
 
-TEST_F(ProcessorImuJacobians, dDq_dwb_noise_1Em8_)
+TEST_F(ProcessorImu3dJacobians, dDq_dwb_noise_1Em8_)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
@@ -313,7 +313,7 @@ TEST_F(ProcessorImuJacobians, dDq_dwb_noise_1Em8_)
         << std::endl;
 }
 
-TEST_F(ProcessorImuJacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
+TEST_F(ProcessorImu3dJacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> q_in_1(NULL), q_in_2(NULL);
@@ -407,7 +407,7 @@ TEST_F(ProcessorImuJacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
     dDo_doz = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,0,dthetaz)) )/dthetaz
      */
      
-TEST_F(ProcessorImuJacobians, dDp_dP)
+TEST_F(ProcessorImu3dJacobians, dDp_dP)
 {
     using namespace wolf;
     Eigen::Matrix3d dDp_dP;
@@ -420,7 +420,7 @@ TEST_F(ProcessorImuJacobians, dDp_dP)
      "\ndDp_dP_a - dDp_dP_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) - dDp_dP << std::endl;
 }
 
-TEST_F(ProcessorImuJacobians, dDp_dV)
+TEST_F(ProcessorImu3dJacobians, dDp_dV)
 {
     using namespace wolf;
     Eigen::Matrix3d dDp_dV;
@@ -433,7 +433,7 @@ TEST_F(ProcessorImuJacobians, dDp_dV)
      "\ndDp_dV_a - dDp_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) - dDp_dV << std::endl;
 }
 
-TEST_F(ProcessorImuJacobians, dDp_dO)
+TEST_F(ProcessorImu3dJacobians, dDp_dO)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
@@ -450,7 +450,7 @@ TEST_F(ProcessorImuJacobians, dDp_dO)
      "\ndDp_dO_a - dDp_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) - dDp_dO << std::endl;
 }
 
-TEST_F(ProcessorImuJacobians, dDv_dV)
+TEST_F(ProcessorImu3dJacobians, dDv_dV)
 {
     using namespace wolf;
     Eigen::Matrix3d dDv_dV;
@@ -463,7 +463,7 @@ TEST_F(ProcessorImuJacobians, dDv_dV)
      "\ndDv_dV_a - dDv_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) - dDv_dV << std::endl;
 }
 
-TEST_F(ProcessorImuJacobians, dDv_dO)
+TEST_F(ProcessorImu3dJacobians, dDv_dO)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
@@ -482,7 +482,7 @@ TEST_F(ProcessorImuJacobians, dDv_dO)
 
 //dDo_dP = dDo_dV = [0, 0, 0]
 
-TEST_F(ProcessorImuJacobians, dDo_dO)
+TEST_F(ProcessorImu3dJacobians, dDo_dO)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
@@ -499,7 +499,7 @@ TEST_F(ProcessorImuJacobians, dDo_dO)
      "\ndDo_dO_a - dDo_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) - dDo_dO << std::endl;
 }
 
-TEST_F(ProcessorImuJacobians, dDp_dp)
+TEST_F(ProcessorImu3dJacobians, dDp_dp)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL);
@@ -517,7 +517,7 @@ TEST_F(ProcessorImuJacobians, dDp_dp)
 
 //dDv_dp = [0, 0, 0]
 
-TEST_F(ProcessorImuJacobians, dDv_dv)
+TEST_F(ProcessorImu3dJacobians, dDv_dv)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL);
@@ -535,7 +535,7 @@ TEST_F(ProcessorImuJacobians, dDv_dv)
 
 //dDo_dp = dDo_dv = [0, 0, 0]
 
-TEST_F(ProcessorImuJacobians, dDo_do)
+TEST_F(ProcessorImu3dJacobians, dDo_do)
 {
     using namespace wolf;
     Eigen::Map<Eigen::Quaterniond> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp
index c51059f7d373cc09f1bd9ccc7092f85a2cc3a753..8f9f3e68d7076331becdb64cbd0f1dedbb71f4d6 100644
--- a/test/gtest_processor_motion_intrinsics_update.cpp
+++ b/test/gtest_processor_motion_intrinsics_update.cpp
@@ -28,7 +28,7 @@
 #include "core/problem/problem.h"
 #include <core/ceres_wrapper/solver_ceres.h>
 #include "imu/sensor/sensor_imu.h"
-#include "imu/processor/processor_imu.h"
+#include "imu/processor/processor_imu_3d.h"
 #include "imu/capture/capture_imu.h"
 
 using namespace wolf;
diff --git a/test/gtest_sensor_compass.cpp b/test/gtest_sensor_compass.cpp
index ffbf48619e0c5372aaa01aff91a46fdfda635ab1..62b4a557c5993b823e48a30ffc38ac58f09dd00b 100644
--- a/test/gtest_sensor_compass.cpp
+++ b/test/gtest_sensor_compass.cpp
@@ -23,7 +23,6 @@
 #include "imu/internal/config.h"
 #include "imu/capture/capture_compass.h"
 #include "imu/sensor/sensor_compass.h"
-#include "core/sensor/factory_sensor.h"
 
 #include <cstdio>
 
@@ -31,152 +30,106 @@ using namespace wolf;
 using namespace Eigen;
 using namespace yaml_schema_cpp;
 
-std::string wolf_root = _WOLF_IMU_CODE_DIR;
-
+std::string imu_dir         = _WOLF_IMU_CODE_DIR;
+std::string wolf_schema_dir = _WOLF_SCHEMA_DIR;
 
 class sensor_compass_test : public testing::Test
 {
-    public:
-       
-        VectorXd p_state;
-        VectorXd o_state;
-        VectorXd i_state;
-        VectorXd f_state;
-
-        double stdev_noise;
-
-        MatrixXd noise_cov;
-
-        std::string p_mode, 
-                    o_mode, 
-                    i_mode, 
-                    f_mode;
-
-        bool p_dynamic, 
-             o_dynamic, 
-             i_dynamic, 
-             f_dynamic;
-
-        void SetUp( ) override
-        {
-        }
-
-        void LoadYamlGroundtruth(int dim)
-        {
-            // load from yaml
-            YamlServer server({wolf_root}, wolf_root + (dim == 2 ? 
-                                                        "/test/yaml/sensor_compass_2d.yaml" :
-                                                        "/test/yaml/sensor_compass_3d.yaml"));
-            auto node = server.getNode();
-
-            p_state     = node["states"]["P"]["state"].as<VectorXd>();
-            o_state     = node["states"]["O"]["state"].as<VectorXd>();
-            i_state     = node["states"]["I"]["state"].as<VectorXd>();
-            f_state     = node["states"]["F"]["state"].as<VectorXd>();
-
-            p_mode      = node["states"]["P"]["mode"].as<std::string>();
-            o_mode      = node["states"]["O"]["mode"].as<std::string>();
-            i_mode      = node["states"]["I"]["mode"].as<std::string>();
-            f_mode      = node["states"]["F"]["mode"].as<std::string>();
-            
-            p_dynamic   = node["states"]["P"]["dynamic"].as<bool>();
-            o_dynamic   = node["states"]["O"]["dynamic"].as<bool>();
-            i_dynamic   = node["states"]["I"]["dynamic"].as<bool>();
-            f_dynamic   = node["states"]["F"]["dynamic"].as<bool>();
-
-            stdev_noise  = node["stdev_noise"].as<double>();
-            noise_cov = VectorXd::Constant(dim, stdev_noise*stdev_noise).asDiagonal();
-        }
-
-        void RandomGroundtruth(int dim)
-        {
-            p_state     = VectorXd::Random(dim) * 10;
-            o_state     = VectorXd::Random(dim == 2 ? 1 : 4).normalized();
-            i_state     = VectorXd::Random(dim) * 10;
-            f_state     = VectorXd::Random(dim) * 10;
-
-            p_mode      = "fix";
-            o_mode      = "fix";
-            i_mode      = "initial_guess";
-            f_mode      = "initial_guess";
-
-            p_dynamic   = true;
-            o_dynamic   = false;
-            i_dynamic   = true;
-            f_dynamic   = false;
-
-            stdev_noise  = Vector1d::Random()(0);
-            noise_cov = VectorXd::Constant(dim, stdev_noise*stdev_noise).asDiagonal();
-        }
+  public:
+    VectorXd p_state;
+    VectorXd o_state;
+    VectorXd i_state;
+    VectorXd f_state;
+
+    double stdev_noise;
+
+    MatrixXd noise_cov;
+
+    std::string p_mode, o_mode, i_mode, f_mode;
+
+    bool p_dynamic, o_dynamic, i_dynamic, f_dynamic;
+
+    void SetUp() override {}
+
+    void LoadYamlGroundtruth(int dim)
+    {
+        // load from yaml
+        YamlServer server(
+            {imu_dir, wolf_schema_dir},
+            imu_dir + (dim == 2 ? "/test/yaml/sensor_compass_2d.yaml" : "/test/yaml/sensor_compass_3d.yaml"));
+        auto node = server.getNode();
+
+        p_state = node["states"]["P"]["state"].as<VectorXd>();
+        o_state = node["states"]["O"]["state"].as<VectorXd>();
+        i_state = node["states"]["I"]["state"].as<VectorXd>();
+        f_state = node["states"]["F"]["state"].as<VectorXd>();
+
+        p_mode = node["states"]["P"]["mode"].as<std::string>();
+        o_mode = node["states"]["O"]["mode"].as<std::string>();
+        i_mode = node["states"]["I"]["mode"].as<std::string>();
+        f_mode = node["states"]["F"]["mode"].as<std::string>();
+
+        p_dynamic = node["states"]["P"]["dynamic"].as<bool>();
+        o_dynamic = node["states"]["O"]["dynamic"].as<bool>();
+        i_dynamic = node["states"]["I"]["dynamic"].as<bool>();
+        f_dynamic = node["states"]["F"]["dynamic"].as<bool>();
+
+        stdev_noise = node["stdev_noise"].as<double>();
+        noise_cov   = VectorXd::Constant(dim, stdev_noise * stdev_noise).asDiagonal();
+    }
+
+    void RandomGroundtruth(int dim)
+    {
+        p_state = VectorXd::Random(dim) * 10;
+        o_state = VectorXd::Random(dim == 2 ? 1 : 4).normalized();
+        i_state = VectorXd::Random(dim) * 10;
+        f_state = VectorXd::Random(dim) * 10;
+
+        p_mode = "fix";
+        o_mode = "fix";
+        i_mode = "initial_guess";
+        f_mode = "initial_guess";
+
+        p_dynamic = true;
+        o_dynamic = false;
+        i_dynamic = true;
+        f_dynamic = false;
+
+        stdev_noise = Vector1d::Random()(0);
+        noise_cov   = VectorXd::Constant(dim, stdev_noise * stdev_noise).asDiagonal();
+    }
 };
 
-TEST_F(sensor_compass_test, constructor_priors_3d)
-{
-    RandomGroundtruth(3);
-
-    auto params = std::make_shared<ParamsSensorCompass>();
-    params->stdev_noise = stdev_noise;
-
-    SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint3d",     p_state, p_mode, VectorXd(0), p_dynamic)}, 
-                               {'O',SpecStateSensor("StateQuaternion",  o_state, o_mode, VectorXd(0), o_dynamic)}, 
-                               {'I',SpecStateSensor("StateBlock",       i_state, i_mode, VectorXd(0), i_dynamic)}, 
-                               {'F',SpecStateSensor("StateBlock",       f_state, f_mode, VectorXd(0), f_dynamic)}};
-
-    auto sen = std::make_shared<SensorCompass>(params, priors);
-
-    ASSERT_NE(sen, nullptr);
-
-    ASSERT_EQ(sen->getP()->isFixed(),                       p_mode == "fix");
-    ASSERT_EQ(sen->getO()->isFixed(),                       o_mode == "fix");
-    ASSERT_EQ(sen->getStateBlockDynamic('I')->isFixed(),    i_mode == "fix");
-    ASSERT_EQ(sen->getStateBlockDynamic('F')->isFixed(),    f_mode == "fix");
-
-    ASSERT_EQ(sen->isStateBlockDynamic('P'), p_dynamic);
-    ASSERT_EQ(sen->isStateBlockDynamic('O'), o_dynamic);
-    ASSERT_EQ(sen->isStateBlockDynamic('I'), i_dynamic);
-    ASSERT_EQ(sen->isStateBlockDynamic('F'), f_dynamic);
-
-    ASSERT_MATRIX_APPROX(sen->getP()->getState(),                    p_state, Constants::EPS);
-    ASSERT_MATRIX_APPROX(sen->getO()->getState(),                    o_state, Constants::EPS);
-    ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('I')->getState(), i_state, Constants::EPS);
-    ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('F')->getState(), f_state, Constants::EPS);
-
-    ASSERT_EQ(sen->getParams(), params); 
-    ASSERT_NEAR(sen->getParams()->stdev_noise, stdev_noise, Constants::EPS);
-
-    ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS);
-}
-
 TEST_F(sensor_compass_test, factory)
 {
     LoadYamlGroundtruth(3);
 
-    YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_compass_3d.yaml");
-    
+    YamlServer server({imu_dir, wolf_schema_dir}, imu_dir + "/test/yaml/sensor_compass_3d.yaml");
+
     ASSERT_TRUE(server.applySchema("SensorCompass"));
 
-    auto sb = FactorySensor::create("SensorCompass",server.getNode());
+    auto sb = FactorySensorNode::create("SensorCompass", server.getNode(), {imu_dir, wolf_schema_dir});
 
     SensorCompassPtr sen = std::dynamic_pointer_cast<SensorCompass>(sb);
 
     ASSERT_NE(sen, nullptr);
 
-    ASSERT_EQ(sen->getP()->isFixed(),                       p_mode == "fix");
-    ASSERT_EQ(sen->getO()->isFixed(),                       o_mode == "fix");
-    ASSERT_EQ(sen->getStateBlockDynamic('I')->isFixed(),    i_mode == "fix");
-    ASSERT_EQ(sen->getStateBlockDynamic('F')->isFixed(),    f_mode == "fix");
+    ASSERT_EQ(sen->getP()->isFixed(), p_mode == "fix");
+    ASSERT_EQ(sen->getO()->isFixed(), o_mode == "fix");
+    ASSERT_EQ(sen->getStateBlockDynamic('I')->isFixed(), i_mode == "fix");
+    ASSERT_EQ(sen->getStateBlockDynamic('F')->isFixed(), f_mode == "fix");
 
     ASSERT_EQ(sen->isStateBlockDynamic('P'), p_dynamic);
     ASSERT_EQ(sen->isStateBlockDynamic('O'), o_dynamic);
     ASSERT_EQ(sen->isStateBlockDynamic('I'), i_dynamic);
     ASSERT_EQ(sen->isStateBlockDynamic('F'), f_dynamic);
 
-    ASSERT_MATRIX_APPROX(sen->getP()->getState(),                    p_state, Constants::EPS);
-    ASSERT_MATRIX_APPROX(sen->getO()->getState(),                    o_state, Constants::EPS);
+    ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS);
+    ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS);
     ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('I')->getState(), i_state, Constants::EPS);
     ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('F')->getState(), f_state, Constants::EPS);
 
-    ASSERT_NEAR(sen->getParams()->stdev_noise, stdev_noise, Constants::EPS);
+    ASSERT_NEAR(sen->getStdNoise(), stdev_noise, Constants::EPS);
 
     ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS);
 }
@@ -185,28 +138,29 @@ TEST_F(sensor_compass_test, factory_yaml)
 {
     LoadYamlGroundtruth(3);
 
-    auto sb = FactorySensorYaml::create("SensorCompass", "SensorCompass", wolf_root + "/test/yaml/sensor_compass_3d.yaml", {wolf_root});
+    auto sb = FactorySensorFile::create(
+        "SensorCompass", imu_dir + "/test/yaml/sensor_compass_3d.yaml", {imu_dir, wolf_schema_dir});
 
     SensorCompassPtr sen = std::dynamic_pointer_cast<SensorCompass>(sb);
 
     ASSERT_NE(sen, nullptr);
 
-    ASSERT_EQ(sen->getP()->isFixed(),                       p_mode == "fix");
-    ASSERT_EQ(sen->getO()->isFixed(),                       o_mode == "fix");
-    ASSERT_EQ(sen->getStateBlockDynamic('I')->isFixed(),    i_mode == "fix");
-    ASSERT_EQ(sen->getStateBlockDynamic('F')->isFixed(),    f_mode == "fix");
+    ASSERT_EQ(sen->getP()->isFixed(), p_mode == "fix");
+    ASSERT_EQ(sen->getO()->isFixed(), o_mode == "fix");
+    ASSERT_EQ(sen->getStateBlockDynamic('I')->isFixed(), i_mode == "fix");
+    ASSERT_EQ(sen->getStateBlockDynamic('F')->isFixed(), f_mode == "fix");
 
     ASSERT_EQ(sen->isStateBlockDynamic('P'), p_dynamic);
     ASSERT_EQ(sen->isStateBlockDynamic('O'), o_dynamic);
     ASSERT_EQ(sen->isStateBlockDynamic('I'), i_dynamic);
     ASSERT_EQ(sen->isStateBlockDynamic('F'), f_dynamic);
 
-    ASSERT_MATRIX_APPROX(sen->getP()->getState(),                    p_state, Constants::EPS);
-    ASSERT_MATRIX_APPROX(sen->getO()->getState(),                    o_state, Constants::EPS);
+    ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS);
+    ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS);
     ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('I')->getState(), i_state, Constants::EPS);
     ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('F')->getState(), f_state, Constants::EPS);
 
-    ASSERT_NEAR(sen->getParams()->stdev_noise, stdev_noise, Constants::EPS);
+    ASSERT_NEAR(sen->getStdNoise(), stdev_noise, Constants::EPS);
 
     ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS);
 }
@@ -216,4 +170,3 @@ int main(int argc, char **argv)
     testing::InitGoogleTest(&argc, argv);
     return RUN_ALL_TESTS();
 }
-
diff --git a/test/processor_imu2d_tester.cpp b/test/processor_imu_2d_tester.cpp
similarity index 96%
rename from test/processor_imu2d_tester.cpp
rename to test/processor_imu_2d_tester.cpp
index eb006080f1b496d892fe11193adddd5a3763f98f..6b3185d857a29e6037ced6d9f0786933e708e630 100644
--- a/test/processor_imu2d_tester.cpp
+++ b/test/processor_imu_2d_tester.cpp
@@ -18,7 +18,7 @@
 // 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/>.
 
-#include "processor_imu2d_tester.h"
+#include "processor_imu_2d_tester.h"
 
 namespace wolf {
 
diff --git a/test/processor_imu2d_tester.h b/test/processor_imu_2d_tester.h
similarity index 99%
rename from test/processor_imu2d_tester.h
rename to test/processor_imu_2d_tester.h
index 8f9fb23b9944a7a6caf10b28f16ced52066ddaff..647956b4e7bfceffb226c9a2610feb4a54c60dde 100644
--- a/test/processor_imu2d_tester.h
+++ b/test/processor_imu_2d_tester.h
@@ -21,7 +21,7 @@
 #pragma once
 
 // Wolf
-#include "imu/processor/processor_imu2d.h"
+#include "imu/processor/processor_imu_2d.h"
 #include "core/processor/processor_motion.h"
 
 namespace wolf {
diff --git a/test/processor_imu_tester.cpp b/test/processor_imu_3d_tester.cpp
similarity index 84%
rename from test/processor_imu_tester.cpp
rename to test/processor_imu_3d_tester.cpp
index 35d0bc21e6842035204a8ad5d700899fa3fa546e..03328b5a304a6abc22a743358d9b9cdfd752a56f 100644
--- a/test/processor_imu_tester.cpp
+++ b/test/processor_imu_3d_tester.cpp
@@ -18,16 +18,16 @@
 // 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/>.
 
-#include "processor_imu_tester.h"
+#include "processor_imu_3d_tester.h"
 
 namespace wolf {
 
-ProcessorImuTester::ProcessorImuTester(const YAML::Node& _params) :
-        ProcessorImu(_params),
+ProcessorImu3dTester::ProcessorImu3dTester(const YAML::Node& _params) :
+        ProcessorImu3d(_params),
         Dq_out_(nullptr)
 {}
 
-ProcessorImuTester::~ProcessorImuTester(){}
+ProcessorImu3dTester::~ProcessorImu3dTester(){}
 
 } // namespace wolf
 
diff --git a/test/processor_imu_tester.h b/test/processor_imu_3d_tester.h
similarity index 97%
rename from test/processor_imu_tester.h
rename to test/processor_imu_3d_tester.h
index ea941f55d1f0bc603092707e959fec5111972907..0072b4d1dae820965677686340c8fa76d3bb8d1b 100644
--- a/test/processor_imu_tester.h
+++ b/test/processor_imu_3d_tester.h
@@ -21,7 +21,7 @@
 #pragma once
 
 // Wolf
-#include "imu/processor/processor_imu.h"
+#include "imu/processor/processor_imu_3d.h"
 #include "core/processor/processor_motion.h"
 
 namespace wolf {
@@ -165,12 +165,12 @@ namespace wolf {
             }
     };
 
-    class ProcessorImuTester : public ProcessorImu{
+    class ProcessorImu3dTester : public ProcessorImu3d{
 
         public:
 
-        ProcessorImuTester(const YAML::Node& _params);
-        ~ProcessorImuTester() override;
+        ProcessorImu3dTester(const YAML::Node& _params);
+        ~ProcessorImu3dTester() override;
 
         //Functions to test jacobians with finite difference method
 
@@ -220,7 +220,7 @@ namespace wolf {
 namespace wolf{
 
     //Functions to test jacobians with finite difference method
-inline Imu_jac_bias ProcessorImuTester::finite_diff_ab(const double _dt,
+inline Imu_jac_bias ProcessorImu3dTester::finite_diff_ab(const double _dt,
                                                             Eigen::Vector6d& _data,
                                                             const double& da_b,
                                                             const Eigen::Matrix<double,10,1>& _delta_preint0)
@@ -287,7 +287,7 @@ inline Imu_jac_bias ProcessorImuTester::finite_diff_ab(const double _dt,
     return bias_jacobians;
 }
 
-inline Imu_jac_deltas ProcessorImuTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
+inline Imu_jac_deltas ProcessorImu3dTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
 {
     //we do not propagate any noise from data vector
     Eigen::VectorXd Delta_; //will contain the preintegrated Delta affected by added noise
diff --git a/test/yaml/imu_mocap_fusion/processor_imu.yaml b/test/yaml/imu_mocap_fusion/processor_imu_3d.yaml
similarity index 95%
rename from test/yaml/imu_mocap_fusion/processor_imu.yaml
rename to test/yaml/imu_mocap_fusion/processor_imu_3d.yaml
index bc011b0aff7d61c9f0a569ac7b45fbcdca972b5b..ceb0ec1e48fc486c4d36e9c613aa8cb5986123cf 100644
--- a/test/yaml/imu_mocap_fusion/processor_imu.yaml
+++ b/test/yaml/imu_mocap_fusion/processor_imu_3d.yaml
@@ -1,6 +1,6 @@
 name: "cool processor imu"
 plugin: imu
-type: ProcessorImu
+type: ProcessorImu3d
 
 time_tolerance: 0.0025         # Time tolerance for joining KFs
 unmeasured_perturbation_std: 0.00001
diff --git a/test/yaml/imu_mocap_fusion/sensor_imu.yaml b/test/yaml/imu_mocap_fusion/sensor_imu_3d.yaml
similarity index 100%
rename from test/yaml/imu_mocap_fusion/sensor_imu.yaml
rename to test/yaml/imu_mocap_fusion/sensor_imu_3d.yaml
diff --git a/test/yaml/problem_3d_gtest_feature_imu.yaml b/test/yaml/problem_3d_gtest_feature_imu.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..e8da703dda9950e16951363c130d16ae405cac66
--- /dev/null
+++ b/test/yaml/problem_3d_gtest_feature_imu.yaml
@@ -0,0 +1,28 @@
+problem:
+  dimension: 3
+  first_frame:
+    P:
+      state: [0,0,0]
+      mode: "factor"
+      noise_std: [1, 1, 1]
+    O: 
+      state: [0,0,0,1]
+      mode: "factor"
+      noise_std: [1, 1, 1]
+    V: 
+      state: [0,0,0]
+      mode: "factor"
+      noise_std: [1, 1, 1]
+  tree_manager: 
+    type: "None"
+
+map:
+  type: "MapBase"
+  plugin: "core"
+
+sensors: 
+  - follow: sensor_imu_3d.yaml
+    
+processors:
+  - follow: processor_imu_3d.yaml
+    sensor_name: "cool sensor imu"
\ No newline at end of file
diff --git a/test/yaml/processor_imu.yaml b/test/yaml/processor_imu.yaml
deleted file mode 100644
index 7de284af7f8c8680abeb11dd4afaa233e8e61426..0000000000000000000000000000000000000000
--- a/test/yaml/processor_imu.yaml
+++ /dev/null
@@ -1,17 +0,0 @@
-name: "cool processor imu"
-plugin: imu
-type: ProcessorImu
-
-time_tolerance: 0.0025         # Time tolerance for joining KFs
-unmeasured_perturbation_std: 0.00001
-apply_loss_function: false
-
-keyframe_vote:
-    voting_active:      false
-    max_time_span:      2.0   # seconds
-    max_buff_length:  20000    # motion deltas
-    max_dist_traveled:      2.0   # meters
-    max_angle_turned:       0.2   # radians (1 rad approx 57 deg, approx 60 deg)
-
-state_provider: true
-state_provider_order: 1
diff --git a/test/yaml/processor_imu2d.yaml b/test/yaml/processor_imu_2d.yaml
similarity index 100%
rename from test/yaml/processor_imu2d.yaml
rename to test/yaml/processor_imu_2d.yaml
diff --git a/test/yaml/processor_imu2d_static_init.yaml b/test/yaml/processor_imu_2d_static_init.yaml
similarity index 100%
rename from test/yaml/processor_imu2d_static_init.yaml
rename to test/yaml/processor_imu_2d_static_init.yaml
diff --git a/test/yaml/processor_imu_3d.yaml b/test/yaml/processor_imu_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..bcc77ad5c352c8ef97ac10ce0814d77eb35365f7
--- /dev/null
+++ b/test/yaml/processor_imu_3d.yaml
@@ -0,0 +1,20 @@
+name: "cool processor imu"
+plugin: imu
+type: ProcessorImu3d
+
+time_tolerance: 0.0025         # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.00001
+apply_loss_function: false
+
+keyframe_vote:
+  voting_active:      false
+  max_time_span:      2.0   # seconds
+  max_buff_length:    20000    # motion deltas
+  max_dist_traveled:  2.0   # meters
+  max_angle_turned:   0.2   # radians (1 rad approx 57 deg, approx 60 deg)
+
+state_provider: true
+state_provider_order: 1
+
+bootstrap:
+  enabled: false
\ No newline at end of file
diff --git a/test/yaml/processor_imu_static_init.yaml b/test/yaml/processor_imu_3d_static_init.yaml
similarity index 84%
rename from test/yaml/processor_imu_static_init.yaml
rename to test/yaml/processor_imu_3d_static_init.yaml
index 7cf7350a1d5de35cc41402e00124721a5505a455..c1b733c3b96b6ac23626cf63127f4b1dcc08d671 100644
--- a/test/yaml/processor_imu_static_init.yaml
+++ b/test/yaml/processor_imu_3d_static_init.yaml
@@ -1,6 +1,6 @@
 name: "cool processor imu"
 plugin: imu
-type: ProcessorImu
+type: ProcessorImu3d
 
 time_tolerance: 0.0025         # Time tolerance for joining KFs
 unmeasured_perturbation_std: 0.0001
@@ -15,3 +15,8 @@ keyframe_vote:
 
 state_provider: true
 state_provider_order: 1
+
+bootstrap:
+    enabled: true
+    method: STATIC
+    averaging_length: 10
\ No newline at end of file
diff --git a/test/yaml/sensor_imu2d.yaml b/test/yaml/sensor_imu_2d.yaml
similarity index 100%
rename from test/yaml/sensor_imu2d.yaml
rename to test/yaml/sensor_imu_2d.yaml
diff --git a/test/yaml/sensor_imu2d_with_gravity.yaml b/test/yaml/sensor_imu_2d_with_gravity.yaml
similarity index 100%
rename from test/yaml/sensor_imu2d_with_gravity.yaml
rename to test/yaml/sensor_imu_2d_with_gravity.yaml
diff --git a/test/yaml/sensor_imu.yaml b/test/yaml/sensor_imu_3d.yaml
similarity index 97%
rename from test/yaml/sensor_imu.yaml
rename to test/yaml/sensor_imu_3d.yaml
index 072a77156a456f6fb765b78ee1d084dc9c754cda..19bd9b31b43c2f12842c563ac6512cc408ccccde 100644
--- a/test/yaml/sensor_imu.yaml
+++ b/test/yaml/sensor_imu_3d.yaml
@@ -1,6 +1,6 @@
 name: "cool sensor imu"
 plugin: imu
-type: SensorImu
+type: SensorImu3d
 
 states:
   P:
diff --git a/test/yaml/solver_ceres.yaml b/test/yaml/solver_ceres.yaml
index 0bc74798825c7884edb203ea20e87b11e84e62be..ad1f2ec81b4f315144d0c71e12be0bbc3b9e8131 100644
--- a/test/yaml/solver_ceres.yaml
+++ b/test/yaml/solver_ceres.yaml
@@ -6,11 +6,11 @@ verbose: 2
 interrupt_on_problem_change: false
 max_num_iterations: 100
 n_threads: 2
-function_tolerance: 0.0000000001
-gradient_tolerance: 0.0000000001
+function_tolerance: 0.00000000001
+gradient_tolerance: 0.00000000001
 minimizer: "LEVENBERG_MARQUARDT"
 use_nonmonotonic_steps: false         # only for LEVENBERG_MARQUARDT and DOGLEG
-max_consecutive_nonmonotonic_steps: 5 # only if use_nonmonotonic_steps = true
-compute_cov: true
-cov_period: 1                         # only if compute_cov
-cov_enum: 2                           # only if compute_cov
\ No newline at end of file
+# max_consecutive_nonmonotonic_steps: 5 # only if use_nonmonotonic_steps = true
+compute_cov: false
+# cov_period: 1                         # only if compute_cov
+# cov_enum: 2                           # only if compute_cov
\ No newline at end of file