diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index 766cf7bf5f6460355ef723dc50125d5f37ac2f51..07f06456f291ff014b2466aed1c8230086f92621 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -49,7 +49,7 @@ struct ParamsProcessorImu : public ParamsProcessorMotion
         ParamsProcessorMotion(_n)
     {
         bootstrap_enable = _n["bootstrap"]["enable"].as<bool>();
-        if (_n["bootstrap"]["method"])
+        if (bootstrap_enable)
         {
             string str = _n["bootstrap"]["method"].as<string>();
             std::transform(str.begin(), str.end(), str.begin(), ::toupper);
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index 87865841c71d6494756c9ffc27ca0b7ace9607fa..7b207bfc9c5be67b9013b557472871c89de8b0ab 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/include/imu/sensor/sensor_imu.h
@@ -80,10 +80,6 @@ class SensorImu : public SensorBase
 
         double getGyroNoise() const;
         double getAccelNoise() const;
-        double getWbInitialStdev() const;
-        double getAbInitialStdev() const;
-        double getWbRateStdev() const;
-        double getAbRateStdev() const;
         bool   isGravityOrthogonal() const;
 
         Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override;
@@ -109,7 +105,7 @@ inline double SensorImu<DIM>::getAccelNoise() const
 template <unsigned int DIM>
 inline bool SensorImu<DIM>::isGravityOrthogonal() const
 {
-    return params_imu_->orthogonal_gravity;
+    return DIM==2 and params_imu_->orthogonal_gravity;
 }
 
 template <>
diff --git a/schema/processor/ProcessorCompass.schema b/schema/processor/ProcessorCompass.schema
new file mode 100644
index 0000000000000000000000000000000000000000..3c97f7469fc38b34aa0998e8c96fc47d177e4aa9
--- /dev/null
+++ b/schema/processor/ProcessorCompass.schema
@@ -0,0 +1 @@
+follow: ProcessorBase.schema
\ No newline at end of file
diff --git a/schema/processor/ProcessorImu.schema b/schema/processor/ProcessorImu.schema
new file mode 100644
index 0000000000000000000000000000000000000000..ee5f2cd9d433ba713f59c492fbb060928484db30
--- /dev/null
+++ b/schema/processor/ProcessorImu.schema
@@ -0,0 +1,17 @@
+follow: ProcessorMotion.schema
+bootstrap:
+  enable:
+    _mandatory: true
+    _type: bool
+    _doc: If any boostrap mechanism is enabled for the processor.
+  method:
+    _mandatory: false
+    _type: string
+    _options: ["STATIC", "G", "V0_G"]
+    _doc: (only if enable==true) Boostrap method.
+  averaging_length:
+    _mandatory: false
+    _type: double
+    _doc: Averaging length used by the bootstrap mechanism.
+
+  
\ No newline at end of file
diff --git a/schema/processor/ProcessorImu2d.schema b/schema/processor/ProcessorImu2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..3823a081464db632c04599be4818e27240e98929
--- /dev/null
+++ b/schema/processor/ProcessorImu2d.schema
@@ -0,0 +1 @@
+follow: ProcessorMotion.schema
\ No newline at end of file
diff --git a/schema/sensor/SensorCompass.schema b/schema/sensor/SensorCompass.schema
new file mode 100644
index 0000000000000000000000000000000000000000..84aeb60585725bfe1073f226c0fa075c3d9fc4d7
--- /dev/null
+++ b/schema/sensor/SensorCompass.schema
@@ -0,0 +1,58 @@
+follow: SensorBase.schema
+
+stdev_noise:
+  _mandatory: true
+  _type: double
+  _doc: standard deviation of the compass measurements (square root of the covariance matrix diagonal).
+
+states:
+  P:
+    follow: SpecStateSensorP3d.schema
+  O:
+    follow: SpecStateSensorO3d.schema
+  I:
+    follow: SpecStateSensor.schema
+    type:
+      _type: string
+      _mandatory: false
+      _default: StateParams3
+      _options: [StateParams3]
+      _doc: The derived type of the StateBlock for compass biases
+    state:
+      _type: Vector3d
+      _mandatory: true
+      _doc: The initial values of the compass biases
+    dynamic:
+      _type: bool
+      _mandatory: true
+      _doc: If the bias are dynamic, i.e. they change along time.
+    noise_std:
+      _type: Vector3d
+      _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
+      _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.
+  F:
+    follow: SpecStateSensor.schema
+    type:
+      _type: string
+      _mandatory: false
+      _default: StateParams3
+      _options: [StateParams3]
+      _doc: The derived type of the StateBlock for magnetic field
+    state:
+      _type: Vector3d
+      _mandatory: true
+      _doc: The initial values of the compass biases
+    dynamic:
+      _type: bool
+      _mandatory: false
+      _default: false
+      _options: [false]
+      _doc: The magnetic field is not dynamic.
+    noise_std:
+      _type: Vector3d
+      _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.
diff --git a/schema/sensor/SensorImu2d.schema b/schema/sensor/SensorImu2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..0be0668ebaba53bfda68ad4499cf7273b7ac5d4a
--- /dev/null
+++ b/schema/sensor/SensorImu2d.schema
@@ -0,0 +1,43 @@
+follow: SensorBase.schema
+
+w_noise:
+  _mandatory: true
+  _type: double
+  _doc: standard deviation of the gyroscope measurements (square root of the covariance matrix diagonal).
+
+a_noise:
+  _mandatory: true
+  _type: double
+  _doc: standard deviation of the accelerometer measurements (square root of the covariance matrix diagonal).
+
+states:
+  P:
+    follow: SpecStateSensorP2d.schema
+  O:
+    follow: SpecStateSensorO2d.schema
+  I:
+    follow: SpecStateSensor.schema
+    type:
+      _type: string
+      _mandatory: false
+      _default: StateParams3
+      _options: [StateParams3]
+      _doc: The derived type of the StateBlock for IMU biases
+    state:
+      _type: Vector3d
+      _mandatory: false
+      _default: [0,0,0]
+      _doc: The initial values of the IMU biases
+    dynamic:
+      _type: bool
+      _mandatory: true
+      _doc: If the bias are dynamic, i.e. they change along time.
+    noise_std:
+      _type: Vector3d
+      _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
+      _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.
+
diff --git a/schema/sensor/SensorImu3d.schema b/schema/sensor/SensorImu3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..e484cdf39935998c1e8acee7aa3145930d1dc03c
--- /dev/null
+++ b/schema/sensor/SensorImu3d.schema
@@ -0,0 +1,43 @@
+follow: SensorBase.schema
+
+w_noise:
+  _mandatory: true
+  _type: double
+  _doc: standard deviation of the gyroscope measurements (square root of the covariance matrix diagonal).
+
+a_noise:
+  _mandatory: true
+  _type: double
+  _doc: standard deviation of the accelerometer measurements (square root of the covariance matrix diagonal).
+
+states:
+  P:
+    follow: SpecStateSensorP3d.schema
+  O:
+    follow: SpecStateSensorO3d.schema
+  I:
+    follow: SpecStateSensor.schema
+    type:
+      _type: string
+      _mandatory: false
+      _default: StateParams6
+      _options: [StateParams6]
+      _doc: The derived type of the StateBlock for IMU biases
+    state:
+      _type: Vector6d
+      _mandatory: false
+      _default: [0,0,0,0,0,0]
+      _doc: The initial values of the IMU biases
+    dynamic:
+      _type: bool
+      _mandatory: true
+      _doc: If the bias are dynamic, i.e. they change along time.
+    noise_std:
+      _type: Vector6d
+      _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: Vector6d
+      _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.
+
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index d5809731a5e530177267647f312ea0ff0f977852..27559fb4c62cec4f80ba2543170c7038c6ef0d34 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -111,16 +111,9 @@ void ProcessorImu::setCalibration (const CaptureBasePtr _capture, const VectorXd
 
 void ProcessorImu::configure(SensorBasePtr _sensor) 
 {
-    sensor_imu_ = std::static_pointer_cast<SensorImu3d>(_sensor);
-
-    auto acc_drift_std  = sensor_imu_->getAbRateStdev();
-    auto gyro_drift_std = sensor_imu_->getAbRateStdev();
-
-    ArrayXd imu_drift_sigmas(6);
-    imu_drift_sigmas << acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std;
-    imu_drift_cov_  = imu_drift_sigmas.square().matrix().asDiagonal();
-
+    imu_drift_cov_ = _sensor->getDriftCov('I');
 }
+
 void ProcessorImu::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
 {
     // 1. Feature and factor Imu
diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp
index 8db279c69c55283a3fcf37904ccc04593690067b..466e6f294320518edf2057ba01597eebdfb1a849 100644
--- a/src/processor/processor_imu2d.cpp
+++ b/src/processor/processor_imu2d.cpp
@@ -33,281 +33,275 @@
 
 namespace wolf {
 
-  ProcessorImu2d::ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_imu) :
-    ProcessorMotion("ProcessorImu2d", 
-                    {{'P',"StatePoint2d"},{'O',"StateAngle"},{'V',"StateVector2d"}},
-                    2, 5, 5, 5, 6, 3, 
-                    _params_motion_imu),
-    params_motion_Imu_(std::make_shared<ParamsProcessorImu2d>(*_params_motion_imu))
-  {
-      //
-  }
+ProcessorImu2d::ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_imu) :
+  ProcessorMotion("ProcessorImu2d", 
+                  {{'P',"StatePoint2d"},{'O',"StateAngle"},{'V',"StateVector2d"}},
+                  2, 5, 5, 5, 6, 3, 
+                  _params_motion_imu),
+  params_motion_Imu_(std::make_shared<ParamsProcessorImu2d>(*_params_motion_imu))
+{
+    //
+}
 
-  ProcessorImu2d::~ProcessorImu2d()
+ProcessorImu2d::~ProcessorImu2d()
+{
+  //
+}
+
+void ProcessorImu2d::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 ProcessorImu2d::voteForKeyFrame() const
+{
+  // time span
+  if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_Imu_->max_time_span)
   {
-    //
+    WOLF_DEBUG( "PM: vote: time span" );
+    return true;
   }
-
-  void ProcessorImu2d::preProcess()
+  // buffer length
+  if (getBuffer().size() > params_motion_Imu_->max_buff_length)
   {
-    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());
+    WOLF_DEBUG( "PM: vote: buffer length" );
+    return true;
   }
-
-  bool ProcessorImu2d::voteForKeyFrame() const
+  // angle turned
+  double angle = std::abs(delta_integrated_(2));
+  if (angle > params_motion_Imu_->angle_turned)
   {
-    // time span
-    if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_Imu_->max_time_span)
-    {
-      WOLF_DEBUG( "PM: vote: time span" );
-      return true;
-    }
-    // buffer length
-    if (getBuffer().size() > params_motion_Imu_->max_buff_length)
-    {
-      WOLF_DEBUG( "PM: vote: buffer length" );
-      return true;
-    }
-    // angle turned
-    double angle = std::abs(delta_integrated_(2));
-    if (angle > params_motion_Imu_->angle_turned)
-    {
-      WOLF_DEBUG( "PM: vote: angle turned" );
-      return true;
-    }
-    //WOLF_DEBUG( "PM: do not vote" );
-    return false;
+    WOLF_DEBUG( "PM: vote: angle turned" );
+    return true;
   }
+  //WOLF_DEBUG( "PM: do not vote" );
+  return false;
+}
 
 
-  CaptureMotionPtr ProcessorImu2d::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));
-    setCalibration(cap_motion, _calib);
-    cap_motion->setCalibrationPreint(_calib_preint);
-
-    return cap_motion;
-  }
+CaptureMotionPtr ProcessorImu2d::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));
+  setCalibration(cap_motion, _calib);
+  cap_motion->setCalibrationPreint(_calib_preint);
+
+  return cap_motion;
+}
 
-  VectorXd ProcessorImu2d::getCalibration (const CaptureBaseConstPtr _capture) const
-  {
-      if (_capture)
-          return _capture->getStateBlock('I')->getState();
-      else
-          return getSensor()->getStateBlockDynamic('I')->getState();
-  }
+VectorXd ProcessorImu2d::getCalibration (const CaptureBaseConstPtr _capture) const
+{
+    if (_capture)
+        return _capture->getStateBlock('I')->getState();
+    else
+        return getSensor()->getStateBlockDynamic('I')->getState();
+}
 
-  void ProcessorImu2d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
-  {
+void ProcessorImu2d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+{
     _capture->getSensorIntrinsic()->setState(_calibration);
-  }
+}
+
 void ProcessorImu2d::configure(SensorBasePtr _sensor) 
 {
-    sensor_imu2d_ = std::static_pointer_cast<SensorImu2d>(_sensor);
+    imu_drift_cov_  = _sensor->getDriftCov('I');
+}
 
-    auto acc_drift_std  = sensor_imu2d_->getAbRateStdev();
-    auto gyro_drift_std = sensor_imu2d_->getWbRateStdev();
+void ProcessorImu2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
+{
+    auto feature = FeatureBase::emplace<FeatureImu2d>(
+        _capture_own, _capture_own->getBuffer().back().delta_integr_,
+        _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+        _capture_own->getCalibrationPreint(), _capture_own->getBuffer().back().jacobian_calib_);
 
-    Array3d imu_drift_sigmas(acc_drift_std, acc_drift_std, gyro_drift_std);
-    imu_drift_cov_  = imu_drift_sigmas.square().matrix().asDiagonal();
+    CaptureImuPtr   cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
+    FeatureImu2dPtr ftr_imu = std::static_pointer_cast<FeatureImu2d>(feature);
 
-}
+    if (std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal())
+        FactorBase::emplace<FactorImu2d>(ftr_imu, ftr_imu, cap_imu, shared_from_this(),
+                                          params_->apply_loss_function);
+    else
+        FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu, ftr_imu, cap_imu, shared_from_this(),
+                                                    params_->apply_loss_function);
 
-  void ProcessorImu2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
-  {
-      auto feature = FeatureBase::emplace<FeatureImu2d>(
-          _capture_own, _capture_own->getBuffer().back().delta_integr_,
-          _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
-          _capture_own->getCalibrationPreint(), _capture_own->getBuffer().back().jacobian_calib_);
-
-      CaptureImuPtr   cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
-      FeatureImu2dPtr ftr_imu = std::static_pointer_cast<FeatureImu2d>(feature);
-
-      if (std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal())
-          FactorBase::emplace<FactorImu2d>(ftr_imu, ftr_imu, cap_imu, shared_from_this(),
-                                           params_->apply_loss_function);
-      else
-          FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu, ftr_imu, cap_imu, shared_from_this(),
-                                                      params_->apply_loss_function);
-
-      if (getSensor()->isStateBlockDynamic('I'))
-      {
-          const auto& sb_imubias_own    = _capture_own->getStateBlock('I');
-          const auto& sb_imubias_origin = _capture_origin->getStateBlock('I');
-          if (sb_imubias_own != sb_imubias_origin)  // make sure it's two different state blocks! -- just in case
-          {
-              auto dt       = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
-              auto ftr_bias = FeatureBase::emplace<FeatureBase>(
-                  _capture_own, "FeatureBase",
-                  Vector3d::Zero(),      // mean IMU drift is zero
-                  imu_drift_cov_ * dt);  // IMU drift cov specified in continuous time
-              auto fac_bias =
-                  FactorBase::emplace<FactorBlockDifference>(ftr_bias, ftr_bias,
-                                                             sb_imubias_own,      // IMU bias block at t=own
-                                                             sb_imubias_origin,   // IMU bias block at t=origin
-                                                             nullptr,             // frame other
-                                                             _capture_origin,     // origin capture
-                                                             nullptr,             // feature other
-                                                             nullptr,             // landmark other
-                                                             0,                   // take all of first state block
-                                                             -1,                  // take all of first state block
-                                                             0,                   // take all of first second block
-                                                             -1,                  // take all of first second block
-                                                             shared_from_this(),  // this processor
-                                                             params_->apply_loss_function);  // loss function
-
-              if( std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal() ) 
-                FactorBase::emplace<FactorImu2d>(ftr_imu, 
-                                                 ftr_imu, 
-                                                 cap_imu, 
-                                                 shared_from_this(),
-                                                 params_->apply_loss_function);
-              else 
-                FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu,
-                                                            ftr_imu,
-                                                            cap_imu, 
-                                                            shared_from_this(), 
-                                                            params_->apply_loss_function);
-          }
-      }
-  }
+    if (getSensor()->isStateBlockDynamic('I'))
+    {
+        const auto& sb_imubias_own    = _capture_own->getStateBlock('I');
+        const auto& sb_imubias_origin = _capture_origin->getStateBlock('I');
+        if (sb_imubias_own != sb_imubias_origin)  // make sure it's two different state blocks! -- just in case
+        {
+            auto dt       = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
+            auto ftr_bias = FeatureBase::emplace<FeatureBase>(
+                _capture_own, "FeatureBase",
+                Vector3d::Zero(),      // mean IMU drift is zero
+                imu_drift_cov_ * dt);  // IMU drift cov specified in continuous time
+            auto fac_bias =
+                FactorBase::emplace<FactorBlockDifference>(ftr_bias, ftr_bias,
+                                                            sb_imubias_own,      // IMU bias block at t=own
+                                                            sb_imubias_origin,   // IMU bias block at t=origin
+                                                            nullptr,             // frame other
+                                                            _capture_origin,     // origin capture
+                                                            nullptr,             // feature other
+                                                            nullptr,             // landmark other
+                                                            0,                   // take all of first state block
+                                                            -1,                  // take all of first state block
+                                                            0,                   // take all of first second block
+                                                            -1,                  // take all of first second block
+                                                            shared_from_this(),  // this processor
+                                                            params_->apply_loss_function);  // loss function
+
+            if( std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal() ) 
+              FactorBase::emplace<FactorImu2d>(ftr_imu, 
+                                                ftr_imu, 
+                                                cap_imu, 
+                                                shared_from_this(),
+                                                params_->apply_loss_function);
+            else 
+              FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu,
+                                                          ftr_imu,
+                                                          cap_imu, 
+                                                          shared_from_this(), 
+                                                          params_->apply_loss_function);
+        }
+    }
+}
 
-  void ProcessorImu2d::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
-  {
-    using namespace Eigen;
-    assert(_data.size() == data_size_ && "Wrong data size!");
-    Vector3d data_2d;
-    data_2d << _data(0), _data(1), _data(5);
-    Matrix3d data_cov_2d;
-    data_cov_2d << _data_cov(0,0), _data_cov(0,1), _data_cov(0,5),
-                   _data_cov(1,0), _data_cov(1,1), _data_cov(1,5),
-                   _data_cov(5,0), _data_cov(5,1), _data_cov(5,5);
-
-
-
-    Matrix<double, 5, 3> jac_delta_data;
-    /*
-     * We have the following computing pipeline:
-     *     Input: data, calib, dt
-     *     Output: delta, delta_cov, jac_calib
-     *
-     * We do:
-     *     body         = data - calib (measurement - bias)
-     *     delta        = body2delta(body, dt) --> jac_body
-     *     jac_data     = jac_body
-     *     jac_calib    = - jac_body
-     *     delta_cov  <-- propagate data_cov using jac_data
-     *
-     * where body2delta(.) produces a delta from body=(a,w) as follows:
-     *     dp = P * a * dt^2
-     *     dth = exp(w * dt) = w * dt
-     *     dv = Q * a * dt
-     * where P and Q are defined in imu2d::exp(), and also a jacobian J^delta_data
-     */
-
-    //create delta
-    imu2d::body2delta(data_2d - _calib, _dt, _delta, jac_delta_data);
-
-    // compute delta_cov
-    _delta_cov = jac_delta_data * data_cov_2d * jac_delta_data.transpose();
-
-    // compute jacobian_calib
-    _jac_delta_calib = - jac_delta_data;
-  }
+void ProcessorImu2d::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
+{
+  using namespace Eigen;
+  assert(_data.size() == data_size_ && "Wrong data size!");
+  Vector3d data_2d;
+  data_2d << _data(0), _data(1), _data(5);
+  Matrix3d data_cov_2d;
+  data_cov_2d << _data_cov(0,0), _data_cov(0,1), _data_cov(0,5),
+                  _data_cov(1,0), _data_cov(1,1), _data_cov(1,5),
+                  _data_cov(5,0), _data_cov(5,1), _data_cov(5,5);
+
+
+
+  Matrix<double, 5, 3> jac_delta_data;
+  /*
+    * We have the following computing pipeline:
+    *     Input: data, calib, dt
+    *     Output: delta, delta_cov, jac_calib
+    *
+    * We do:
+    *     body         = data - calib (measurement - bias)
+    *     delta        = body2delta(body, dt) --> jac_body
+    *     jac_data     = jac_body
+    *     jac_calib    = - jac_body
+    *     delta_cov  <-- propagate data_cov using jac_data
+    *
+    * where body2delta(.) produces a delta from body=(a,w) as follows:
+    *     dp = P * a * dt^2
+    *     dth = exp(w * dt) = w * dt
+    *     dv = Q * a * dt
+    * where P and Q are defined in imu2d::exp(), and also a jacobian J^delta_data
+    */
+
+  //create delta
+  imu2d::body2delta(data_2d - _calib, _dt, _delta, jac_delta_data);
+
+  // compute delta_cov
+  _delta_cov = jac_delta_data * data_cov_2d * jac_delta_data.transpose();
+
+  // compute jacobian_calib
+  _jac_delta_calib = - jac_delta_data;
+}
 
-  void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
-                                      const Eigen::VectorXd& _delta,
-                                      const double _dt,
-                                      Eigen::VectorXd& _delta_preint_plus_delta) const
-  {
-    /* MATHS:
-     * Simple composition,
-     * (D o d)p   =   Dp + Dth*dp + Dv*dt
-     * (D o d)th  =   Dth+dth
-     * (D o d)v   =   Dv + Dth*dv
-     * where Dth*dp and Dth*dv are rotations and not scalar products. The version with jacobians is not used here.
-     */
-    _delta_preint_plus_delta = imu2d::compose(_delta_preint, _delta, _dt);
-  }
+void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+                                    const Eigen::VectorXd& _delta,
+                                    const double _dt,
+                                    Eigen::VectorXd& _delta_preint_plus_delta) const
+{
+  /* MATHS:
+    * Simple composition,
+    * (D o d)p   =   Dp + Dth*dp + Dv*dt
+    * (D o d)th  =   Dth+dth
+    * (D o d)v   =   Dv + Dth*dv
+    * where Dth*dp and Dth*dv are rotations and not scalar products. The version with jacobians is not used here.
+    */
+  _delta_preint_plus_delta = imu2d::compose(_delta_preint, _delta, _dt);
+}
 
-  void ProcessorImu2d::statePlusDelta(const VectorComposite& _x,
-                                      const Eigen::VectorXd& _delta,
-                                      const double _dt,
-                                      VectorComposite& _x_plus_delta) const
-  {
-    assert(_x.has("POV") && "Any key missing in _x");
-    assert(_delta.size() == 5 && "Wrong _delta vector size");
-    assert(_dt >= 0 && "Time interval _dt is negative!");
-
-    const auto& delta = VectorComposite("POV", _delta, {2,1,2});
-    /*
-     * MATHS:
-     *
-     * In the absence of gravity (2D case) it's the same as deltaPlusDelta(), so the same compose() function is used
-     */
-    if( std::static_pointer_cast<const SensorImu2d>(getSensor())->isGravityOrthogonal() ) 
-        _x_plus_delta = imu2d::composeOverState(_x, delta, _dt);
-    else
-      _x_plus_delta = imu2d::composeOverStateWithGravity(_x, delta, _dt, getSensor()->getState("G"));
+void ProcessorImu2d::statePlusDelta(const VectorComposite& _x,
+                                    const Eigen::VectorXd& _delta,
+                                    const double _dt,
+                                    VectorComposite& _x_plus_delta) const
+{
+  assert(_x.has("POV") && "Any key missing in _x");
+  assert(_delta.size() == 5 && "Wrong _delta vector size");
+  assert(_dt >= 0 && "Time interval _dt is negative!");
+
+  const auto& delta = VectorComposite("POV", _delta, {2,1,2});
+  /*
+    * MATHS:
+    *
+    * In the absence of gravity (2D case) it's the same as deltaPlusDelta(), so the same compose() function is used
+    */
+  if( std::static_pointer_cast<const SensorImu2d>(getSensor())->isGravityOrthogonal() ) 
+      _x_plus_delta = imu2d::composeOverState(_x, delta, _dt);
+  else
+    _x_plus_delta = imu2d::composeOverStateWithGravity(_x, delta, _dt, getSensor()->getState("G"));
 
-  }
+}
 
-  void ProcessorImu2d::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:
-     *
-     *     Dp' = Dp + Dv*dt + Dq*dp
-     *     Dv' = Dv + Dq*dv
-     *     Dq' = Dq * dq
-     *
-     * Jacobians for covariance propagation.
-     *
-     * a. With respect to Delta, gives _jacobian_delta_preint = D'_D as:
-     *
-     *   D'_D = [ I    DR*skew(1)*dp    I*dt   
-     *            0    1                0
-     *            0    DR*skew(1)*dv    I   ] 
-     *
-     * b. With respect to delta, gives _jacobian_delta = D'_d as:
-     *
-     *   D'_d = [ DR   0    0
-     *            0    1    0
-     *            0    0    DR ]
-     *
-     * Note: covariance propagation, i.e.,  P+ = D_D * P * D_D' + D_d * M * D_d', is done in ProcessorMotion.
-     */
-    imu2d::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in imu2d_tools
-  }
+void ProcessorImu2d::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:
+    *
+    *     Dp' = Dp + Dv*dt + Dq*dp
+    *     Dv' = Dv + Dq*dv
+    *     Dq' = Dq * dq
+    *
+    * Jacobians for covariance propagation.
+    *
+    * a. With respect to Delta, gives _jacobian_delta_preint = D'_D as:
+    *
+    *   D'_D = [ I    DR*skew(1)*dp    I*dt   
+    *            0    1                0
+    *            0    DR*skew(1)*dv    I   ] 
+    *
+    * b. With respect to delta, gives _jacobian_delta = D'_d as:
+    *
+    *   D'_d = [ DR   0    0
+    *            0    1    0
+    *            0    0    DR ]
+    *
+    * Note: covariance propagation, i.e.,  P+ = D_D * P * D_D' + D_d * M * D_d', is done in ProcessorMotion.
+    */
+  imu2d::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in imu2d_tools
+}
 
-  Eigen::VectorXd ProcessorImu2d::correctDelta (const Eigen::VectorXd& delta_preint,
-                                                const Eigen::VectorXd& delta_step) const
-  {
-    return imu2d::plus(delta_preint, delta_step);
-  }
+Eigen::VectorXd ProcessorImu2d::correctDelta (const Eigen::VectorXd& delta_preint,
+                                              const Eigen::VectorXd& delta_step) const
+{
+  return imu2d::plus(delta_preint, delta_step);
+}
 
 
 } // namespace wolf
diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu.cpp
index 5509a069b9d8ae5d3faeab4977db2763376d65b6..e10a29b62613ed8ad81433e85576041893a44315 100644
--- a/test/gtest_factor_imu.cpp
+++ b/test/gtest_factor_imu.cpp
@@ -19,12 +19,6 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-/**
- * \file gtest_factor_imu.cpp
- *
- *  Created on: Jan 01, 2017
- *      \author: Dinesh Atchuthan
- */
 
 #include <core/ceres_wrapper/solver_ceres.h>
 #include <core/utils/utils_gtest.h>
@@ -88,8 +82,8 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test
         solver->getSolverOptions().max_num_iterations = 1e4;
 
         // SENSOR + PROCESSOR Imu
-        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml");
-        processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+        SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
+        processor_ = problem->installProcessor("ProcessorImu", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
         sen_imu = static_pointer_cast<SensorImu>(sen0_ptr);
         processor_imu = static_pointer_cast<ProcessorImu>(processor_);
     
diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu2d.cpp
index 8409d49f4e8835dc1627615545e2e5ddffb35f79..1c7f90950bc03d7b1c6c15b9bdc2a3ade6c7a266 100644
--- a/test/gtest_factor_imu2d.cpp
+++ b/test/gtest_factor_imu2d.cpp
@@ -68,11 +68,11 @@ class FactorImu2d_test : public testing::Test
       solver->getSolverOptions().gradient_tolerance = 1e-8;
 
       // Two frames
-      frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero());
-      frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero());
+      frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "POV", Vector5d::Zero());
+      frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero());
 
       // Imu2d sensor
-      sensor = problem_ptr->installSensor("SensorImu", "imu odometer", wolf_root + "/test/yaml/sensor_imu2d.yaml");
+      sensor = problem_ptr->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d.yaml", {wolf_root});
 
       //capture from frm1 to frm0
       cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
diff --git a/test/gtest_factor_imu2d_with_gravity.cpp b/test/gtest_factor_imu2d_with_gravity.cpp
index 3fab496313fccf2215d35dfa4c4aa32d9ba3f0e4..e0c72559fa4ecd0b4fd6a0e0d0c99cadf6571134 100644
--- a/test/gtest_factor_imu2d_with_gravity.cpp
+++ b/test/gtest_factor_imu2d_with_gravity.cpp
@@ -68,11 +68,11 @@ class FactorImu2dwithGravity_test : public testing::Test
       solver->getSolverOptions().gradient_tolerance = 1e-9;
 
       // Two frames
-      frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero());
-      frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero());
+      frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "POV", Vector5d::Zero());
+      frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero());
 
       // Imu2d sensor
-      sensor = problem_ptr->installSensor("SensorImu", "imu odometer", wolf_root + "/test/yaml/sensor_imu2d_with_gravity.yaml");
+      sensor = problem_ptr->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d_with_gravity.yaml", {wolf_root});
 
       //capture from frm1 to frm0
       cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
diff --git a/test/gtest_feature_imu.cpp b/test/gtest_feature_imu.cpp
index b803b67c6528c5ddde87f5194e33027ad5c83a82..567a3a37baa58abd613c8b025eb66fb6678eac37 100644
--- a/test/gtest_feature_imu.cpp
+++ b/test/gtest_feature_imu.cpp
@@ -49,7 +49,6 @@ class FeatureImu_test : public testing::Test
         FrameBasePtr last_frame;
         FrameBasePtr origin_frame;
         MatrixXd dD_db_jacobians;
-        ProcessorBasePtr processor_ptr_;
         ProcessorMotionPtr processor_motion_ptr_;
 
     //a new of this will be created for each new test
@@ -59,17 +58,16 @@ class FeatureImu_test : public testing::Test
     // 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)}};
+                                       {'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 = problem->installSensor("SensorImu", "Main Imu", sen_imu_params, imu_priors);
+        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_ptr_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, prc_imu_params);
-        processor_motion_ptr_ = static_pointer_cast<ProcessorMotion>(processor_ptr_);
+        processor_motion_ptr_ = ProcessorBase::emplace<ProcessorImu>(sensor_ptr, prc_imu_params);
 
     // Time and data variables
         TimeStamp t;
@@ -78,20 +76,23 @@ class FeatureImu_test : public testing::Test
         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);
+        // 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 = static_pointer_cast<CaptureImu>(
-                CaptureBase::emplace<CaptureImu>(origin_frame,
-                                                 t,
-                                                 sensor_ptr,
-                                                 data_,
-                                                 Matrix6d::Identity(),
-                                                 Vector6d::Zero().eval()) );
+        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;
@@ -106,8 +107,8 @@ class FeatureImu_test : public testing::Test
 
     //emplace Frame
         ts          = problem->getTimeStamp();
-        state_vec   = problem->getState().vector(problem->getFrameStructure());
-        last_frame  = problem->emplaceFrame(ts, state_vec);
+        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_;
diff --git a/test/gtest_processor_imu.cpp b/test/gtest_processor_imu.cpp
index a2e9bdaf855412a0ba54ad2b10c32d27615db8a9..8cd96d2c68bcb76d73e6c9d9f55a67065bc6ee89 100644
--- a/test/gtest_processor_imu.cpp
+++ b/test/gtest_processor_imu.cpp
@@ -40,6 +40,19 @@ using std::static_pointer_cast;
 
 std::string wolf_root = _WOLF_IMU_ROOT_DIR;
 
+SpecComposite problem_prior_{{'P', SpecState("StatePoint3d",
+                                             Vector3d::Zero(),
+                                             "factor",
+                                             Vector3d::Ones())},
+                             {'O', SpecState("StateQuaternion",
+                                             Quaterniond::Identity().coeffs(),
+                                             "factor",
+                                             Vector3d::Ones())},
+                             {'V', SpecState("StateVector3d",
+                                             Vector3d::Zero(),
+                                             "factor",
+                                             Vector3d::Ones())}};
+
 class ProcessorImut : public testing::Test
 {
 
@@ -62,8 +75,8 @@ class ProcessorImut : public testing::Test
     {
         // Wolf problem
         problem = Problem::create("POV", 3);
-        sensor_ptr = problem->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu.yaml");
-        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+        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});
         processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
 
         // Time and data variables
@@ -108,18 +121,18 @@ TEST(ProcessorImu_constructors, ALL)
     ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned);
 
     //Factory constructor with pointers
-    std::string wolf_root = _WOLF_IMU_ROOT_DIR;
     ProblemPtr problem = Problem::create("POV", 3);
-    SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu.yaml");
+    SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
     ParamsProcessorImuPtr params_default = std::make_shared<ParamsProcessorImu>();
-    ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, params_default);
+    ProcessorBasePtr processor_ptr = ProcessorBase::emplace<ProcessorImu>(sensor_ptr, params_default);
+    //ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, params_default);
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan(),   params_default->max_time_span);
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxBuffLength(), params_default->max_buff_length);
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getDistTraveled(),  params_default->dist_traveled);
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getAngleTurned(),   params_default->angle_turned);
 
     //Factory constructor with yaml
-    processor_ptr = problem->installProcessor("ProcessorImu", "Sec Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml");
+    processor_ptr = problem->installProcessor("ProcessorImu", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root});
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan(),   2.0);
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxBuffLength(), 20000);
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getDistTraveled(),  2.0);
@@ -130,20 +143,19 @@ TEST(ProcessorImu, voteForKeyFrame)
 {
     // Wolf problem
     ProblemPtr problem = Problem::create("POV", 3);
-    SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu.yaml");
+    SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root});
     ParamsProcessorImuPtr prc_imu_params = std::make_shared<ParamsProcessorImu>();
     prc_imu_params->max_time_span = 1;
     prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
     prc_imu_params->dist_traveled = 1000000000;
     prc_imu_params->angle_turned = 1000000000;
     prc_imu_params->voting_active = true;
-    ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, prc_imu_params);
+    auto processor_ptr = ProcessorBase::emplace<ProcessorImu>(sensor_ptr, prc_imu_params);
+    //ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, prc_imu_params);
     
     //setting origin
     TimeStamp t(0);
-    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)});
-    problem->setPriorFactor(x0, s0, t);
+    problem->setPrior(problem_prior_, t);
 
     //data variable and covariance matrix
     // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
@@ -205,13 +217,14 @@ TEST(ProcessorImu, voteForKeyFrame)
 TEST_F(ProcessorImut, acc_x)
 {
     t.set(0); // clock in 0,1 ms ticks
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -227,7 +240,7 @@ TEST_F(ProcessorImut, acc_x)
     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
     Vector6d b; b << 0,0,0, 0,0,0;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS_SMALL);
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS_SMALL);
 }
@@ -238,11 +251,13 @@ TEST_F(ProcessorImut, acc_y)
     // difference hier is that we integrate over 1ms periods
 
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -258,7 +273,7 @@ TEST_F(ProcessorImut, acc_y)
     x << 0,0.00001,0, 0,0,0,1, 0,0.02,0; // advanced at a=20m/s2 during 0.001s ==> dx = 0.5*20*0.001^2 = 0.00001; dvx = 20*0.001 = 0.02
     Vector6d b; b<< 0,0,0, 0,0,0;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS_SMALL);
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS_SMALL);
 
@@ -272,7 +287,7 @@ 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->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS);
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS);
 }
@@ -280,11 +295,13 @@ TEST_F(ProcessorImut, acc_y)
 TEST_F(ProcessorImut, acc_z)
 {
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -300,7 +317,7 @@ TEST_F(ProcessorImut, acc_z)
     x << 0,0,0.01, 0,0,0,1, 0,0,0.2; // advanced at a=2m/s2 during 0.1s ==> dz = 0.5*2*0.1^2 = 0.01; dvz = 2*0.1 = 0.2
     Vector6d b; b<< 0,0,0, 0,0,0;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV").head(10) , x, Constants::EPS_SMALL);
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS_SMALL);
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS_SMALL);
 
@@ -314,7 +331,7 @@ 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->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS);
     ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS);
 }
@@ -322,11 +339,13 @@ TEST_F(ProcessorImut, acc_z)
 TEST_F(ProcessorImut, check_Covariance)
 {
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -348,11 +367,13 @@ TEST_F(ProcessorImut, gyro_x)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -371,7 +392,7 @@ TEST_F(ProcessorImut, gyro_x)
     VectorXd x(10);
     x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
 
     //do so for 5s
     const unsigned int iter = 1000; //how many ms 
@@ -380,7 +401,7 @@ TEST_F(ProcessorImut, gyro_x)
         // quaternion composition
         quat_comp = quat_comp * v2q(data.tail(3)*dt);
 
-        Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3);
+        Quaterniond rot(problem->getState().vector("O").data());
         data.head(3) =  rot.conjugate() * (- gravity());
 
         cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -389,7 +410,7 @@ TEST_F(ProcessorImut, gyro_x)
     }
 
     x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
 TEST_F(ProcessorImut, gyro_x_biasedAbx)
@@ -407,9 +428,11 @@ TEST_F(ProcessorImut, gyro_x_biasedAbx)
     MatrixXd P0(9,9); P0.setIdentity();
 
     // init things
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -434,7 +457,7 @@ TEST_F(ProcessorImut, gyro_x_biasedAbx)
     x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
 
     VectorXd x_est(10);
-    x_est = problem->getState().vector(problem->getFrameStructure()).head(10);
+    x_est = problem->getState().vector("POV");
 
     ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), Constants::EPS);
 
@@ -445,7 +468,7 @@ TEST_F(ProcessorImut, gyro_x_biasedAbx)
         // quaternion composition
         quat_comp = quat_comp * v2q(data.tail(3)*dt);
 
-        Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3);
+        Quaterniond rot(problem->getState().vector("O").data());
         data.head(3) =  rot.conjugate() * (- gravity()) + acc_bias;
 
         cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -454,7 +477,7 @@ TEST_F(ProcessorImut, gyro_x_biasedAbx)
     }
 
     x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x_true, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x_true, Constants::EPS);
 
 }
 
@@ -463,11 +486,13 @@ TEST_F(ProcessorImut, gyro_xy_biasedAbxy)
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
     double abx(0.002), aby(0.01);
-    x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -492,7 +517,7 @@ TEST_F(ProcessorImut, gyro_xy_biasedAbxy)
     VectorXd x(10);
     x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;//, abx,aby,0, 0,0,0;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL);// << "expected state : " << problem->getState().vector(problem->getFrameStructure()).transpose()
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);// << "expected state : " << problem->getState().vector(problem->getFrameStructure()).transpose()
 //    << "\n estimated state : " << x.transpose();
 
     //do so for 5s
@@ -502,7 +527,7 @@ TEST_F(ProcessorImut, gyro_xy_biasedAbxy)
         // quaternion composition
         quat_comp = quat_comp * v2q(data.tail(3)*dt);
 
-        Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3);
+        Quaterniond rot(problem->getState().vector("O").data());
         data.head(3) =  rot.conjugate() * (- gravity()) + acc_bias;
 
         cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -511,7 +536,7 @@ TEST_F(ProcessorImut, gyro_xy_biasedAbxy)
     }
 
     x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);// << "estimated state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() <<
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);// << "estimated state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() <<
 //    "\n expected state is : \n" << x.transpose() << std::endl;
 }
 
@@ -519,11 +544,13 @@ TEST_F(ProcessorImut, gyro_z)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -542,7 +569,7 @@ TEST_F(ProcessorImut, gyro_z)
     VectorXd x(10);
     x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
 
     //do so for 5s
     const unsigned int iter = 1000; //how many ms 
@@ -556,17 +583,19 @@ TEST_F(ProcessorImut, gyro_z)
     }
 
     x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
 TEST_F(ProcessorImut, gyro_xyz)
 {
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -610,7 +639,7 @@ TEST_F(ProcessorImut, gyro_xyz)
         quat_comp = quat_comp * v2q(tmp_vec*dt);
         R0 = R0 * v2R(tmp_vec*dt);
         // use processorImu
-        Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3);
+        Quaterniond rot(problem->getState().vector("O").data());
         data.head(3) =  rot.conjugate() * (- gravity()); //gravity measured
         data.tail(3) = tmp_vec;
 
@@ -637,28 +666,30 @@ TEST_F(ProcessorImut, gyro_xyz)
     VectorXd x(10);
     x << 0,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0,0,0;
 
-    Quaterniond result_quat(problem->getState().vector(problem->getFrameStructure()).data() + 3);
+    Quaterniond result_quat(problem->getState().vector("O").data());
     //std::cout << "final orientation : " << q2v(result_quat).transpose() << std::endl;
 
     //check position part
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(3) , x.head(3), Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS);
 
     //check orientation part
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).segment(3,4) , x.segment(3,4) , Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("O"), x.segment(3,4) , Constants::EPS);
 
     //check velocity and bias parts
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).segment(7,3) , x.segment(7,3), Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7,3), Constants::EPS);
 }
 
 TEST_F(ProcessorImut, gyro_z_ConstVelocity)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  2,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  2,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -677,7 +708,7 @@ TEST_F(ProcessorImut, gyro_z_ConstVelocity)
     VectorXd x(10);
     x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002m
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
 
     //do so for 1s
     const unsigned int iter = 1000; //how many ms 
@@ -691,18 +722,20 @@ TEST_F(ProcessorImut, gyro_z_ConstVelocity)
     }
 
     x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
 TEST_F(ProcessorImut, gyro_x_ConstVelocity)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  2,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  2,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -722,7 +755,7 @@ TEST_F(ProcessorImut, gyro_x_ConstVelocity)
     // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
     x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
 
     //do so for 1s
     const unsigned int iter = 1000; //how many ms 
@@ -731,7 +764,7 @@ TEST_F(ProcessorImut, gyro_x_ConstVelocity)
         // quaternion composition
         quat_comp = quat_comp * v2q(data.tail(3)*dt);
 
-        Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3);
+        Quaterniond rot(problem->getState().vector("O").data());
         data.head(3) =  rot.conjugate() * (- gravity());
 
         cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -740,18 +773,20 @@ TEST_F(ProcessorImut, gyro_x_ConstVelocity)
     }
 
     x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
 TEST_F(ProcessorImut, gyro_xy_ConstVelocity)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  2,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  2,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -771,7 +806,7 @@ TEST_F(ProcessorImut, gyro_xy_ConstVelocity)
     // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
     x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
 
     //do so for 1s
     const unsigned int iter = 1000; //how many ms 
@@ -780,7 +815,7 @@ TEST_F(ProcessorImut, gyro_xy_ConstVelocity)
         // quaternion composition
         quat_comp = quat_comp * v2q(data.tail(3)*dt);
 
-        Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3);
+        Quaterniond rot(problem->getState().vector("O").data());
         data.head(3) =  rot.conjugate() * (- gravity());
 
         cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -789,18 +824,20 @@ TEST_F(ProcessorImut, gyro_xy_ConstVelocity)
     }
 
     x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
 TEST_F(ProcessorImut, gyro_y_ConstVelocity)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  2,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  2,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -820,7 +857,7 @@ TEST_F(ProcessorImut, gyro_y_ConstVelocity)
     // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
     x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);
 
     //do so for 1s
     const unsigned int iter = 1000; //how many ms 
@@ -829,7 +866,7 @@ TEST_F(ProcessorImut, gyro_y_ConstVelocity)
         // quaternion composition
         quat_comp = quat_comp * v2q(data.tail(3)*dt);
 
-        Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3);
+        Quaterniond rot(problem->getState().vector("O").data());
         data.head(3) =  rot.conjugate() * (- gravity());
 
         cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -838,17 +875,19 @@ TEST_F(ProcessorImut, gyro_y_ConstVelocity)
     }
 
     x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
 TEST_F(ProcessorImut, gyro_xyz_ConstVelocity)
 {
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  2,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  2,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -892,7 +931,7 @@ TEST_F(ProcessorImut, gyro_xyz_ConstVelocity)
         quat_comp = quat_comp * v2q(tmp_vec*dt);
         R0 = R0 * v2R(tmp_vec*dt);
         // use processorImu
-        Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3);
+        Quaterniond rot(problem->getState().vector("O").data());
         data.head(3) =  rot.conjugate() * (- gravity()); //gravity measured
         data.tail(3) = tmp_vec;
 
@@ -920,17 +959,17 @@ TEST_F(ProcessorImut, gyro_xyz_ConstVelocity)
     //rotation + translation due to initial velocity
     x << 2,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 2,0,0;
 
-    Quaterniond result_quat(problem->getState().vector(problem->getFrameStructure()).data() + 3);
+    Quaterniond result_quat(problem->getState().vector("O").data());
     //std::cout << "final orientation : " << q2v(result_quat).transpose() << std::endl;
 
     //check position part
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(3) , x.head(3), Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("P") , x.head(3), Constants::EPS);
 
     //check orientation part
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).segment(3,4) , x.segment(3,4) , Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("O") , x.segment(3,4) , Constants::EPS);
 
     //check velocity
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).segment(7,3) , x.segment(7,3), Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("V") , x.segment(7,3), Constants::EPS);
 
 }
 
@@ -938,11 +977,13 @@ TEST_F(ProcessorImut, gyro_x_acc_x)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -964,7 +1005,7 @@ TEST_F(ProcessorImut, gyro_x_acc_x)
     // v = a*dt = 0.001
     x << 0.0000005,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0.001,0,0;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); // << "1. current state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() <<
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); // << "1. current state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() <<
 //    "\n current x is : \n" << x.transpose() << std::endl;
 
     //do so for 1s
@@ -974,7 +1015,7 @@ TEST_F(ProcessorImut, gyro_x_acc_x)
         // quaternion composition
         quat_comp = quat_comp * v2q(data.tail(3)*dt);
 
-        Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3);
+        Quaterniond rot(problem->getState().vector("O").data());
         data.head(3) =  rot.conjugate() * (- gravity()) + (Vector3d()<<1,0,0).finished();
 
         cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
@@ -985,18 +1026,20 @@ TEST_F(ProcessorImut, gyro_x_acc_x)
     // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on x axis
     // v = a*dt = 1
     x << 0.5,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 1,0,0;
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
 TEST_F(ProcessorImut, gyro_y_acc_y)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -1018,7 +1061,7 @@ TEST_F(ProcessorImut, gyro_y_acc_y)
     // v = a*dt = 0.001
     x << 0,0.0000005,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0.001,0;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); // << "1. current state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() <<
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); // << "1. current state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() <<
 //    "\n current x is : \n" << x.transpose() << std::endl;
 
     //do so for 1s
@@ -1028,7 +1071,7 @@ TEST_F(ProcessorImut, gyro_y_acc_y)
         // quaternion composition
         quat_comp = quat_comp * v2q(data.tail(3)*dt);
 
-        Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3);
+        Quaterniond rot(problem->getState().vector("O").data());
         data.head(3) =  rot.conjugate() * (- gravity()) + (Vector3d()<<0,1,0).finished();
 
         cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
@@ -1039,18 +1082,20 @@ TEST_F(ProcessorImut, gyro_y_acc_y)
     // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on y axis
     // v = a*dt = 1
     x << 0,0.5,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,1,0;
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
 TEST_F(ProcessorImut, gyro_z_acc_z)
 {
     double dt(0.001);
     t.set(0); // clock in 0,1 ms ticks
-    x0 << 0,0,0,  0,0,0,1,  0,0,0;
-    MatrixXd P0(9,9); P0.setIdentity();
-    x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
-    s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
-    problem->setPriorFactor(x0c, s0c, t);
+    // x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    // MatrixXd P0(9,9); P0.setIdentity();
+    // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
+    // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)});
+    // problem->setPriorFactor(x0c, s0c, t);
+    problem->setPrior(problem_prior_, t);
+
     // process this capture for joining prior KF (t=0) and set it as origin KF
     cap_imu_ptr->setTimeStamp(t);
     cap_imu_ptr->process();
@@ -1072,7 +1117,7 @@ TEST_F(ProcessorImut, gyro_z_acc_z)
     // v = a*dt = 0.001
     x << 0,0,0.0000005, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0.001;
 
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 
     //do so for 1s
     const unsigned int iter = 1000; //how many ms 
@@ -1081,7 +1126,7 @@ TEST_F(ProcessorImut, gyro_z_acc_z)
         // quaternion composition
         quat_comp = quat_comp * v2q(data.tail(3)*dt);
 
-        Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3);
+        Quaterniond rot(problem->getState().vector("O").data());
         data.head(3) =  rot.conjugate() * (- gravity()) + (Vector3d()<<0,0,1).finished();
 
         cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
@@ -1092,8 +1137,8 @@ TEST_F(ProcessorImut, gyro_z_acc_z)
     // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on z axis
     // v = a*dt = 1
     x << 0,0,0.5, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,1;
-    WOLF_DEBUG(problem->getState().vector(problem->getFrameStructure()).head(10).transpose());
-    ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);
+    WOLF_DEBUG(problem->getState().vector("POV").transpose());
+    ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);
 }
 
 int main(int argc, char **argv)
diff --git a/test/gtest_processor_imu2d.cpp b/test/gtest_processor_imu2d.cpp
index ef7224c7d59b2f5a7bf2b86799ba6f9cd1e23be6..a00b943697c35b13129e1d14294022e326d4b780 100644
--- a/test/gtest_processor_imu2d.cpp
+++ b/test/gtest_processor_imu2d.cpp
@@ -19,12 +19,6 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-/**
- * \file gtest_processor_imu2d.cpp
- *
- *  Created on: Nov 24, 2020
- *      \author: igeer
- */
 
 #include "imu/internal/config.h"
 #include "imu/capture/capture_imu.h"
@@ -66,8 +60,8 @@ class ProcessorImu2dTest : public testing::Test
     {
         // Wolf problem
         problem = Problem::create("POV", 2);
-        sensor_ptr = problem->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu2d.yaml");
-        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu2d.yaml");
+        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});
         processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
 
         // Time and data variables
@@ -102,17 +96,18 @@ TEST(ProcessorImu2d_constructors, ALL)
     std::string wolf_root = _WOLF_IMU_ROOT_DIR;
     ProblemPtr problem = Problem::create("POV", 2);
     std::cout << "creating sensor_ptr" << std::endl;
-    SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu2d.yaml");
+    SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d.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);
+    // ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Imu pre-integrator", sensor_ptr, params_default);
+    ProcessorBasePtr processor_ptr = ProcessorBase::emplace<ProcessorImu2d>(sensor_ptr, params_default);
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxTimeSpan(),   params_default->max_time_span);
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxBuffLength(), params_default->max_buff_length);
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getDistTraveled(),  params_default->dist_traveled);
     ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getAngleTurned(),   params_default->angle_turned);
 
     //Factory constructor with yaml
-    processor_ptr = problem->installProcessor("ProcessorImu2d", "Sec Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu2d.yaml");
+    processor_ptr = problem->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu2d.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);
@@ -122,10 +117,14 @@ TEST(ProcessorImu2d_constructors, ALL)
 TEST_F(ProcessorImu2dTest, Prior)
 {
     // Set the origin
-    x0c['P'] = Vector2d(1,2);
-    x0c['O'] = Vector1d(0);
-    x0c['V'] = Vector2d(4,5);
-    auto KF0 = problem->setPriorFix(x0c, t0);
+    // x0c['P'] = Vector2d(1,2);
+    // x0c['O'] = Vector1d(0);
+    // x0c['V'] = Vector2d(4,5);
+    // auto KF0 = problem->setPriorFix(x0c, t0);
+    SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
+                                {'O', SpecState("StateAngle",    Vector1d::Zero())},
+                                {'V', SpecState("StateVector2d", Vector2d(4,5))}};
+    auto KF0 = problem->setPrior(problem_prior, t0);
     processor_motion->setOrigin(KF0);
     //WOLF_DEBUG("x0 =", x0c);
     //WOLF_DEBUG("KF0 state =", problem->getTrajectory()->getFrameMap().at(t0)->getState("POV"));
@@ -133,126 +132,132 @@ TEST_F(ProcessorImu2dTest, Prior)
 
 TEST_F(ProcessorImu2dTest, MRUA)
 {
-   data << 1, 0, 0,   0, 0, 0; 
-   data_cov *= 1e-3;
-   // Set the origin
-   x0c['P'] = Vector2d(1,2);
-   x0c['O'] = Vector1d(0);
-   x0c['V'] = Vector2d(4,5);
-   WOLF_INFO("1");
-   auto KF0 = problem->setPriorFix(x0c, t0);
-   WOLF_INFO("2");
-   processor_motion->setOrigin(KF0);
-   WOLF_INFO("3");
-
-   problem->print(4,1,1,1);
-   WOLF_INFO("Current State: ", problem->getState());
-   for(t = t0; t <= t0 + 1.0 + dt/2; t+=dt){
-       WOLF_INFO("4");
-       C->setTimeStamp(t);
-       WOLF_INFO("5");
-       C->setData(data);
-       WOLF_INFO("6");
-       C->setDataCovariance(data_cov);
-       WOLF_INFO("7");
-       C->process();
-       WOLF_INFO("8");
-       WOLF_INFO("Current State: ", problem->getState()['P'].transpose());
-       WOLF_INFO((x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2)).transpose());
-       ASSERT_MATRIX_APPROX(x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2), problem->getState()['P'], 1e-9);
-   }
+    data << 1, 0, 0,   0, 0, 0; 
+    data_cov *= 1e-3;
+    // Set the origin
+    // x0c['P'] = Vector2d(1,2);
+    // x0c['O'] = Vector1d(0);
+    // x0c['V'] = Vector2d(4,5);
+    // auto KF0 = problem->setPriorFix(x0c, t0);
+    SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
+                                {'O', SpecState("StateAngle",    Vector1d::Zero())},
+                                {'V', SpecState("StateVector2d", Vector2d(4,5))}};
+    auto KF0 = problem->setPrior(problem_prior, t0);
+    processor_motion->setOrigin(KF0);
+ 
+    problem->print(4,1,1,1);
+    WOLF_INFO("Current State: ", problem->getState());
+    for(t = t0; t <= t0 + 1.0 + dt/2; t+=dt){
+        C->setTimeStamp(t);
+        C->setData(data);
+        C->setDataCovariance(data_cov);
+        C->process();
+        WOLF_INFO("Current State: ", problem->getState()['P'].transpose());
+        WOLF_INFO((x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2)).transpose());
+        ASSERT_MATRIX_APPROX(x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2), problem->getState()['P'], 1e-9);
+    }
 }
 
 TEST_F(ProcessorImu2dTest, parabola)
 {
-   double v0 = 10;
-   double a = 1;
-
-   Vector2d pos;
-   // Set the origin
-   x0c['P'] = Vector2d(0, 0);
-   x0c['O'] = Vector1d(0);
-   x0c['V'] = Vector2d(v0, 0);
-
-   data_cov *= 1e-3;
-   auto KF0 = problem->setPriorFix(x0c, t0);
-   processor_motion->setOrigin(KF0);
-
-   for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){
-       C->setTimeStamp(t);
-       data <<  0, a, 0,    0,  0, 0;
-       C->setData(data);
-       C->setDataCovariance(data_cov);
-       C->process();
-       //WOLF_DEBUG("Current State: ", problem->getState());
-       pos << v0*(t-t0),
-              0.5*a*std::pow(t-t0, 2);
-       //WOLF_DEBUG("Calculated State: ", pos.transpose());
-       EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9);
-       EXPECT_MATRIX_APPROX(Vector2d(v0, a*(t-t0)), problem->getState()['V'], 1e-9);
-   }
+    double v0 = 10;
+    double a = 1;
+    data_cov *= 1e-3;
+ 
+    Vector2d pos;
+    // Set the origin
+    // x0c['P'] = Vector2d(0, 0);
+    // x0c['O'] = Vector1d(0);
+    // x0c['V'] = Vector2d(v0, 0);
+    // auto KF0 = problem->setPriorFix(x0c, t0);
+    SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
+                                {'O', SpecState("StateAngle",    Vector1d::Zero())},
+                                {'V', SpecState("StateVector2d", Vector2d(4,5))}};
+    auto KF0 = problem->setPrior(problem_prior, t0);
+    processor_motion->setOrigin(KF0);
+ 
+    for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){
+        C->setTimeStamp(t);
+        data <<  0, a, 0,    0,  0, 0;
+        C->setData(data);
+        C->setDataCovariance(data_cov);
+        C->process();
+        //WOLF_DEBUG("Current State: ", problem->getState());
+        pos << v0*(t-t0),
+               0.5*a*std::pow(t-t0, 2);
+        //WOLF_DEBUG("Calculated State: ", pos.transpose());
+        EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9);
+        EXPECT_MATRIX_APPROX(Vector2d(v0, a*(t-t0)), problem->getState()['V'], 1e-9);
+    }
 }
 
 TEST_F(ProcessorImu2dTest, parabola_deluxe)
 {
-   Vector2d v0(13, 56);
-   Vector2d a(1, 2);
-
-   Vector2d pos;
-   // Set the origin
-   x0c['P'] = Vector2d(0, 0);
-   x0c['O'] = Vector1d(0);
-   x0c['V'] = v0;
-
-   data_cov *= 1e-3;
-   auto KF0 = problem->setPriorFix(x0c, t0);
-   processor_motion->setOrigin(KF0);
-
-   for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){
-       C->setTimeStamp(t);
-       data <<  a(0), a(1), 0,    0,  0, 0;
-       C->setData(data);
-       C->setDataCovariance(data_cov);
-       C->process();
-       //WOLF_DEBUG("Current State: ", problem->getState());
-       pos = v0*(t-t0) + 0.5*a*std::pow(t-t0, 2);
-       //WOLF_DEBUG("Calculated State: ", pos.transpose());
-       EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9);
-       EXPECT_MATRIX_APPROX(v0 + a*(t-t0), problem->getState()['V'], 1e-9);
-   }
+    Vector2d v0(13, 56);
+    Vector2d a(1, 2);
+    data_cov *= 1e-3;
+ 
+    Vector2d pos;
+    // Set the origin
+    // x0c['P'] = Vector2d(0, 0);
+    // x0c['O'] = Vector1d(0);
+    // x0c['V'] = v0;
+    // auto KF0 = problem->setPriorFix(x0c, t0);
+    SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
+                                {'O', SpecState("StateAngle",    Vector1d::Zero())},
+                                {'V', SpecState("StateVector2d", Vector2d(4,5))}};
+    auto KF0 = problem->setPrior(problem_prior, t0);
+    processor_motion->setOrigin(KF0);
+ 
+    for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){
+        C->setTimeStamp(t);
+        data <<  a(0), a(1), 0,    0,  0, 0;
+        C->setData(data);
+        C->setDataCovariance(data_cov);
+        C->process();
+        //WOLF_DEBUG("Current State: ", problem->getState());
+        pos = v0*(t-t0) + 0.5*a*std::pow(t-t0, 2);
+        //WOLF_DEBUG("Calculated State: ", pos.transpose());
+        EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9);
+        EXPECT_MATRIX_APPROX(v0 + a*(t-t0), problem->getState()['V'], 1e-9);
+    }
 }
 
 TEST_F(ProcessorImu2dTest, Circular_move)
 {
-   double pi = 3.14159265358993;
-   double r = 1;
-   double w = 1;
-   double alpha = pi/4.;
-   Vector2d pos;
-   // Set the origin
-   x0c['P'] = Vector2d(r, 0);
-   pos = x0c['P'];
-   x0c['O'] = Vector1d(alpha);
-   x0c['V'] = Vector2d(0, w*r);
-
-   data_cov *= 1e-3;
-   //dt = 0.0001;
-   auto KF0 = problem->setPriorFix(x0c, t0);
-   processor_motion->setOrigin(KF0);
-
-   WOLF_DEBUG("Current State: ", problem->getState());
-   for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){
-       C->setTimeStamp(t);
-       data <<  -std::cos(alpha)*w*w*r, std::sin(alpha)*w*w*r, 0,    0,  0, w;
-       C->setData(data);
-       C->setDataCovariance(data_cov);
-       C->process();
-       WOLF_DEBUG("Current State: ", problem->getState());
-       pos << r*std::cos( w*(t-t0) ),
-              r*std::sin( w*(t-t0) );
-       WOLF_DEBUG("Calculated State: ", pos.transpose());
-       EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9);
-   }
+    double pi = 3.14159265358993;
+    double r = 1;
+    double w = 1;
+    double alpha = pi/4.;
+    Vector2d pos(r, 0);
+    data_cov *= 1e-3;
+    //dt = 0.0001;
+ 
+    // Set the origin
+    // x0c['P'] = pos;
+    // x0c['O'] = Vector1d(alpha);
+    // x0c['V'] = Vector2d(0, w*r);
+    //auto KF0 = problem->setPriorFix(x0c, t0);
+    SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  pos)},
+                                {'O', SpecState("StateAngle",    Vector1d(alpha))},
+                                {'V', SpecState("StateVector2d", Vector2d(0, w*r))}};
+    auto KF0 = problem->setPrior(problem_prior, t0);
+ 
+    processor_motion->setOrigin(KF0);
+ 
+    WOLF_DEBUG("Current State: ", problem->getState());
+    for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){
+        C->setTimeStamp(t);
+        data <<  -std::cos(alpha)*w*w*r, std::sin(alpha)*w*w*r, 0,    0,  0, w;
+        C->setData(data);
+        C->setDataCovariance(data_cov);
+        C->process();
+        WOLF_DEBUG("Current State: ", problem->getState());
+        pos << r*std::cos( w*(t-t0) ),
+               r*std::sin( w*(t-t0) );
+        WOLF_DEBUG("Calculated State: ", pos.transpose());
+        EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9);
+    }
 }
 
 int main(int argc, char **argv)
diff --git a/test/gtest_processor_imu2d_with_gravity.cpp b/test/gtest_processor_imu2d_with_gravity.cpp
index d947eb48bc60f50094cf1df8cc3d958f6c1bfb9a..d09460d634b441a0436d1f6b953e574b763e867b 100644
--- a/test/gtest_processor_imu2d_with_gravity.cpp
+++ b/test/gtest_processor_imu2d_with_gravity.cpp
@@ -19,12 +19,6 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
-/**
- * \file gtest_processor_imu2d.cpp
- *
- *  Created on: Nov 24, 2020
- *      \author: igeer
- */
 
 #include "imu/internal/config.h"
 #include "imu/capture/capture_imu.h"
@@ -74,8 +68,8 @@ class ProcessorImu2dTest : public testing::Test
 
         // Wolf problem
         problem = Problem::create("POV", 2);
-        sensor_ptr = problem->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu2d_with_gravity.yaml");
-        ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu2d.yaml");
+        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});
         processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
 
         // Time and data variables
@@ -97,10 +91,14 @@ TEST_F(ProcessorImu2dTest, MUA_Only_G)
    data << 0, 0, 0,   0, 0, 0; 
    data_cov *= 1e-3;
    // Set the origin
-   x0c['P'] = Vector2d(1,2);
-   x0c['O'] = Vector1d(0);
-   x0c['V'] = Vector2d(4,5);
-   auto KF0 = problem->setPriorFix(x0c, t0);
+//    x0c['P'] = Vector2d(1,2);
+//    x0c['O'] = Vector1d(0);
+//    x0c['V'] = Vector2d(4,5);
+//    auto KF0 = problem->setPriorFix(x0c, t0);
+   SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
+                               {'O', SpecState("StateAngle",    Vector1d::Zero())},
+                               {'V', SpecState("StateVector2d", Vector2d(4,5))}};
+   auto KF0 = problem->setPrior(problem_prior, t0);
    processor_motion->setOrigin(KF0);
 
    // Set the gravity
@@ -125,10 +123,14 @@ TEST_F(ProcessorImu2dTest, MUA)
    data << 1, 2, 0,   0, 0, 0; 
    data_cov *= 1e-3;
    // Set the origin
-   x0c['P'] = Vector2d(1,2);
-   x0c['O'] = Vector1d(0);
-   x0c['V'] = Vector2d(4,5);
-   auto KF0 = problem->setPriorFix(x0c, t0);
+//    x0c['P'] = Vector2d(1,2);
+//    x0c['O'] = Vector1d(0);
+//    x0c['V'] = Vector2d(4,5);
+//    auto KF0 = problem->setPriorFix(x0c, t0);
+   SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
+                               {'O', SpecState("StateAngle",    Vector1d::Zero())},
+                               {'V', SpecState("StateVector2d", Vector2d(4,5))}};
+   auto KF0 = problem->setPrior(problem_prior, t0);
    processor_motion->setOrigin(KF0);
 
    // Set the gravity
@@ -154,10 +156,14 @@ TEST_F(ProcessorImu2dTest, Spinning)
    data << 0, 0, 0,   0, 0, w; 
    data_cov *= 1e-3;
    // Set the origin
-   x0c['P'] = Vector2d(1,2);
-   x0c['O'] = Vector1d(0);
-   x0c['V'] = Vector2d(4,5);
-   auto KF0 = problem->setPriorFix(x0c, t0);
+//    x0c['P'] = Vector2d(1,2);
+//    x0c['O'] = Vector1d(0);
+//    x0c['V'] = Vector2d(4,5);
+//    auto KF0 = problem->setPriorFix(x0c, t0);
+   SpecComposite problem_prior{{'P', SpecState("StatePoint2d",  Vector2d(1,2))},
+                               {'O', SpecState("StateAngle",    Vector1d::Zero())},
+                               {'V', SpecState("StateVector2d", Vector2d(4,5))}};
+   auto KF0 = problem->setPrior(problem_prior, t0);
    processor_motion->setOrigin(KF0);
 
    // Set the gravity
diff --git a/test/gtest_sensor_compass.cpp b/test/gtest_sensor_compass.cpp
index 15e223cd84c513a660ee72fb3da6d0f6eef003d4..9a919f2f93c7f0d3afef0b2d31d6dd796a4edcee 100644
--- a/test/gtest_sensor_compass.cpp
+++ b/test/gtest_sensor_compass.cpp
@@ -26,12 +26,11 @@
 #include "imu/sensor/sensor_compass.h"
 #include "core/sensor/factory_sensor.h"
 
-// #include "core/math/covariance.h"
-
 #include <cstdio>
 
 using namespace wolf;
 using namespace Eigen;
+using namespace yaml_schema_cpp;
 
 std::string wolf_root = _WOLF_IMU_ROOT_DIR;
 
@@ -66,28 +65,27 @@ class sensor_compass_test : public testing::Test
         void LoadYamlGroundtruth(int dim)
         {
             // load from yaml
-            ParserYaml parser   = ParserYaml(wolf_root + (dim == 2 ? 
-                                                          "/test/yaml/sensor_compass_2d.yaml" :
-                                                          "/test/yaml/sensor_compass_3d.yaml"),
-                                             true);
-            ParamsServer server = ParamsServer(parser.getParams());
-
-            p_state     = server.getParam<VectorXd>("states/P/state");
-            o_state     = server.getParam<VectorXd>("states/O/state");
-            i_state     = server.getParam<VectorXd>("states/I/state");
-            f_state     = server.getParam<VectorXd>("states/F/state");
-
-            p_mode      = server.getParam<std::string>("states/P/mode");
-            o_mode      = server.getParam<std::string>("states/O/mode");
-            i_mode      = server.getParam<std::string>("states/I/mode");
-            f_mode      = server.getParam<std::string>("states/F/mode");
-
-            p_dynamic   = server.getParam<bool>("states/P/dynamic");
-            o_dynamic   = server.getParam<bool>("states/O/dynamic");
-            i_dynamic   = server.getParam<bool>("states/I/dynamic");
-            f_dynamic   = server.getParam<bool>("states/F/dynamic");
-
-            stdev_noise  = server.getParam<double>("stdev_noise");
+            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();
         }
 
@@ -112,43 +110,6 @@ class sensor_compass_test : public testing::Test
             noise_cov = VectorXd::Constant(dim, stdev_noise*stdev_noise).asDiagonal();
         }
 };
-        
-TEST_F(sensor_compass_test, constructor_priors_2d)
-{
-    RandomGroundtruth(2);
-
-    auto params = std::make_shared<ParamsSensorCompass>();
-    params->stdev_noise = stdev_noise;
-
-    SpecSensorComposite priors{{'P',SpecStateSensor("P",           p_state, p_mode, VectorXd(0), p_dynamic)}, 
-                  {'O',SpecStateSensor("O",           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>("sensor_1", 2, 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, constructor_priors_3d)
 {
@@ -157,12 +118,12 @@ TEST_F(sensor_compass_test, constructor_priors_3d)
     auto params = std::make_shared<ParamsSensorCompass>();
     params->stdev_noise = stdev_noise;
 
-    SpecComposite priors{{'P',SpecStateSensor("P",           p_state, p_mode, VectorXd(0), p_dynamic)}, 
-                  {'O',SpecStateSensor("O",           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)}};
+    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>("sensor_1", 3, params, priors);
+    auto sen = std::make_shared<SensorCompass>(params, priors);
 
     ASSERT_NE(sen, nullptr);
 
@@ -187,141 +148,15 @@ TEST_F(sensor_compass_test, constructor_priors_3d)
     ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS);
 }
 
-TEST_F(sensor_compass_test, constructor_server_2d)
-{
-    LoadYamlGroundtruth(2);
-
-    ParserYaml parser   = ParserYaml(wolf_root + "/test/yaml/sensor_compass_2d.yaml", true);
-    ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
-
-    auto params = std::make_shared<ParamsSensorCompass>("sensor_1", server);
-    auto sen = std::make_shared<SensorCompass>("sensor_1", 2, params, server);
-
-    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_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, constructor_server_3d)
-{
-    LoadYamlGroundtruth(3);
-    
-    ParserYaml parser   = ParserYaml(wolf_root + "/test/yaml/sensor_compass_3d.yaml", true);
-    ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
-
-    auto params = std::make_shared<ParamsSensorCompass>("sensor_1", server);
-    auto sen = std::make_shared<SensorCompass>("sensor_1", 3, params, server);
-
-    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_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_2d)
-{
-    LoadYamlGroundtruth(2);
-
-    ParserYaml parser   = ParserYaml(wolf_root + "/test/yaml/sensor_compass_2d.yaml", true);
-    ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
-
-    auto sb = FactorySensor::create("SensorCompass","sensor_1", 2, server);
-
-    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->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_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_3d)
+TEST_F(sensor_compass_test, factory)
 {
     LoadYamlGroundtruth(3);
 
-    ParserYaml parser   = ParserYaml(wolf_root + "/test/yaml/sensor_compass_3d.yaml", true);
-    ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
-
-    auto sb = FactorySensor::create("SensorCompass","sensor_1", 3, server);
-
-    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->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_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_yaml_2d)
-{
-    LoadYamlGroundtruth(2);
+    YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_compass_3d.yaml");
+    
+    ASSERT_TRUE(server.applySchema("SensorCompass"));
 
-    auto sb = FactorySensorYaml::create("SensorCompass","sensor_1", 2, wolf_root + "/test/yaml/sensor_compass_2d.yaml");
+    auto sb = FactorySensor::create("SensorCompass",server.getNode());
 
     SensorCompassPtr sen = std::dynamic_pointer_cast<SensorCompass>(sb);
 
@@ -347,49 +182,11 @@ TEST_F(sensor_compass_test, factory_yaml_2d)
     ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS);
 }
 
-TEST_F(sensor_compass_test, factory_yaml_3d)
+TEST_F(sensor_compass_test, factory_yaml)
 {
     LoadYamlGroundtruth(3);
 
-    auto sb = FactorySensorYaml::create("SensorCompass","sensor_1", 3, wolf_root + "/test/yaml/sensor_compass_3d.yaml");
-
-    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->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_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_priors_2d)
-{
-    RandomGroundtruth(2);
-
-    auto params = std::make_shared<ParamsSensorCompass>();
-    params->stdev_noise = stdev_noise;
-
-    SpecComposite priors{{'P',SpecStateSensor("P",           p_state, p_mode, VectorXd(0), p_dynamic)}, 
-                  {'O',SpecStateSensor("O",           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 sb = FactorySensorPriorSensorMap::create("SensorCompass","sensor_1", 2, params, priors);
+    auto sb = FactorySensorYaml::create("SensorCompass", "SensorCompass", wolf_root + "/test/yaml/sensor_compass_3d.yaml", {wolf_root});
 
     SensorCompassPtr sen = std::dynamic_pointer_cast<SensorCompass>(sb);
 
@@ -410,46 +207,6 @@ TEST_F(sensor_compass_test, factory_priors_2d)
     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_priors_3d)
-{
-    RandomGroundtruth(3);
-
-    auto params = std::make_shared<ParamsSensorCompass>();
-    params->stdev_noise = stdev_noise;
-
-    SpecComposite priors{{'P',SpecStateSensor("P",           p_state, p_mode, VectorXd(0), p_dynamic)}, 
-                  {'O',SpecStateSensor("O",           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 sb = FactorySensorPriorSensorMap::create("SensorCompass","sensor_1", 3, params, priors);
-
-    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->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);