diff --git a/source/RobotAPI/libraries/core/TrajectoryController.cpp b/source/RobotAPI/libraries/core/TrajectoryController.cpp
index 063bef2f10e5ccfb62fdf15bdcb3f6c8d42bb783..0884d4cff46f75a3d58e873e8b05a76998f08771 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.cpp
+++ b/source/RobotAPI/libraries/core/TrajectoryController.cpp
@@ -47,12 +47,12 @@ namespace armarx
         veloctities.resize(traj->dim(), 1);
     }
 
-    Eigen::VectorXf TrajectoryController::update(double deltaT, const Eigen::VectorXf currentPosition)
+    const Eigen::VectorXf& TrajectoryController::update(double deltaT, const Eigen::VectorXf& currentPosition)
     {
         ARMARX_CHECK_EXPRESSION(pid);
         ARMARX_CHECK_EXPRESSION(traj);
         ARMARX_CHECK_EQUAL(static_cast<std::size_t>(currentPosition.rows()), traj->dim());
-        int dim = traj->dim();
+        size_t dim = traj->dim();
         currentTimestamp  = currentTimestamp + deltaT;
 
         if (currentTimestamp < 0.0)
@@ -60,7 +60,7 @@ namespace armarx
             currentTimestamp = 0.0;
         }
 
-        for (int i = 0; i < dim; ++i)
+        for (size_t i = 0; i < dim; ++i)
         {
             positions(i) = traj->getState(currentTimestamp, i, 0);
             veloctities(i) = (std::signbit(deltaT) ? -1.0 : 1.0) * traj->getState(currentTimestamp, i, 1);
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.h b/source/RobotAPI/libraries/core/TrajectoryController.h
index a12d592f4f9fdefcf7e96bc6fc0967c09f95ac24..f715d29aed2fccf9b077be81bf27207fd5dab555 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.h
+++ b/source/RobotAPI/libraries/core/TrajectoryController.h
@@ -33,7 +33,7 @@ namespace armarx
     {
     public:
         TrajectoryController(const TrajectoryPtr& traj, float kp, float ki = 0.0f, float kd = 0.0f, bool threadSafe = true);
-        Eigen::VectorXf update(double deltaT, const Eigen::VectorXf currentPosition);
+        const Eigen::VectorXf& update(double deltaT, const Eigen::VectorXf& currentPosition);
         //const MultiDimPIDControllerPtr& getPid() const;
         //void setPid(const MultiDimPIDControllerPtr& value);