diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index d78cd5169f3dfc523c9435c11dc65b41ef6b3591..5b59b02421ad1050c97a4a188957c6274d1be946 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -82,19 +82,19 @@ class CaptureMotion : public CaptureBase
         const MotionBuffer& getBuffer() const;
 
         // Buffer's initial conditions for pre-integration
-        VectorXd getCalibrationPreint() const;
-        void setCalibrationPreint(const VectorXd& _calib_preint);
-        MatrixXd getJacobianCalib() const;
-        MatrixXd getJacobianCalib(const TimeStamp& _ts) const;
+        VectorComposite getCalibrationPreint() const;
+        void setCalibrationPreint(const VectorComposite& _calib_preint);
+        MatrixComposite getJacobianCalib() const;
+        MatrixComposite getJacobianCalib(const TimeStamp& _ts) const;
 
         // Get delta preintegrated, and corrected for changes on calibration params
-        VectorXd getDeltaCorrected(const VectorXd& _calib_current) const;
-        VectorXd getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const;
-        VectorXd getDeltaPreint() const;
-        VectorXd getDeltaPreint(const TimeStamp& _ts) const;
-        MatrixXd getDeltaPreintCov() const;
-        MatrixXd getDeltaPreintCov(const TimeStamp& _ts) const;
-        virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const;
+        VectorComposite getDeltaCorrected(const VectorComposite& _calib_current) const;
+        VectorComposite getDeltaCorrected(const VectorComposite& _calib_current, const TimeStamp& _ts) const;
+        VectorComposite getDeltaPreint() const;
+        VectorComposite getDeltaPreint(const TimeStamp& _ts) const;
+        MatrixComposite getDeltaPreintCov() const;
+        MatrixComposite getDeltaPreintCov(const TimeStamp& _ts) const;
+        virtual VectorComposite correctDelta(const VectorComposite& _delta, const VectorComposite& _delta_error) const;
 
         // Origin frame and capture
         CaptureBasePtr getOriginCapture();
@@ -104,7 +104,7 @@ class CaptureMotion : public CaptureBase
     private:
         Eigen::VectorXd data_;              ///< Motion data in form of vector mandatory
         Eigen::MatrixXd data_cov_;          ///< Motion data covariance
-        Eigen::VectorXd calib_preint_;      ///< Calibration parameters used during pre-integration
+        VectorComposite calib_preint_;      ///< Calibration parameters used during pre-integration
         MotionBuffer buffer_;               ///< Buffer of motions between this Capture and the next one.
         CaptureBaseWPtr capture_origin_ptr_;    ///< Pointer to the origin capture of the motion
 };
@@ -142,17 +142,17 @@ inline MotionBuffer& CaptureMotion::getBuffer()
     return buffer_;
 }
 
-inline Eigen::MatrixXd CaptureMotion::getJacobianCalib() const
+inline MatrixComposite CaptureMotion::getJacobianCalib() const
 {
     return getBuffer().get().back().jacobian_calib_;
 }
 
-inline Eigen::MatrixXd CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const
+inline MatrixComposite CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const
 {
     return getBuffer().getMotion(_ts).jacobian_calib_;
 }
 
-inline Eigen::VectorXd CaptureMotion::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
+inline VectorComposite CaptureMotion::correctDelta(const VectorComposite& _delta, const VectorComposite& _delta_error) const
 {
     WOLF_DEBUG("WARNING: using Cartesian sum for delta correction. \nIf your deltas lie on a manifold, derive this function and implement the proper delta correction!")
     return _delta + _delta_error;
@@ -168,32 +168,32 @@ inline void CaptureMotion::setOriginCapture(CaptureBasePtr _capture_origin_ptr)
     capture_origin_ptr_ = _capture_origin_ptr;
 }
 
-inline VectorXd CaptureMotion::getCalibrationPreint() const
+inline VectorComposite CaptureMotion::getCalibrationPreint() const
 {
     return calib_preint_;
 }
 
-inline void CaptureMotion::setCalibrationPreint(const VectorXd& _calib_preint)
+inline void CaptureMotion::setCalibrationPreint(const VectorComposite& _calib_preint)
 {
     calib_preint_ = _calib_preint;
 }
 
-inline VectorXd CaptureMotion::getDeltaPreint() const
+inline VectorComposite CaptureMotion::getDeltaPreint() const
 {
     return getBuffer().get().back().delta_integr_;
 }
 
-inline VectorXd CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
+inline VectorComposite CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
 {
     return getBuffer().getMotion(_ts).delta_integr_;
 }
 
-inline MatrixXd CaptureMotion::getDeltaPreintCov() const
+inline MatrixComposite CaptureMotion::getDeltaPreintCov() const
 {
     return getBuffer().get().back().delta_integr_cov_;
 }
 
-inline MatrixXd CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const
+inline MatrixComposite CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const
 {
     return getBuffer().getMotion(_ts).delta_integr_cov_;
 }
diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h
index a8e06e0e112bfb5f07910ad56353bb71b2517ca8..bb37eeb34801900c0e65cde582fddf982f5a6da4 100644
--- a/include/core/processor/motion_buffer.h
+++ b/include/core/processor/motion_buffer.h
@@ -10,6 +10,7 @@
 
 #include "core/common/wolf.h"
 #include "core/common/time_stamp.h"
+#include "core/state_block/state_composite.h"
 
 #include <list>
 #include <algorithm>
@@ -24,26 +25,26 @@ struct Motion
         TimeStamp ts_;                          ///< Time stamp
         Eigen::VectorXd data_;                  ///< instantaneous motion data
         Eigen::MatrixXd data_cov_;              ///< covariance of the instantaneous data
-        Eigen::VectorXd delta_;                 ///< instantaneous motion delta
-        Eigen::MatrixXd delta_cov_;             ///< covariance of the instantaneous delta
-        Eigen::VectorXd delta_integr_;          ///< the integrated motion or delta-integral
-        Eigen::MatrixXd delta_integr_cov_;      ///< covariance of the integrated delta
-        Eigen::MatrixXd jacobian_delta_;        ///< Jacobian of the integration wrt delta_
-        Eigen::MatrixXd jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_
-        Eigen::MatrixXd jacobian_calib_;        ///< Jacobian of delta_integr wrt extra states (TBD by the derived processors)
+        VectorComposite delta_;                 ///< instantaneous motion delta
+        MatrixComposite delta_cov_;             ///< covariance of the instantaneous delta
+        VectorComposite delta_integr_;          ///< the integrated motion or delta-integral
+        MatrixComposite delta_integr_cov_;      ///< covariance of the integrated delta
+        MatrixComposite jacobian_delta_;        ///< Jacobian of the integration wrt delta_
+        MatrixComposite jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_
+        MatrixComposite jacobian_calib_;        ///< Jacobian of delta_integr wrt extra states (TBD by the derived processors)
     public:
         Motion() = delete; // completely delete unpredictable stuff like this
-        Motion(const TimeStamp& _ts, SizeEigen _data_size, SizeEigen _delta_size, SizeEigen _cov_size, SizeEigen _calib_size);
+        Motion(const TimeStamp& _ts, SizeEigen _data_size, SizeEigen _delta_size, SizeEigen _cov_size, SizeEigen _calib_size); // TODO remove
         Motion(const TimeStamp& _ts,
                const VectorXd& _data,
                const MatrixXd& _data_cov,
-               const VectorXd& _delta,
-               const MatrixXd& _delta_cov,
-               const VectorXd& _delta_int,
-               const MatrixXd& _delta_integr_cov,
-               const MatrixXd& _jac_delta,
-               const MatrixXd& _jac_delta_int,
-               const MatrixXd& _jacobian_calib);
+               const VectorComposite& _delta,
+               const MatrixComposite& _delta_cov,
+               const VectorComposite& _delta_int,
+               const MatrixComposite& _delta_integr_cov,
+               const MatrixComposite& _jac_delta,
+               const MatrixComposite& _jac_delta_int,
+               const MatrixComposite& _jacobian_calib);
         ~Motion();
     private:
 
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 31d621a48855d8c774820b578fd4e0294d423484..0ca665a661a83409d17540f8caa94a5dfd1b5603 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -194,7 +194,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
         /** \brief Gets the delta preintegrated covariance from all integrations so far
          * \return the delta preintegrated covariance matrix
          */
-        const Eigen::MatrixXd getCurrentDeltaPreintCov() const;
+        const MatrixComposite getCurrentDeltaPreintCov() const;
 
         /** \brief Provide the motion integrated so far
          * \return the integrated motion
@@ -358,13 +358,13 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
          *
          *  where `F_data = d_f/d_data` is the Jacobian of `f()`.
          */
-        virtual void 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& _jacobian_calib) const = 0;
+//        virtual void 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& _jacobian_calib) const = 0;
 
         virtual void computeCUrrentDelta(const VectorXd& _data,
                                          const MatrixXd& _data_cov,
@@ -382,10 +382,10 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
          *
          * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2.
          */
-        virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
-                                    const Eigen::VectorXd& _delta2,
-                                    const double _dt2,
-                                    Eigen::VectorXd& _delta1_plus_delta2) const = 0;
+//        virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+//                                    const Eigen::VectorXd& _delta2,
+//                                    const double _dt2,
+//                                    Eigen::VectorXd& _delta1_plus_delta2) const = 0;
         virtual void deltaPlusDelta(const VectorComposite& _delta1,
                                     const VectorComposite& _delta2,
                                     const double _dt2,
@@ -401,12 +401,12 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
          *
          * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its jacobians.
          */
-        virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
-                                    const Eigen::VectorXd& _delta2,
-                                    const double _dt2,
-                                    Eigen::VectorXd& _delta1_plus_delta2,
-                                    Eigen::MatrixXd& _jacobian1,
-                                    Eigen::MatrixXd& _jacobian2) const = 0;
+//        virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+//                                    const Eigen::VectorXd& _delta2,
+//                                    const double _dt2,
+//                                    Eigen::VectorXd& _delta1_plus_delta2,
+//                                    Eigen::MatrixXd& _jacobian1,
+//                                    Eigen::MatrixXd& _jacobian2) const = 0;
         virtual void deltaPlusDelta(const VectorComposite& _delta1,
                                     const VectorComposite& _delta2,
                                     const double _dt2,
@@ -422,10 +422,10 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
          *
          * This function implements the composition (+) so that _x2 = _x1 (+) _delta.
          */
-        virtual void statePlusDelta(const Eigen::VectorXd& _x,
-                                    const Eigen::VectorXd& _delta,
-                                    const double _dt,
-                                    Eigen::VectorXd& _x_plus_delta) const = 0;
+//        virtual void statePlusDelta(const Eigen::VectorXd& _x,
+//                                    const Eigen::VectorXd& _delta,
+//                                    const double _dt,
+//                                    Eigen::VectorXd& _x_plus_delta) const = 0;
         virtual void statePlusDelta(const VectorComposite& _x,
                                     const VectorComposite& _delta,
                                     const double _dt,
@@ -439,7 +439,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
          *   - 3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
          *   - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!
          */
-        virtual Eigen::VectorXd deltaZero() const = 0;
+//        virtual Eigen::VectorXd deltaZero() const = 0;
         virtual VectorComposite deltaZero() const = 0;
 
 
@@ -514,15 +514,15 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
         // helpers to avoid allocation
         double dt_;                             ///< Time step
         Eigen::VectorXd x_;                     ///< current state
-        Eigen::VectorXd delta_;                 ///< current delta
-        Eigen::MatrixXd delta_cov_;             ///< current delta covariance
-        Eigen::VectorXd delta_integrated_;      ///< integrated delta
-        Eigen::MatrixXd delta_integrated_cov_;  ///< integrated delta covariance
-        Eigen::VectorXd calib_preint_;          ///< calibration vector used during pre-integration
-        Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
-        Eigen::MatrixXd jacobian_delta_;        ///< jacobian of delta composition w.r.t current delta
-        Eigen::MatrixXd jacobian_calib_;        ///< jacobian of delta preintegration wrt calibration params
-        Eigen::MatrixXd jacobian_delta_calib_;  ///< jacobian of delta wrt calib params
+        VectorComposite delta_;                 ///< current delta
+        MatrixComposite delta_cov_;             ///< current delta covariance
+        VectorComposite delta_integrated_;      ///< integrated delta
+        MatrixComposite delta_integrated_cov_;  ///< integrated delta covariance
+        VectorComposite calib_preint_;          ///< calibration vector used during pre-integration
+        MatrixComposite jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
+        MatrixComposite jacobian_delta_;        ///< jacobian of delta composition w.r.t current delta
+        MatrixComposite jacobian_calib_;        ///< jacobian of delta preintegration wrt calibration params
+        MatrixComposite jacobian_delta_calib_;  ///< jacobian of delta wrt calib params
         Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
 };
 
@@ -556,7 +556,7 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
     statePlusDelta(curr_x, last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
 }
 
-inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const
+inline const MatrixComposite ProcessorMotion::getCurrentDeltaPreintCov() const
 {
     return getBuffer().get().back().delta_integr_cov_;
 }
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index 6a2816a5c44db5777531b0eb88e4ba6329dc48f9..5c947f386efcd4270ed14e02cf1886d8e6acd3ee 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -32,6 +32,8 @@ typedef std::unordered_map < std::string, Eigen::VectorXd > VectorComposite;
 namespace Eigen
 {
 std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
+
+wolf::VectorComposite& operator + (wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2);
 }
 
 namespace wolf{
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index a736c242e7fb7047498d061d9121ce8fb56f817c..13b11dda7909e80d1c81687eb4e7313e79994a50 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -51,7 +51,7 @@ CaptureMotion::~CaptureMotion()
     //
 }
 
-Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current) const
+VectorComposite CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current) const
 {
     VectorXd calib_preint    = getCalibrationPreint();
     VectorXd delta_preint    = getBuffer().get().back().delta_integr_;
@@ -61,7 +61,7 @@ Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current)
     return   delta_corrected;
 }
 
-Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const
+VectorComposite CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const
 {
     Motion   motion          = getBuffer().getMotion(_ts);
     VectorXd delta_preint    = motion.delta_integr_;
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index e7e349e9196b3b3b298fc1d5142d8a38f348e1a6..a12de2212f53992b7bb2bb2e74b5ee98ccfa2ce4 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -5,13 +5,13 @@ namespace wolf
 Motion::Motion(const TimeStamp& _ts,
                const VectorXd& _data,
                const MatrixXd& _data_cov,
-               const VectorXd& _delta,
-               const MatrixXd& _delta_cov,
-               const VectorXd& _delta_integr,
-               const MatrixXd& _delta_integr_cov,
-               const MatrixXd& _jac_delta,
-               const MatrixXd& _jac_delta_int,
-               const MatrixXd& _jac_calib) :
+               const VectorComposite& _delta,
+               const MatrixComposite& _delta_cov,
+               const VectorComposite& _delta_integr,
+               const MatrixComposite& _delta_integr_cov,
+               const MatrixComposite& _jac_delta,
+               const MatrixComposite& _jac_delta_int,
+               const MatrixComposite& _jac_calib) :
         ts_(_ts),
         data_(_data),
         data_cov_(_data_cov),
@@ -126,12 +126,12 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b
             }
             if (show_delta)
             {
-                cout << "   delta: " << mot.delta_.transpose() << endl;
+                cout << "   delta: " << mot.delta_ << endl;
                 cout << "   delta cov: \n" << mot.delta_cov_ << endl;
             }
             if (show_delta_int)
             {
-                cout << "   delta integrated: " << mot.delta_integr_.transpose() << endl;
+                cout << "   delta integrated: " << mot.delta_integr_ << endl;
                 cout << "   delta integrated cov: \n" << mot.delta_integr_cov_ << endl;
             }
             if (show_jacs)
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 29154c33afe3cd97d24b34c41afa03a977a78ece..80ed5d181c609f0e9eee82ff98e26c1d147a8b3f 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -15,6 +15,20 @@ std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x)
     }
     return _os;
 }
+
+wolf::VectorComposite& operator + (const wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2)
+{
+    wolf::VectorComposite v;
+    for (const auto& pair_key_vec1 : _v1)
+    {
+        const auto &key  = pair_key_vec1.first;
+        const auto &vec1 = pair_key_vec1.second;
+        const auto& vec2 = _v2.at(key);
+        v.at(key) = vec1 + vec2;
+    }
+    return v;
+}
+
 }
 
 namespace wolf