diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index a9c836515a8d3940ce2d18cc1a4f1603cb2e5773..6a978e80120f45608b2c54685653ee90bf3868a1 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -44,11 +44,16 @@ void PlatformUnitSimulation::onInitPlatformUnit()
     maxLinearVelocity = getProperty<float>("LinearVelocity").getValue();
     maxAngularVelocity = getProperty<float>("AngularVelocity").getValue();
 
+    velPID.reset(new MultiDimPIDController(getProperty<float>("Kp_Velocity").getValue(), 0, 0, getProperty<float>("MaxLinearAcceleration").getValue()));
+    posPID.reset(new MultiDimPIDController(getProperty<float>("Kp_Position").getValue(), 0, 0, maxLinearVelocity, getProperty<float>("MaxLinearAcceleration").getValue()));
+
+
     positionalAccuracy = 0.01;
 
     intervalMs = getProperty<int>("IntervalMs").getValue();
     ARMARX_VERBOSE << "Starting platform unit simulation with " << intervalMs << " ms interval";
     simulationTask = new PeriodicTask<PlatformUnitSimulation>(this, &PlatformUnitSimulation::simulationFunction, intervalMs, false, "PlatformUnitSimulation");
+
 }
 
 void PlatformUnitSimulation::onStartPlatformUnit()
@@ -78,8 +83,9 @@ void PlatformUnitSimulation::onExitPlatformUnit()
 void PlatformUnitSimulation::simulationFunction()
 {
     // the time it took until this method was called again
-    double timeDeltaInSeconds = (TimeUtil::GetTime() - lastExecutionTime).toMilliSecondsDouble() / 1000.0;
-    lastExecutionTime = TimeUtil::GetTime();
+    auto now = TimeUtil::GetTime();
+    double timeDeltaInSeconds = (now - lastExecutionTime).toSecondsDouble();
+    lastExecutionTime = now;
     std::vector<float> vel(3, 0.0f);
     {
         ScopedLock lock(currentPoseMutex);
@@ -87,26 +93,16 @@ void PlatformUnitSimulation::simulationFunction()
         {
             case ePositionControl:
             {
-                float diff = fabs(targetPositionX - currentPositionX);
-                ARMARX_INFO << deactivateSpam(0.5) << "Distance to goal X: " << diff;
-
-                if (diff > positionalAccuracy)
-                {
-                    int sign = copysignf(1.0f, (targetPositionX - currentPositionX));
-                    currentPositionX += sign * std::min<float>(maxLinearVelocity * timeDeltaInSeconds, diff);
-                    vel[0] = linearVelocityX;
-                }
+                posPID->update(timeDeltaInSeconds,
+                               Eigen::Vector2f(currentPositionX, currentPositionY),
+                               Eigen::Vector2f(targetPositionX, targetPositionY));
+                float newXTickMotion = posPID->getControlValue()[0] * timeDeltaInSeconds;
+                currentPositionX += newXTickMotion;
+                currentPositionY += posPID->getControlValue()[1] * timeDeltaInSeconds;
+                vel[0] = posPID->getControlValue()[0];
+                vel[1] = posPID->getControlValue()[1];
 
-                diff = fabs(targetPositionY - currentPositionY);
-
-                if (diff > positionalAccuracy)
-                {
-                    int sign = copysignf(1.0f, (targetPositionY - currentPositionY));
-                    currentPositionY +=  sign * std::min<float>(maxLinearVelocity * timeDeltaInSeconds, diff);
-                    vel[1] = linearVelocityY;
-                }
-
-                diff = fabs(targetRotation - currentRotation);
+                float diff = fabs(targetRotation - currentRotation);
 
                 if (diff > orientationalAccuracy)
                 {
@@ -132,10 +128,14 @@ void PlatformUnitSimulation::simulationFunction()
             case eVelocityControl:
             {
                 Eigen::Vector2f targetVel(linearVelocityX, linearVelocityY);
+                velPID->update(timeDeltaInSeconds, currentTranslationVelocity, targetVel);
+                currentTranslationVelocity += timeDeltaInSeconds * velPID->getControlValue();
+                currentPositionX += timeDeltaInSeconds * currentTranslationVelocity[0];
+                currentPositionY += timeDeltaInSeconds * currentTranslationVelocity[1];
+
+
                 Eigen::Rotation2Df rot(currentRotation);
                 targetVel = rot * targetVel;
-                currentPositionX += timeDeltaInSeconds * targetVel[0];
-                currentPositionY += timeDeltaInSeconds * targetVel[1];
                 currentRotation += angularVelocity * timeDeltaInSeconds;
 
                 // stay between +/- M_2_PI
@@ -148,8 +148,8 @@ void PlatformUnitSimulation::simulationFunction()
                 {
                     currentRotation += 2 * M_PI;
                 }
-                vel[0] = linearVelocityY;
-                vel[1] = linearVelocityY;
+                vel[0] = currentTranslationVelocity[0];
+                vel[1] = currentTranslationVelocity[1];
                 vel[2] = angularVelocity;
             }
             break;
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.h b/source/RobotAPI/components/units/PlatformUnitSimulation.h
index 71444589c8277c6cb4cdc1b35a0d96091b228847..8ebf0526b9396365b7da24e919c64a59b0cc1db5 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.h
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.h
@@ -29,6 +29,7 @@
 
 #include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 #include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <RobotAPI/libraries/core/PIDController.h>
 
 #include <IceUtil/Time.h>
 
@@ -53,6 +54,9 @@ namespace armarx
             defineOptionalProperty<float>("InitialPosition.y", 0, "Initial y position of the platform.");
             defineOptionalProperty<std::string>("ReferenceFrame", "Platform", "Reference frame in which the platform position is reported.");
             defineOptionalProperty<float>("LinearVelocity", 1000.0, "Linear velocity of the platform (default: 1000 mm/sec).");
+            defineOptionalProperty<float>("MaxLinearAcceleration", 1000.0, "Linear acceleration of the platform (default: 1000 mm/sec).");
+            defineOptionalProperty<float>("Kp_Position", 5.0, "P value of the PID position controller");
+            defineOptionalProperty<float>("Kp_Velocity", 5.0, "P value of the PID velocity controller");
             defineOptionalProperty<float>("AngularVelocity", 1.5, "Angular velocity of the platform (default: 1.5 rad/sec).");
         }
     };
@@ -125,6 +129,8 @@ namespace armarx
         ::Ice::Float orientationalAccuracy;
 
         std::string referenceFrame;
+        MultiDimPIDControllerPtr velPID, posPID;
+        Eigen::Vector2f currentTranslationVelocity = Eigen::Vector2f::Zero();
 
         PeriodicTask<PlatformUnitSimulation>::pointer_type simulationTask;