diff --git a/source/RobotAPI/libraries/core/MultiDimPIDController.h b/source/RobotAPI/libraries/core/MultiDimPIDController.h
index 8c97be3dd22267f25fc34171c961e0a74bb07907..4186cb9b15f0134e0d876194924dfe97fb7a735c 100644
--- a/source/RobotAPI/libraries/core/MultiDimPIDController.h
+++ b/source/RobotAPI/libraries/core/MultiDimPIDController.h
@@ -71,6 +71,12 @@ namespace armarx
         void update(const double deltaSec, const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
         {
             ScopedRecursiveLockPtr lock = getLock();
+            if (stackAllocations.zeroVec.rows() == 0)
+            {
+                preallocate(measuredValue.rows());
+            }
+            ARMARX_CHECK_EQUAL(measuredValue.rows(), targetValue.rows());
+            ARMARX_CHECK_EQUAL(measuredValue.rows(), stackAllocations.zeroVec.rows());
             processValue = measuredValue;
             target = targetValue;
 
@@ -96,7 +102,7 @@ namespace armarx
             if (!firstRun)
             {
                 integral += error * deltaSec;
-                integral = std::min(integral,maxIntegral);
+                integral = std::min(integral, maxIntegral);
                 if (deltaSec > 0.0)
                 {
                     derivative = (error - previousError) / deltaSec;