diff --git a/src/problem.cpp b/src/problem.cpp
index fc92b0f7c20e7ad1b1c829216615c6ed80f5fc22..513cc64c2c4628b26eb6cda6eb63d55a08164063 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -235,13 +235,6 @@ Eigen::VectorXs Problem::getCurrentState()
     return state;
 }
 
-Eigen::VectorXs Problem::getCurrentStateAndStamp(TimeStamp& ts)
-{
-    Eigen::VectorXs state(getFrameStructureSize());
-    getCurrentStateAndStamp(state, ts);
-    return state;
-}
-
 void Problem::getCurrentState(Eigen::VectorXs& state)
 {
     assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
@@ -260,7 +253,10 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
     assert(state.size() == getFrameStructureSize() && "Problem::getCurrentState: bad state size");
 
     if (processor_motion_ptr_ != nullptr)
-        processor_motion_ptr_->getCurrentStateAndStamp(state, ts);
+    {
+        processor_motion_ptr_->getCurrentState(state);
+        processor_motion_ptr_->getCurrentTimeStamp(ts);
+    }
     else if (trajectory_ptr_->getLastKeyFramePtr() != nullptr)
     {
         trajectory_ptr_->getLastKeyFramePtr()->getTimeStamp(ts);
@@ -300,39 +296,12 @@ Eigen::VectorXs Problem::getState(const TimeStamp& _ts)
 Size Problem::getFrameStructureSize() const
 {
     return state_size_;
-//    if (trajectory_ptr_->getFrameStructure() == "PO 2D")
-//        return 3;
-//    if (trajectory_ptr_->getFrameStructure() == "PO 3D")
-//        return 7;
-//    if (trajectory_ptr_->getFrameStructure() == "POV 3D")
-//        return 10;
-//    throw std::runtime_error(
-//            "Problem::getFrameStructureSize(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
 }
 
 void Problem::getFrameStructureSize(Size& _x_size, Size& _cov_size) const
 {
     _x_size = state_size_;
     _cov_size = state_cov_size_;
-
-//    if (trajectory_ptr_->getFrameStructure() == "PO 2D")
-//    {
-//        _x_size = 3;
-//        _cov_size = 3;
-//    }
-//    else if (trajectory_ptr_->getFrameStructure() == "PO 3D")
-//    {
-//        _x_size = 7;
-//        _cov_size = 6;
-//    }
-//    else if (trajectory_ptr_->getFrameStructure() == "POV 3D")
-//    {
-//        _x_size = 10;
-//        _cov_size = 9;
-//    }
-//    else
-//        throw std::runtime_error(
-//                    "Problem::getFrameStructureSize(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
 }
 
 Eigen::VectorXs Problem::zeroState()
@@ -573,9 +542,9 @@ Eigen::MatrixXs Problem::getFrameCovariance(FrameBasePtr _frame_ptr)
     for (const auto& sb : _frame_ptr->getStateBlockVec())
         if (sb)
             sz += sb->getSize();
+
     Eigen::MatrixXs covariance(sz, sz);
 
-//    Eigen::MatrixXs covariance = Eigen::MatrixXs::Zero(_frame_ptr->getPPtr()->getSize()+_frame_ptr->getOPtr()->getSize(), _frame_ptr->getPPtr()->getSize()+_frame_ptr->getOPtr()->getSize());
     getFrameCovariance(_frame_ptr, covariance);
     return covariance;
 }
diff --git a/src/problem.h b/src/problem.h
index d46e1f6d6785c7f487e966a7ac9b24ae46e1017e..86c56553641ee955bf95cbb0a2bbb07f2dc8f4cc 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -203,7 +203,6 @@ class Problem : public std::enable_shared_from_this<Problem>
         // State getters
         Eigen::VectorXs getCurrentState();
         void getCurrentState(Eigen::VectorXs& state);
-        Eigen::VectorXs getCurrentStateAndStamp(TimeStamp& _ts);
         void getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& _ts);
         Eigen::VectorXs getState(const TimeStamp& _ts);
         void getState(const TimeStamp& _ts, Eigen::VectorXs& state);