diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index fa980c0fa536a5b30cfbfffd243ae49047d8b3d5..69227baf8c19f94c5d16637a392e217fbbb63692 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -57,6 +57,14 @@ void PIDController::reset()
     previousError = 0;
     integral = 0;
     lastUpdateTime = TimeUtil::GetTime();
+    if (pdOutputFilter)
+    {
+        pdOutputFilter->reset();
+    }
+    if (differentialFilter)
+    {
+        differentialFilter->reset();
+    }
 }
 
 ScopedRecursiveLockPtr PIDController::getLock() const
@@ -144,7 +152,12 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
 
     firstRun = false;
     double oldControlValue = controlValue;
-    controlValue = Kp * error + Ki * integral + Kd * derivative;
+    double pdControlValue = Kp * error + Kd * derivative;
+    if (pdOutputFilter)
+    {
+        pdControlValue = pdOutputFilter->update(deltaSec, pdControlValue);
+    }
+    controlValue = pdControlValue + Ki * integral;
     if (deltaSec > 0.0)
     {
         double deriv = (controlValue - oldControlValue) / deltaSec;
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index 164acfd90464d15ee22395dc553913e06efbf887..5d9652e9a8d15068e873c7902b225285b50d9b75 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -68,6 +68,7 @@ namespace armarx
         bool limitless;
         bool threadSafe = true;
         rtfilters::RTFilterBasePtr differentialFilter;
+        rtfilters::RTFilterBasePtr pdOutputFilter;
     private:
         ScopedRecursiveLockPtr getLock() const;
         mutable RecursiveMutex mutex;