diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 9d306d5983883edea43f8712f07484d2086df065..8e5c10205a488143a7f5fc44dbf603658768cf5c 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -46,7 +46,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         HardwareBasePtr     hardware_ptr_;
         TrajectoryBasePtr   trajectory_ptr_;
         MapBasePtr          map_ptr_;
-        IsMotionPtr  processor_motion_ptr_;
+        std::vector<IsMotionPtr>  processor_is_motion_vec_;
 //        IsMotionPtr         is_motion_ptr_;
         std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
         SizeEigen state_size_, state_cov_size_, dim_;
@@ -149,8 +149,8 @@ class Problem : public std::enable_shared_from_this<Problem>
          *
          * Set the processor motion.
          */
-        void setProcessorMotion(IsMotionPtr _processor_motion_ptr);
-        IsMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
+        void addProcessorMotion(IsMotionPtr _processor_motion_ptr);
+        IsMotionPtr addProcessorMotion(const std::string& _unique_processor_name);
         void clearProcessorMotion();
 
     public:
@@ -364,7 +364,14 @@ inline bool Problem::priorIsSet() const
 
 inline IsMotionPtr& Problem::getProcessorMotion()
 {
-    return processor_motion_ptr_;
+    if (!processor_is_motion_vec_.empty())
+        return processor_is_motion_vec_[0];
+    return nullptr
+}
+
+inline IsMotionPtr& Problem::getIsMotionVector()
+{
+    return processor_is_motion_vec_;
 }
 
 inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap()
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index ff4c8b3acbdb47dfcd5ff6b28748185760be62fc..57c164469f6e12c888ea9b6d280f8df7dfc3edaf 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -466,7 +466,6 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
 
     protected:
         // Attributes
-        std::string state_structure_;///< The structure of the state vector (to retrieve state blocks from frames)
         SizeEigen x_size_;           ///< The size of the state vector
         SizeEigen data_size_;        ///< the size of the incoming data
         SizeEigen delta_size_;       ///< the size of the deltas
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 26ade79a1b0a3b7b285838683815ce2dd97d7adb..5b741765daf9ab29ceeecba4c7198081d9039092 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -34,7 +34,7 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
         hardware_ptr_(std::make_shared<HardwareBase>()),
         trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
         map_ptr_(std::make_shared<MapBase>()),
-        processor_motion_ptr_(),
+        processor_is_motion_vec_(std::vector<IsMotionPtr>()),
         prior_is_set_(false),
         frame_structure_(_frame_structure)
 {
@@ -227,9 +227,9 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
     if (prc_ptr->isMotion() && prior_is_set_)
         (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
 
-    // setting the main processor motion
-    if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
-        processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(prc_ptr);
+    // adding processor is motion to the processor is motion vector
+    if (prc_ptr->isMotion())
+        processor_is_motion_vec_.push_back(std::dynamic_pointer_cast<IsMotion>(prc_ptr));
 
     return prc_ptr;
 }
@@ -269,9 +269,9 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
     if (prc_ptr->isMotion() && prior_is_set_)
         (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
 
-    // setting the main processor motion
-    if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
-        processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);
+    // adding processor is motion to the processor is motion vector
+    if (prc_ptr->isMotion())
+        processor_is_motion_vec_.push_back(std::dynamic_pointer_cast<IsMotion>(prc_ptr));
 
     return prc_ptr;
 }
@@ -291,7 +291,12 @@ SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
     return (*sen_it);
 }
 
-IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
+void Problem::addProcessorMotion(IsMotionPtr _processor_motion_ptr)
+{
+    processor_is_motion_vec_.push_back(_processor_motion_ptr);
+}
+
+IsMotionPtr Problem::addProcessorMotion(const std::string& _processor_name)
 {
     for (auto sen : getHardware()->getSensorList()) // loop all sensors
     {
@@ -306,9 +311,10 @@ IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
         {
             if ((*prc_it)->isMotion()) // found, and it's motion!
             {
-                std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl;
-                processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(*prc_it);
-                return processor_motion_ptr_;
+                std::cout << "Found processor '" << _processor_name << "', of type Motion, and added to the motion processor vector." << std::endl;
+                IsMotionPtr motion_processor_ptr = std::dynamic_pointer_cast<IsMotion>(*prc_it);
+                processor_is_motion_vec_.push_back(motion_processor_ptr);
+                return motion_processor_ptr;
             }
             else // found, but it's not motion!
             {
@@ -322,15 +328,11 @@ IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
     return nullptr;
 }
 
-void Problem::setProcessorMotion(IsMotionPtr _processor_motion_ptr)
-{
-    processor_motion_ptr_ = _processor_motion_ptr;
-}
-
-void Problem::clearProcessorMotion()
-{
-    processor_motion_ptr_.reset();
-}
+// Not used in the code based --> TODO
+// void Problem::clearProcessorMotion()
+// {
+//     processor_motion_ptr_.reset();
+// }
 
 FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
                                    const SizeEigen _dim, //
@@ -395,17 +397,30 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& ts) co
     else
         _state = zeroState();
 }
+VectorXd full_state = problem->getCurrentState();
+Vector3d angular_momentum = full_state.segment<3>(16);
 
 void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
 {
     // try to get the state from processor_motion if any, otherwise...
-    if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, _state))
-    {
+    // if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, _state))
+    // {
+    //     FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+    //     if (closest_frame != nullptr)
+    //         closest_frame->getState(_state);
+    //     else
+    //         _state = zeroState();
+    // }
+    if (processor_is_motion_vec_.empty()){
         FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
         if (closest_frame != nullptr)
             closest_frame->getState(_state);
         else
-            _state = zeroState();
+            _state = zeroState();  // TODO: generic implementation ? 
+    }
+    else {
+        std::list<Eigen::VectorXd> states_to_concat;
+
     }
 }
 
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 3cf7743c4e673dc358f076db4dbc56376a71f46e..302923f94a6c23e07e493e1d463d820db6f8d467 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -60,27 +60,28 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
         buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr);
 }
 
-void ProcessorBase::remove()
-{
-    if (!is_removing_)
-    {
-        is_removing_ = true;
-        ProcessorBasePtr this_p = shared_from_this();
-
-        // clear Problem::processor_motion_ptr_
-        if (isMotion())
-        {
-            ProblemPtr P = getProblem();
-            if(P && (P->getProcessorMotion() == std::dynamic_pointer_cast<IsMotion>( shared_from_this() ) ) )
-                P->clearProcessorMotion();
-        }
-
-        // remove from upstream
-        SensorBasePtr sen = sensor_ptr_.lock();
-        if(sen)
-            sen->removeProcessor(this_p);
-    }
-}
+// Not used in the code base --> TODO
+// void ProcessorBase::remove()
+// {
+//     if (!is_removing_)
+//     {
+//         is_removing_ = true;
+//         ProcessorBasePtr this_p = shared_from_this();
+
+//         // clear Problem::processor_motion_ptr_
+//         if (isMotion())
+//         {
+//             ProblemPtr P = getProblem();
+//             if(P && (P->getProcessorMotion() == std::dynamic_pointer_cast<IsMotion>( shared_from_this() ) ) )
+//                 P->clearProcessorMotion();
+//         }
+
+//         // remove from upstream
+//         SensorBasePtr sen = sensor_ptr_.lock();
+//         if(sen)
+//             sen->removeProcessor(this_p);
+//     }
+// }
 
 void ProcessorBase::link(SensorBasePtr _sen_ptr)
 {
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 387f4a2170a296312fa057cc5971ee10c3ca84c4..56e9adcadd9bd4500467e679c5826d7de2545fe1 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -629,8 +629,9 @@ void ProcessorMotion::setProblem(ProblemPtr _problem)
         this->setOrigin(this->getProblem()->getLastKeyFrame());
 
     // set the main processor motion
-    if (this->getProblem()->getProcessorMotion() == nullptr)
-        this->getProblem()->setProcessorMotion(std::static_pointer_cast<ProcessorMotion>(shared_from_this()));
+    if (this->getProblem()->getProcessorMotion() == nullptr){
+        this->getProblem()->addProcessorMotion(std::static_pointer_cast<ProcessorMotion>(shared_from_this()));
+    }
 };
 
 bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr)