diff --git a/include/IMU/sensor/sensor_IMU.h b/include/IMU/sensor/sensor_IMU.h
index b70d25aab736531beb58faebfa6d6c9089b08530..17b451ff3fd4bb48e5d0de20df3b24e78f366011 100644
--- a/include/IMU/sensor/sensor_IMU.h
+++ b/include/IMU/sensor/sensor_IMU.h
@@ -8,10 +8,10 @@
 
 namespace wolf {
 
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsIMU);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorIMU);
 
 
-struct IntrinsicsIMU : public IntrinsicsBase
+struct ParamsSensorIMU : public ParamsSensorBase
 {
     //noise std dev
     double w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
@@ -25,13 +25,13 @@ struct IntrinsicsIMU : public IntrinsicsBase
     double ab_rate_stdev = 0.00001;
     double wb_rate_stdev = 0.00001;
 
-    virtual ~IntrinsicsIMU() = default;
-    IntrinsicsIMU()
+    virtual ~ParamsSensorIMU() = default;
+    ParamsSensorIMU()
     {
         //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
     }
-    IntrinsicsIMU(std::string _unique_name, const ParamsServer& _server):
-        IntrinsicsBase(_unique_name, _server)
+    ParamsSensorIMU(std::string _unique_name, const ParamsServer& _server):
+        ParamsSensorBase(_unique_name, _server)
     {
         w_noise             = _server.getParam<double>(prefix + _unique_name + "/w_noise");
         a_noise             = _server.getParam<double>(prefix + _unique_name + "/a_noise");
@@ -42,7 +42,7 @@ struct IntrinsicsIMU : public IntrinsicsBase
     }
     std::string print() const
     {
-        return "\n" + IntrinsicsBase::print()                           + "\n"
+        return "\n" + ParamsSensorBase::print()                           + "\n"
             + "w_noise: "           + std::to_string(w_noise)           + "\n"
             + "a_noise: "           + std::to_string(a_noise)           + "\n"
             + "ab_initial_stdev: "  + std::to_string(ab_initial_stdev)  + "\n"
@@ -69,9 +69,9 @@ class SensorIMU : public SensorBase
 
     public:
 
-        SensorIMU(const Eigen::VectorXd& _extrinsics, const IntrinsicsIMU& _params);
-        SensorIMU(const Eigen::VectorXd& _extrinsics, IntrinsicsIMUPtr _params);
-        WOLF_SENSOR_CREATE(SensorIMU, IntrinsicsIMU, 7);
+        SensorIMU(const Eigen::VectorXd& _extrinsics, const ParamsSensorIMU& _params);
+        SensorIMU(const Eigen::VectorXd& _extrinsics, ParamsSensorIMUPtr _params);
+        WOLF_SENSOR_CREATE(SensorIMU, ParamsSensorIMU, 7);
 
         double getGyroNoise() const;
         double getAccelNoise() const;
@@ -81,6 +81,7 @@ class SensorIMU : public SensorBase
         double getAbRateStdev() const;
 
         virtual ~SensorIMU();
+
 };
 
 inline double SensorIMU::getGyroNoise() const
diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp
index deb652ce43bc32f52be45a81403b6b79d5dff84f..cb42c09812018b8fa31f40cce479627f8098850c 100644
--- a/src/sensor/sensor_IMU.cpp
+++ b/src/sensor/sensor_IMU.cpp
@@ -4,13 +4,13 @@
 
 namespace wolf {
 
-SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, IntrinsicsIMUPtr _params) :
+SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, ParamsSensorIMUPtr _params) :
         SensorIMU(_extrinsics, *_params)
 {
     //
 }
 
-SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, const IntrinsicsIMU& _params) :
+SensorIMU::SensorIMU(const Eigen::VectorXd& _extrinsics, const ParamsSensorIMU& _params) :
         SensorBase("SensorIMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6d()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, false, true),
         a_noise(_params.a_noise),
         w_noise(_params.w_noise),
diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp
index ab67315b6e3c09bd431b0f88c3bcfb62bdc29078..89d7c382f48d8202fa439009ba9ff3a5191cb3e9 100644
--- a/src/yaml/sensor_IMU_yaml.cpp
+++ b/src/yaml/sensor_IMU_yaml.cpp
@@ -21,7 +21,7 @@ namespace wolf
 namespace
 {
 
-static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_yaml)
+static ParamsSensorBasePtr createParamsSensorIMU(const std::string & _filename_dot_yaml)
 {
     YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
 
@@ -30,7 +30,7 @@ static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_y
         YAML::Node variances        = config["motion_variances"];
         YAML::Node kf_vote          = config["keyframe_vote"];
 
-        IntrinsicsIMUPtr params = std::make_shared<IntrinsicsIMU>();
+        ParamsSensorIMUPtr params = std::make_shared<ParamsSensorIMU>();
 
         params->a_noise             = variances["a_noise"]          .as<double>();
         params->w_noise             = variances["w_noise"]          .as<double>();
@@ -47,7 +47,7 @@ static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_y
 }
 
 // Register in the SensorFactory
-const bool WOLF_UNUSED registered_imu_intr = IntrinsicsFactory::get().registerCreator("SensorIMU", createIntrinsicsIMU);
+const bool WOLF_UNUSED registered_imu_intr = ParamsSensorFactory::get().registerCreator("SensorIMU", createParamsSensorIMU);
 
 } // namespace [unnamed]
 
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 266bdfb1e5e5e3faf8adf566570285b0be5eee57..a6a1d9cd781eeb9d1c3349e7db9794e5c57d1aea 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -14,6 +14,7 @@ target_link_libraries(gtest_example ${PLUGIN_NAME})      #
 ###########################################################
 
 wolf_add_gtest(gtest_processor_IMU gtest_processor_IMU.cpp)
+message("WHAT IS HERE ${PLUGIN_NAME} ${wolf_LIBRARY}")
 target_link_libraries(gtest_processor_IMU ${PLUGIN_NAME} ${wolf_LIBRARY})
 
 wolf_add_gtest(gtest_IMU gtest_IMU.cpp)
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 73d5145e89df89adf717dea14d8248263b340691..ba537cc567b516cbc4ee5470644b2d059296e372 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -24,6 +24,7 @@ class FeatureIMU_test : public testing::Test
         wolf::FrameBasePtr origin_frame;
         Eigen::MatrixXd dD_db_jacobians;
         wolf::ProcessorBasePtr processor_ptr_;
+        wolf::ProcessorMotionPtr processor_motion_ptr_;
 
     //a new of this will be created for each new test
     virtual void SetUp()
@@ -37,7 +38,7 @@ class FeatureIMU_test : public testing::Test
         problem = Problem::create("POV", 3);
         Eigen::VectorXd IMU_extrinsics(7);
         IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
-        IntrinsicsIMUPtr sen_imu_params = std::make_shared<IntrinsicsIMU>();
+        ParamsSensorIMUPtr sen_imu_params = std::make_shared<ParamsSensorIMU>();
         SensorBasePtr sensor_ptr = problem->installSensor("SensorIMU", "Main IMU", IMU_extrinsics, sen_imu_params);
         ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
         prc_imu_params->max_time_span   = 0.5;
@@ -45,6 +46,7 @@ class FeatureIMU_test : public testing::Test
         prc_imu_params->dist_traveled   = 5;
         prc_imu_params->angle_turned    = 0.5;
         processor_ptr_ = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
+        processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr_);
 
     // Time and data variables
         TimeStamp t;
@@ -80,16 +82,15 @@ class FeatureIMU_test : public testing::Test
         sensor_ptr->process(imu_ptr);
 
     //emplace Frame
-        ts = problem->getProcessorMotion()->getBuffer().get().back().ts_;
-        state_vec = problem->getProcessorMotion()->getCurrentState();
+        problem->getCurrentStateAndStamp(state_vec, ts);
         last_frame = problem->emplaceFrame(KEY, state_vec, ts);
 
     //emplace a feature
-        delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_;
-        delta_preint_cov = problem->getProcessorMotion()->getMotion().delta_integr_cov_ + MatrixXd::Identity(9,9)*1e-08;
-        VectorXd calib_preint = problem->getProcessorMotion()->getLast()->getCalibrationPreint();
-        dD_db_jacobians = problem->getProcessorMotion()->getMotion().jacobian_calib_;
-        feat_imu = std::static_pointer_cast<FeatureIMU>(
+        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                = std::static_pointer_cast<FeatureIMU>(
                 FeatureBase::emplace<FeatureIMU>(imu_ptr,
                                                  delta_preint,
                                                  delta_preint_cov,
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp
index 08d8342b7f86399ec281d74272278a5206641dac..1f059b193f8e5fbf5f96f5024543223ffa0840c2 100644
--- a/test/gtest_processor_IMU.cpp
+++ b/test/gtest_processor_IMU.cpp
@@ -30,6 +30,7 @@ class ProcessorIMUt : public testing::Test
     public: //These can be accessed in fixtures
         wolf::ProblemPtr problem;
         wolf::SensorBasePtr sensor_ptr;
+        wolf::ProcessorMotionPtr processor_motion;
         wolf::TimeStamp t;
         double mti_clock, tmp;
         double dt;
@@ -55,6 +56,7 @@ class ProcessorIMUt : public testing::Test
         Vector7d extrinsics = (Vector7d() << 0,0,0, 0,0,0,1).finished();
         sensor_ptr = problem->installSensor("SensorIMU", "Main IMU", extrinsics,  wolf_root + "/demos/sensor_imu.yaml");
         ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorIMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml");
+        processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
 
         // Time and data variables
         data = Vector6d::Zero();
@@ -221,8 +223,8 @@ TEST_F(ProcessorIMUt, acc_x)
     Vector6d b; b << 0,0,0, 0,0,0;
 
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-    ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
-    ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
 }
 
 TEST_F(ProcessorIMUt, acc_y)
@@ -247,8 +249,8 @@ TEST_F(ProcessorIMUt, acc_y)
     Vector6d b; b<< 0,0,0, 0,0,0;
 
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-    ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
-    ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
 
     //do so for 5s
     const unsigned int iter = 1000; //how many ms
@@ -261,8 +263,8 @@ TEST_F(ProcessorIMUt, acc_y)
     // advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
     x << 0,10,0, 0,0,0,1, 0,20,0;
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-    ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS);
-    ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
+    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibration() , b, wolf::Constants::EPS);
+    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
 }
 
 TEST_F(ProcessorIMUt, acc_z)
@@ -284,8 +286,8 @@ TEST_F(ProcessorIMUt, acc_z)
     Vector6d b; b<< 0,0,0, 0,0,0;
 
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-    ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
-    ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
 
     //do so for 5s
     const unsigned int iter = 50; //how 0.1s 
@@ -298,8 +300,8 @@ TEST_F(ProcessorIMUt, acc_z)
     // advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
     x << 0,0,25, 0,0,0,1, 0,0,10;
     ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-    ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS);
-    ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
+    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibration() , b, wolf::Constants::EPS);
+    ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
 }
 
 TEST_F(ProcessorIMUt, check_Covariance)
@@ -315,8 +317,8 @@ TEST_F(ProcessorIMUt, check_Covariance)
     cap_imu_ptr->setTimeStamp(0.1);
     sensor_ptr->process(cap_imu_ptr);
 
-    VectorXd delta_preint(problem->getProcessorMotion()->getMotion().delta_integr_);
-//    Matrix<double,9,9> delta_preint_cov = problem->getProcessorMotion()->getCurrentDeltaPreintCov();
+    VectorXd delta_preint(processor_motion->getMotion().delta_integr_);
+//    Matrix<double,9,9> delta_preint_cov = processor_motion->getCurrentDeltaPreintCov();
 
     ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
 //    ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXd::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
@@ -382,8 +384,8 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
     // init things
     problem->setPrior(x0, P0, t, 0.01);
 
-    problem->getProcessorMotion()->getOrigin()->setCalibration(bias);
-    problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias);
+    processor_motion->getOrigin()->setCalibration(bias);
+    processor_motion->getLast()->setCalibrationPreint(bias);
 //    WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
 
     // data
@@ -438,8 +440,8 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
     Vector6d bias; bias << abx,aby,0,  0,0,0;
     Vector3d acc_bias = bias.head(3);
 
-    problem->getProcessorMotion()->getOrigin()->setCalibration(bias);
-    problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias);
+    processor_motion->getOrigin()->setCalibration(bias);
+    processor_motion->getLast()->setCalibrationPreint(bias);
 
     double rate_of_turn = 5 * M_PI/180.0;
 //    data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity