diff --git a/src/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp
index 75d0c293951a09c467096297062d60ba1b0ce9f0..d8becd8210492f1d07cf3826d4b29bbaa28fb9d9 100644
--- a/src/examples/test_kf_callback.cpp
+++ b/src/examples/test_kf_callback.cpp
@@ -21,7 +21,10 @@ int main()
     ProblemPtr problem = Problem::create("PO 2D");
 
     SensorBasePtr sen_odo    = problem->installSensor   ("ODOM 2D", "main odometer", (Vector3s()<<0,0,0).finished(),"");
-    ProcessorBasePtr prc_odo = problem->installProcessor("ODOM 2D", "odometry integrator", "main odometer", "");
+    ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
+    params_odo->elapsed_time_th_ = 2;
+    params_odo->theta_traveled_th_ = M_PI; // 180 degrees turn
+    ProcessorBasePtr prc_odo = problem->installProcessor("ODOM 2D", "odometry integrator", sen_odo, params_odo);
     prc_odo->setTimeTolerance(0.1);
 
     SensorBasePtr sen_ftr    = problem->installSensor   ("ODOM 2D", "other odometer", (Vector3s()<<0,0,0).finished(),"");
@@ -34,29 +37,32 @@ int main()
     cout << "Motion processor : " << problem->getProcessorMotionPtr()->getName() << endl;
 
     TimeStamp t(0);
+    cout << "=======================\n>> TIME: " << t.get() << endl;
     Vector3s x({0,0,0});
-    problem->getProcessorMotionPtr()->setOrigin(x, t);
+    Matrix3s P; P.setZero();
+    problem->setPrior(x, P, t, 0.01);
 
-    cout << "x(0) = " << problem->getCurrentState().transpose() << endl;
+    cout << "x(" << t.get() << ") = " << problem->getCurrentState().transpose() << endl;
 
-    Vector2s odo_data;  odo_data << .1, (M_PI / 2);
+    Vector2s odo_data;  odo_data << .1, (M_PI / 10);
 
-    problem->print(2, false, true, false); // print(level, constr_by, metric, state_blocks)
+    problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks)
 
     Scalar dt = 1;
     for (auto i = 0; i < 4; i++)
     {
-        t += dt;
-        cout << "=======================\n>> TIME: " << t.get() << endl;
 
         cout << "Tracker----------------" << endl;
         sen_ftr->process(make_shared<CaptureVoid>(t, sen_ftr));
-        problem->print(2, false, true, false); // print(level, constr_by, metric, state_blocks)
+        problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks)
+
+        t += dt;
+        cout << "=======================\n>> TIME: " << t.get() << endl;
 
         cout << "Motion-----------------" << endl;
         sen_odo->process(make_shared<CaptureMotion>(t, sen_odo, odo_data, 3, 3, nullptr));
         cout << "x(" << t.get() << ") = " << problem->getCurrentState().transpose() << endl;
-        problem->print(2, false, true, false); // print(level, constr_by, metric, state_blocks)
+        problem->print(2, false, true, true); // print(level, constr_by, metric, state_blocks)
 
     }
 
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index e5f42afb0981a19d3fd6c803c29fcbf565cb576e..92467b61d78ac4c681aa1aafef06cbe63f79baf5 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -136,7 +136,7 @@ void FrameBase::setKey()
 
         getTrajectoryPtr()->sortFrame(shared_from_this());
 
-        WOLF_DEBUG("Set KF", this->id());
+//        WOLF_DEBUG("Set KF", this->id());
     }
 }
 
diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp
index 9c1efa23330838108b68d07e4d0f0b7f95ebcab5..b411ff64832d968b8e82b30a69e4f3ce1270a351 100644
--- a/src/processor_tracker_feature.cpp
+++ b/src/processor_tracker_feature.cpp
@@ -33,7 +33,7 @@ unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_fe
     for (auto ftr : new_features_last_)
     {
         ftr->setTrackId( ftr->id() );
-        WOLF_DEBUG("Det track: ", ftr->trackId(), ", last: ", ftr->id());
+//        WOLF_DEBUG("Det track: ", ftr->trackId(), ", last: ", ftr->id());
     }
 
     // Track new features from last to incoming. This will append new correspondences to matches_last_incoming
@@ -43,7 +43,7 @@ unsigned int ProcessorTrackerFeature::processNew(const unsigned int& _max_new_fe
         ftr->setTrackId(matches_last_from_incoming_[ftr]->feature_ptr_->trackId());
 
         // Print new tracked features
-        WOLF_DEBUG("New track: ", ftr->trackId(), ", last: ", matches_last_from_incoming_[ftr]->feature_ptr_->id(), ", inc: ", ftr->id());
+//        WOLF_DEBUG("New track: ", ftr->trackId(), ", last: ", matches_last_from_incoming_[ftr]->feature_ptr_->id(), ", inc: ", ftr->id());
     }
 
     // Append all new Features to the incoming Captures' list of Features
@@ -94,7 +94,7 @@ unsigned int ProcessorTrackerFeature::processKnown()
     // Print resulting list of matches
     for (auto match : matches_last_from_incoming_)
     {
-        WOLF_DEBUG("Known track: ", match.first->trackId(), ", last: ", match.second->feature_ptr_->id(), ", inc: ", match.first->id());
+//        WOLF_DEBUG("Known track: ", match.first->trackId(), ", last: ", match.second->feature_ptr_->id(), ", inc: ", match.first->id());
     }
 
     return matches_last_from_incoming_.size();
@@ -117,7 +117,7 @@ void ProcessorTrackerFeature::advanceDerived()
     // Print resulting list
     for (auto match: matches_origin_from_last_)
     {
-        WOLF_DEBUG("Matches advanced: track: ", match.first->trackId(), "-", match.second->feature_ptr_->trackId(), " origin: ", match.second->feature_ptr_->id(), " last: ", match.first->id());
+//        WOLF_DEBUG("Matches advanced: track: ", match.first->trackId(), "-", match.second->feature_ptr_->trackId(), " origin: ", match.second->feature_ptr_->id(), " last: ", match.first->id());
     }
 }
 
@@ -133,7 +133,7 @@ void ProcessorTrackerFeature::resetDerived()
     // Print resulting list
     for (auto match: matches_origin_from_last_)
     {
-        WOLF_DEBUG("Matches reset: track: ", match.first->trackId(), "-", match.second->feature_ptr_->trackId(), " origin: ", match.second->feature_ptr_->id(), " last: ", match.first->id());
+//        WOLF_DEBUG("Matches reset: track: ", match.first->trackId(), "-", match.second->feature_ptr_->trackId(), " origin: ", match.second->feature_ptr_->id(), " last: ", match.first->id());
     }
 }
 
@@ -147,9 +147,9 @@ void ProcessorTrackerFeature::establishConstraints()
     }
     for (auto match : matches_origin_from_last_)
     {
-        WOLF_DEBUG( "Constraint: track: " , match.second->feature_ptr_->trackId() ,
-                    " origin: " , match.second->feature_ptr_->id() ,
-                    " from last: " , match.first->id() );
+//        WOLF_DEBUG( "Constraint: track: " , match.second->feature_ptr_->trackId() ,
+//                    " origin: " , match.second->feature_ptr_->id() ,
+//                    " from last: " , match.first->id() );
     }
 }
 
diff --git a/src/sensor_GPS.h b/src/sensor_GPS.h
index b36d09ae0055c177d42f05b6d3fb7517a4e9fd0a..c43cd13cb8a766fd6cfea67000d16d4262ee5f2a 100644
--- a/src/sensor_GPS.h
+++ b/src/sensor_GPS.h
@@ -20,6 +20,7 @@ namespace wolf {
 struct IntrinsicsGPS : public IntrinsicsBase
 {
         // add GPS parameters here
+        virtual ~IntrinsicsGPS() = default;
 };
 
 WOLF_PTR_TYPEDEFS(SensorGPS);
diff --git a/src/sensor_GPS_fix.cpp b/src/sensor_GPS_fix.cpp
index 956e3fb687352c20d609617e0089b6f42e299be7..b97e43c49a8ff74befcd77024e54a37100e64c21 100644
--- a/src/sensor_GPS_fix.cpp
+++ b/src/sensor_GPS_fix.cpp
@@ -10,6 +10,18 @@ SensorGPSFix::SensorGPSFix(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const dou
     //
 }
 
+SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics) :
+        SensorBase("GPS FIX", std::make_shared<StateBlock>(_extrinsics.head(3)), std::make_shared<StateQuaternion>(_extrinsics.tail(4)), nullptr, _intrinsics.noise_std)
+{
+    assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
+}
+
+SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) : SensorGPSFix(_extrinsics, *_intrinsics_ptr)
+{
+    //
+}
+
+
 SensorGPSFix::~SensorGPSFix()
 {
     //
diff --git a/src/sensor_GPS_fix.h b/src/sensor_GPS_fix.h
index 9cb5583a025eaab3264faa8943aaafbb4c720fd6..3291aa538a7f7cd7a8fa838df8190a4b70ab4acb 100644
--- a/src/sensor_GPS_fix.h
+++ b/src/sensor_GPS_fix.h
@@ -12,9 +12,12 @@
 
 namespace wolf {
 
+WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsGPSFix);
 struct IntrinsicsGPSFix : public IntrinsicsBase
 {
+        Eigen::Vector3s noise_std;
         // Empty -- it acts only as a typedef for IntrinsicsBase, but allows future extension with parameters
+        virtual ~IntrinsicsGPSFix() = default;
 };
 
 WOLF_PTR_TYPEDEFS(SensorGPSFix);
@@ -31,6 +34,8 @@ class SensorGPSFix : public SensorBase
          * 
          **/
 		SensorGPSFix(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const double& _noise);
+        SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics);
+        SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr);
 
         virtual ~SensorGPSFix();
         
diff --git a/src/sensor_IMU.cpp b/src/sensor_IMU.cpp
index 6e973746eafa75f0642e646e6d1edec9e33a0a49..ed8046ce84a2be818fc989136f621244f3525ed2 100644
--- a/src/sensor_IMU.cpp
+++ b/src/sensor_IMU.cpp
@@ -4,18 +4,42 @@
 
 namespace wolf {
 
-SensorIMU::SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr params) :
-        SensorBase("IMU", _p_ptr, _o_ptr, std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6s()<<params->a_noise,params->a_noise,params->a_noise,params->w_noise,params->w_noise,params->w_noise).finished(), false, true),
-        a_noise(params->a_noise),
-        w_noise(params->w_noise),
-        ab_initial_stdev(params->ab_initial_stdev),
-        wb_initial_stdev(params->wb_initial_stdev),
-        ab_rate_stdev(params->ab_rate_stdev),
-        wb_rate_stdev(params->wb_rate_stdev)
+SensorIMU::SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsIMU& _params) :
+        SensorBase("IMU", _p_ptr, _o_ptr, std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6s()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, true),
+        a_noise(_params.a_noise),
+        w_noise(_params.w_noise),
+        ab_initial_stdev(_params.ab_initial_stdev),
+        wb_initial_stdev(_params.wb_initial_stdev),
+        ab_rate_stdev(_params.ab_rate_stdev),
+        wb_rate_stdev(_params.wb_rate_stdev)
 {
     //
 }
 
+SensorIMU::SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr _params) :
+        SensorIMU(_p_ptr, _o_ptr, *_params)
+{
+    //
+}
+
+SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params) :
+        SensorIMU(_extrinsics, *_params)
+{
+    //
+}
+
+SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params) :
+        SensorBase("IMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6s()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, true),
+        a_noise(_params.a_noise),
+        w_noise(_params.w_noise),
+        ab_initial_stdev(_params.ab_initial_stdev),
+        wb_initial_stdev(_params.wb_initial_stdev),
+        ab_rate_stdev(_params.ab_rate_stdev),
+        wb_rate_stdev(_params.wb_rate_stdev)
+{
+    assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
+}
+
 
 SensorIMU::~SensorIMU()
 {
@@ -29,11 +53,8 @@ SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::Ve
     // decode extrinsics vector
     assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
 
-    StateBlockPtr pos_ptr  = std::make_shared<StateBlock>(_extrinsics_pq.head(3), true);
-    StateBlockPtr ori_ptr  = std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true);
-
     IntrinsicsIMUPtr params = std::static_pointer_cast<IntrinsicsIMU>(_intrinsics);
-    SensorIMUPtr sen = std::make_shared<SensorIMU>(pos_ptr, ori_ptr, params);
+    SensorIMUPtr sen = std::make_shared<SensorIMU>(_extrinsics_pq, params);
     sen->setName(_unique_name);
     return sen;
 }
diff --git a/src/sensor_IMU.h b/src/sensor_IMU.h
index 823c18507efe6976f002d310f6edf9c89de2b4da..0b34016e29025f9ddc43a07582924988095e01a1 100644
--- a/src/sensor_IMU.h
+++ b/src/sensor_IMU.h
@@ -15,25 +15,18 @@ WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsIMU);
 struct IntrinsicsIMU : public IntrinsicsBase
 {
         //noise std dev
-        wolf::Scalar w_noise; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
-        wolf::Scalar a_noise; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
+        wolf::Scalar w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
+        wolf::Scalar a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
 
         //Initial biases std dev
-        wolf::Scalar ab_initial_stdev; //accelerometer micro_g/sec
-        wolf::Scalar wb_initial_stdev; //gyroscope rad/sec
+        wolf::Scalar ab_initial_stdev = 0.01; //accelerometer micro_g/sec
+        wolf::Scalar wb_initial_stdev = 0.01; //gyroscope rad/sec
 
         // bias rate of change std dev
-        Scalar ab_rate_stdev;
-        Scalar wb_rate_stdev;
-
-        IntrinsicsIMU() :
-            w_noise(0.001),
-            a_noise(0.04),
-            ab_initial_stdev(0.00001),
-            wb_initial_stdev(0.00001),
-            ab_rate_stdev(0.00001),
-            wb_rate_stdev(0.00001)
-        {}
+        Scalar ab_rate_stdev = 0.00001;
+        Scalar wb_rate_stdev = 0.00001;
+
+        virtual ~IntrinsicsIMU() = default;
 };
 
 WOLF_PTR_TYPEDEFS(SensorIMU);
@@ -63,6 +56,9 @@ class SensorIMU : public SensorBase
          *
          **/
         SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsIMUPtr _params);
+        SensorIMU(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsIMU& _params);
+        SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params);
+        SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params);
 
         Scalar getGyroNoise() const;
         Scalar getAccelNoise() const;
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 3a2de9957d572026fb637f0b1d19932974171686..a1befcf792bcb81cca58d972ff1ea33bf4e2b9df 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -24,11 +24,10 @@ namespace wolf {
  */
 struct IntrinsicsBase
 {
-  IntrinsicsBase()          = default;
-  virtual ~IntrinsicsBase() = default;
+        virtual ~IntrinsicsBase() = default;
 
-  std::string type;
-  std::string name;
+        std::string type;
+        std::string name;
 };
 
 class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBase>
diff --git a/src/sensor_camera.cpp b/src/sensor_camera.cpp
index 36e6c9ad53f3d19c37560eeb9a756398acbab736..7a9a9a2c7904b0ea47143c96a4d0bdd1b36c8c50 100644
--- a/src/sensor_camera.cpp
+++ b/src/sensor_camera.cpp
@@ -15,18 +15,23 @@ SensorCamera::SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBloc
     //
 }
 
-SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const std::shared_ptr<IntrinsicsCamera> _intrinsics_ptr) :
-                SensorBase("CAMERA", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(_intrinsics_ptr->pinhole_model, true), 1),
-                img_width_(_intrinsics_ptr->width), //
-                img_height_(_intrinsics_ptr->height), //
-                distortion_(_intrinsics_ptr->distortion), //
+SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const IntrinsicsCamera& _intrinsics) :
+                SensorBase("CAMERA", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(_intrinsics.pinhole_model, true), 1),
+                img_width_(_intrinsics.width), //
+                img_height_(_intrinsics.height), //
+                distortion_(_intrinsics.distortion), //
                 correction_(distortion_.size()+2) // make correction vector of the same size as distortion vector
 {
     assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3D");
-    K_ = setIntrinsicMatrix(_intrinsics_ptr->pinhole_model);
+    K_ = setIntrinsicMatrix(_intrinsics.pinhole_model);
     pinhole::computeCorrectionModel(getIntrinsicPtr()->getState(), distortion_, correction_);
 }
 
+SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr) :
+        SensorCamera(_extrinsics, *_intrinsics_ptr)
+{
+    //
+}
 
 SensorCamera::~SensorCamera()
 {
@@ -58,13 +63,6 @@ SensorBasePtr SensorCamera::create(const std::string& _unique_name, //
     SensorCameraPtr sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr);
     sen_ptr->setName(_unique_name);
 
-//    std::cout << "Created camera:\n\tintrinsics   : " << sen_ptr->getIntrinsicPtr()->getState().transpose() << std::endl;
-//    std::cout << "\tintrinsic matrix  : " << K_ << std::endl;
-//    std::cout << "\tdistortion  : " << distortion_.transpose() << std::endl;
-//    std::cout << "\tcorrection  : " << correction_.transpose() << std::endl;
-//    std::cout << "\tposition     : " << sen_ptr->getPPtr()->getState().transpose() << std::endl;
-//    std::cout << "\torientation  : " << sen_ptr->getOPtr()->getState().transpose() << std::endl;
-
     return sen_ptr;
 }
 
diff --git a/src/sensor_camera.h b/src/sensor_camera.h
index 7c61c98e2a04f5bd437820b6e3f5d17b79f3b8cb..8b5ec2035452d2b9ec21ec6056d00654731466f3 100644
--- a/src/sensor_camera.h
+++ b/src/sensor_camera.h
@@ -7,6 +7,7 @@
 namespace wolf
 {
 
+WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsCamera);
 /** Struct of intrinsic camera parameters
  */
 struct IntrinsicsCamera : public IntrinsicsBase
@@ -15,6 +16,8 @@ struct IntrinsicsCamera : public IntrinsicsBase
         unsigned int height;                ///< Image height in pixels
         Eigen::Vector4s pinhole_model;      ///< k = [u_0, v_0, alpha_u, alpha_v]  vector of pinhole intrinsic parameters
         Eigen::VectorXs distortion;         ///< d = [d_1, d_2, d_3, ...] radial distortion coefficients
+
+        virtual ~IntrinsicsCamera() = default;
 };
 
 WOLF_PTR_TYPEDEFS(SensorCamera);
@@ -36,7 +39,8 @@ class SensorCamera : public SensorBase
          **/
         SensorCamera(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, int _img_width, int _img_height);
 
-        SensorCamera(const Eigen::VectorXs & _extrinsics, const std::shared_ptr<IntrinsicsCamera> _intrinsics_ptr);
+        SensorCamera(const Eigen::VectorXs & _extrinsics, const IntrinsicsCamera& _intrinsics);
+        SensorCamera(const Eigen::VectorXs & _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr);
 
         virtual ~SensorCamera();
 
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
index 738dc3e6b069b80fb86e3f8915c9f3d249659e05..66239d0d0e65d159f2fd3c12dc4e0bad5319ed5d 100644
--- a/src/sensor_laser_2D.cpp
+++ b/src/sensor_laser_2D.cpp
@@ -23,6 +23,19 @@ SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const l
     //
 }
 
+SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params) :
+        SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8),
+        scan_params_(_params.scan_params)
+{
+    //
+}
+
+SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params) :
+        SensorLaser2D(_p_ptr, _o_ptr, *_params)
+{
+    //
+}
+
 SensorLaser2D::~SensorLaser2D()
 {
     //
diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h
index b1e3e9b2af5ed8fbe33a354264e018493178cddb..9cdcf5567499c2993d67f53535f0e1d34ed2682d 100644
--- a/src/sensor_laser_2D.h
+++ b/src/sensor_laser_2D.h
@@ -17,6 +17,8 @@ WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsLaser2D);
 
 struct IntrinsicsLaser2D : public IntrinsicsBase
 {
+        virtual ~IntrinsicsLaser2D() = default;
+
         laserscanutils::LaserScanParams scan_params;
 };
 
@@ -52,6 +54,8 @@ class SensorLaser2D : public SensorBase
          *
          **/
         SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params);
+        SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params);
+        SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params);
 
         virtual ~SensorLaser2D();
         
diff --git a/src/sensor_odom_2D.cpp b/src/sensor_odom_2D.cpp
index d720c4e66f9df27ee82e67129ea26ba64f1b6954..fdb7ab4804809072cada12774e8af9a08ed9c3f4 100644
--- a/src/sensor_odom_2D.cpp
+++ b/src/sensor_odom_2D.cpp
@@ -12,6 +12,22 @@ SensorOdom2D::SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Sca
     //
 }
 
+SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics) :
+        SensorBase("ODOM 2D", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
+        k_disp_to_disp_(_intrinsics.k_disp_to_disp),
+        k_rot_to_rot_(_intrinsics.k_rot_to_rot)
+{
+    assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2D.");
+    //
+}
+
+SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics) :
+        SensorOdom2D(_extrinsics, *_intrinsics)
+{
+    //
+}
+
+
 SensorOdom2D::~SensorOdom2D()
 {
     //
@@ -33,17 +49,20 @@ SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen:
 {
     // decode extrinsics vector
     assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
-    StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true);
-    StateBlockPtr ori_ptr = std::make_shared<StateAngle>(_extrinsics_po(2), true);
-    // cast intrinsics into derived type
+
     SensorOdom2DPtr odo;
     if (_intrinsics)
     {
         std::shared_ptr<IntrinsicsOdom2D> params = std::static_pointer_cast<IntrinsicsOdom2D>(_intrinsics);
-        odo = std::make_shared<SensorOdom2D>(pos_ptr, ori_ptr, params->k_disp_to_disp, params->k_rot_to_rot);
+        odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params);
     }
     else
-        odo = std::make_shared<SensorOdom2D>(pos_ptr, ori_ptr, 1, 1);
+    {
+        IntrinsicsOdom2D params;
+        params.k_disp_to_disp = 1;
+        params.k_rot_to_rot   = 1;
+        odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params);
+    }
     odo->setName(_unique_name);
     return odo;
 }
diff --git a/src/sensor_odom_2D.h b/src/sensor_odom_2D.h
index ad3b02ae557b023be9c4b279d106ce650d299a2f..4ba0fc37dee5a0d4e487548c894f32fff4fc2a86 100644
--- a/src/sensor_odom_2D.h
+++ b/src/sensor_odom_2D.h
@@ -7,10 +7,12 @@
 
 namespace wolf {
 
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsOdom2D)
+WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsOdom2D);
 
 struct IntrinsicsOdom2D : public IntrinsicsBase
 {
+        virtual ~IntrinsicsOdom2D() = default;
+
         Scalar k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
         Scalar k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
 };
@@ -33,7 +35,9 @@ class SensorOdom2D : public SensorBase
          * \param _rot_noise_factor rotation noise factor
          * 
          **/
-		SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _disp_noise_factor, const Scalar&  _rot_noise_factor);
+        SensorOdom2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _disp_noise_factor, const Scalar&  _rot_noise_factor);
+        SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics);
+        SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics);
 
         virtual ~SensorOdom2D();
         
diff --git a/src/sensor_odom_3D.cpp b/src/sensor_odom_3D.cpp
index 21acafbbb484f81cbedf94376e370e6504fa9b5b..36213d9e69b39d1e09c42804b84d3ed4eb060d2e 100644
--- a/src/sensor_odom_3D.cpp
+++ b/src/sensor_odom_3D.cpp
@@ -24,23 +24,46 @@ SensorOdom3D::SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _o_ptr, Intr
     setNoiseCov(noise_cov_); // sets also noise_std_
 }
 
+SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& _intrinsics) :
+                        SensorBase("ODOM 3D", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
+                        k_disp_to_disp_(_intrinsics.k_disp_to_disp),
+                        k_disp_to_rot_(_intrinsics.k_disp_to_rot),
+                        k_rot_to_rot_(_intrinsics.k_rot_to_rot),
+                        min_disp_var_(_intrinsics.min_disp_var),
+                        min_rot_var_(_intrinsics.min_rot_var)
+{
+    assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3D.");
+
+    noise_cov_ = (Eigen::Vector6s() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
+    setNoiseCov(noise_cov_); // sets also noise_std_
+}
+
+SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom3DPtr _intrinsics) :
+        SensorOdom3D(_extrinsics_pq, *_intrinsics)
+{
+    //
+}
+
+
 SensorOdom3D::~SensorOdom3D()
 {
     //
 }
 
 // Define the factory method
-SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po,
+SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq,
                                  const IntrinsicsBasePtr _intrinsics)
 {
     // decode extrinsics vector
-    assert(_extrinsics_po.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
-    StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(3), true);
-    StateQuaternionPtr ori_ptr = std::make_shared<StateQuaternion>(_extrinsics_po.tail(4), true);
+    assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
+
     // cast intrinsics into derived type
     IntrinsicsOdom3DPtr params = std::static_pointer_cast<IntrinsicsOdom3D>(_intrinsics);
-    SensorBasePtr odo = std::make_shared<SensorOdom3D>(pos_ptr, ori_ptr, params);
+
+    // Call constructor and finish
+    SensorBasePtr odo = std::make_shared<SensorOdom3D>(_extrinsics_pq, params);
     odo->setName(_unique_name);
+
     return odo;
 }
 
diff --git a/src/sensor_odom_3D.h b/src/sensor_odom_3D.h
index c676edc95a66756a7d28b68299127dbd09a60d30..80575937c8eef5cbab637531f1c508371b8e29e3 100644
--- a/src/sensor_odom_3D.h
+++ b/src/sensor_odom_3D.h
@@ -23,13 +23,8 @@ struct IntrinsicsOdom3D : public IntrinsicsBase
         Scalar min_disp_var;
         Scalar min_rot_var;
 
-        IntrinsicsOdom3D() :
-            k_disp_to_disp(0),
-            k_disp_to_rot(0),
-            k_rot_to_rot(0),
-            min_disp_var(0),
-            min_rot_var(0)
-        {}
+        virtual ~IntrinsicsOdom3D() = default;
+
 };
 
 WOLF_PTR_TYPEDEFS(SensorOdom3D);
@@ -52,6 +47,8 @@ class SensorOdom3D : public SensorBase
          * \param _params shared_ptr to a struct with parameters
          **/
         SensorOdom3D(StateBlockPtr _p_ptr, StateQuaternionPtr _q_ptr, IntrinsicsOdom3DPtr params);
+        SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& params);
+        SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom3DPtr params);
 
         virtual ~SensorOdom3D();
 
diff --git a/src/sensors/sensor_diff_drive.h b/src/sensors/sensor_diff_drive.h
index 334fea3895676f923f84625997caf79b4637bafc..f4556a84a16c7a347f27a6f7f51ce913c02dc477 100644
--- a/src/sensors/sensor_diff_drive.h
+++ b/src/sensors/sensor_diff_drive.h
@@ -16,30 +16,6 @@ namespace wolf {
 
 struct IntrinsicsDiffDrive : public IntrinsicsBase
 {
-  IntrinsicsDiffDrive() = default;
-
-  IntrinsicsDiffDrive(const Scalar _left_radius,
-                      const Scalar _right_radius,
-                      const Scalar _separation,
-                      const DiffDriveModel _model,
-                      const Eigen::VectorXs& _factors,
-                      const Scalar _left_resolution,
-                      const Scalar _right_resolution,
-                      const Scalar _left_gain,
-                      const Scalar _right_gain) :
-    left_radius_(_left_radius),
-    right_radius_(_right_radius),
-    separation_(_separation),
-    model_(_model),
-    factors_(_factors),
-    left_resolution_(_left_resolution),
-    right_resolution_(_right_resolution),
-    left_gain_(_left_gain),
-    right_gain_(_right_gain)
-  {
-    //
-  }
-
   Scalar left_radius_;
   Scalar right_radius_;
   Scalar separation_;
@@ -53,6 +29,8 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase
 
   Scalar left_gain_  = 0.01;
   Scalar right_gain_ = 0.01;
+
+  virtual ~IntrinsicsDiffDrive() = default;
 };
 
 typedef std::shared_ptr<IntrinsicsDiffDrive> IntrinsicsDiffDrivePtr;