diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 3ded670afcb492449f622cd6b2fbdd6ff532d3f4..ce1a393f6f1a21e015d66c7a336f889c9d8be218 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -200,8 +200,8 @@ class Problem : public std::enable_shared_from_this<Problem>
         void removeMotionProvider(MotionProviderPtr proc);
 
     public:
-        const std::map<int,MotionProviderConstPtr>& getMotionProviderMap() const;
-        const std::map<int,MotionProviderPtr>& getMotionProviderMap();
+        std::map<int,MotionProviderConstPtr> getMotionProviderMap() const;
+        std::map<int,MotionProviderPtr> getMotionProviderMap();
 
         // Trajectory branch ----------------------------------
         TrajectoryBaseConstPtr getTrajectory() const;
@@ -455,12 +455,15 @@ inline bool Problem::isPriorSet() const
     return prior_options_ == nullptr;
 }
 
-inline const std::map<int,MotionProviderConstPtr>& Problem::getMotionProviderMap() const
+inline std::map<int,MotionProviderConstPtr> Problem::getMotionProviderMap() const
 {
-    return motion_provider_const_map_;
+    std::map<int,MotionProviderConstPtr> map_const;
+    for (auto&& pair : motion_provider_map_)
+        map_const[pair.first] = pair.second;
+    return map_const;
 }
 
-inline const std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap()
+inline std::map<int,MotionProviderPtr> Problem::getMotionProviderMap()
 {
     return motion_provider_map_;
 }
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index cc51fa6805640e04f0417e9680daf30e7a641633..74949c604240639ac78bb72145a2b91e13802959 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -65,7 +65,6 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr
         trajectory_ptr_(std::make_shared<TrajectoryBase>()),
         map_ptr_(_map),
         motion_provider_map_(),
-        motion_provider_const_map_(),
         frame_structure_(_frame_structure),
         prior_options_(std::make_shared<PriorOptions>())
 {
@@ -659,7 +658,6 @@ void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr)
 
     // add to map ordered by priority
     motion_provider_map_.emplace(_motion_provider_ptr->getStatePriority(), _motion_provider_ptr);
-    motion_provider_const_map_.emplace(_motion_provider_ptr->getStatePriority(), _motion_provider_ptr);
     appendToStructure(_motion_provider_ptr->getStateStructure());
 }
 
@@ -668,7 +666,6 @@ void Problem::removeMotionProvider(MotionProviderPtr proc)
     WOLF_WARN_COND(motion_provider_map_.count(proc->getStatePriority()) == 0, "Problem::clearMotionProvider: missing processor");
 
     motion_provider_map_.erase(proc->getStatePriority());
-    motion_provider_const_map_.erase(proc->getStatePriority());
 
     // rebuild frame structure with remaining motion processors
     frame_structure_.clear();