Repository: m-lundberg/simple-pid

Issue:
Derivative Term is calculated on error and not input
the derivative term is calculated on the derivative of the error term and not on the derivative of the input ([see here](https://en.wikipedia.org/wiki/PID_controller#Derivative_term)). 

The code below should not contain something like dinput but a derror term which should be calculated somewhere.

https://github.com/m-lundberg/simple-pid/blob/15a030fc81f73dc6bfaf8c3084dcf49a415d7e4b/simple_pid/PID.py#L127

Comments:

Patch:
diff --git a/README.md b/README.md
index 150d087..907aa03 100644
--- a/README.md
+++ b/README.md
@@ -99,6 +99,12 @@ To eliminate overshoot in certain types of systems, you can calculate the [propo
 pid.proportional_on_measurement = True
 ```
 
+#### Differential on measurement
+By default the [differential term is calculated on the measurement](http://brettbeauregard.com/blog/2011/04/improving-the-beginner%e2%80%99s-pid-derivative-kick/) instead of the error. This can be disabled like this:
+```python
+pid.differential_on_measurement = False
+```
+
 #### Error mapping
 To transform the error value to another domain before doing any computations on it, you can supply an `error_map` callback function to the PID. The callback function should take one argument which is the error from the setpoint. This can be used e.g. to get a degree value error in a yaw angle control with values between [-pi, pi):
 ```python
diff --git a/simple_pid/pid.py b/simple_pid/pid.py
index db66c3f..ffcbfbf 100644
--- a/simple_pid/pid.py
+++ b/simple_pid/pid.py
@@ -35,6 +35,7 @@ class PID(object):
         output_limits=(None, None),
         auto_mode=True,
         proportional_on_measurement=False,
+        differetial_on_measurement=True,
         error_map=None,
     ):
         """
@@ -58,6 +59,8 @@ class PID(object):
         :param proportional_on_measurement: Whether the proportional term should be calculated on
             the input directly rather than on the error (which is the traditional way). Using
             proportional-on-measurement avoids overshoot for some types of systems.
+        :param differetial_on_measurement: Whether the differential term should be calculated on
+            the input directly rather than on the error (which is the traditional way).
         :param error_map: Function to transform the error value in another constrained value.
         """
         self.Kp, self.Ki, self.Kd = Kp, Ki, Kd
@@ -67,6 +70,7 @@ class PID(object):
         self._min_output, self._max_output = None, None
         self._auto_mode = auto_mode
         self.proportional_on_measurement = proportional_on_measurement
+        self.differetial_on_measurement = differetial_on_measurement
         self.error_map = error_map
 
         self._proportional = 0
@@ -75,6 +79,7 @@ class PID(object):
 
         self._last_time = None
         self._last_output = None
+        self._last_error = None
         self._last_input = None
 
         self.output_limits = output_limits
@@ -107,6 +112,7 @@ class PID(object):
         # Compute error terms
         error = self.setpoint - input_
         d_input = input_ - (self._last_input if (self._last_input is not None) else input_)
+        d_error = error - (self._last_error if (self._last_error is not None) else error)
 
         # Check if must map the error
         if self.error_map is not None:
@@ -124,7 +130,10 @@ class PID(object):
         self._integral += self.Ki * error * dt
         self._integral = _clamp(self._integral, self.output_limits)  # Avoid integral windup
 
-        self._derivative = -self.Kd * d_input / dt
+        if self.differetial_on_measurement:
+            self._derivative = -self.Kd * d_input / dt
+        else:
+            self._derivative = self.Kd * d_error / dt
 
         # Compute final output
         output = self._proportional + self._integral + self._derivative
@@ -133,6 +142,7 @@ class PID(object):
         # Keep track of state
         self._last_output = output
         self._last_input = input_
+        self._last_error = error
         self._last_time = now
 
         return output
@@ -143,7 +153,8 @@ class PID(object):
             'Kp={self.Kp!r}, Ki={self.Ki!r}, Kd={self.Kd!r}, '
             'setpoint={self.setpoint!r}, sample_time={self.sample_time!r}, '
             'output_limits={self.output_limits!r}, auto_mode={self.auto_mode!r}, '
-            'proportional_on_measurement={self.proportional_on_measurement!r},'
+            'proportional_on_measurement={self.proportional_on_measurement!r}, '
+            'differetial_on_measurement={self.differetial_on_measurement!r}, '
             'error_map={self.error_map!r}'
             ')'
         ).format(self=self)
diff --git a/simple_pid/pid.pyi b/simple_pid/pid.pyi
index dfe451a..2df4f23 100644
--- a/simple_pid/pid.pyi
+++ b/simple_pid/pid.pyi
@@ -15,6 +15,7 @@ class PID(object):
     setpoint: float
     sample_time: Optional[float]
     proportional_on_measurement: bool
+    differential_on_measurement: bool
     error_map: Optional[Callable[[float], float]]
     def __init__(
         self,
@@ -26,6 +27,7 @@ class PID(object):
         output_limits: _Limits = ...,
         auto_mode: bool = ...,
         proportional_on_measurement: bool = ...,
+        differential_on_measurement: bool = ...,
         error_map: Optional[Callable[[float], float]] = ...,
     ) -> None: ...
     def __call__(self, input_: float, dt: Optional[float] = ...) -> Optional[float]: ...


Test patch:
diff --git a/tests/test_pid.py b/tests/test_pid.py
index 886347e..6364684 100644
--- a/tests/test_pid.py
+++ b/tests/test_pid.py
@@ -218,6 +218,27 @@ def test_converge_system():
     assert abs(pv - 5) < 0.1
 
 
+def test_converge_diff_on_error():
+    pid = PID(1, 0.8, 0.04, setpoint=5, output_limits=(-5, 5), differetial_on_measurement=False)
+    pv = 0  # Process variable
+
+    def update_system(c, dt):
+        # Calculate a simple system model
+        return pv + c * dt - 1 * dt
+
+    start_time = time.time()
+    last_time = start_time
+
+    while time.time() - start_time < 12:
+        c = pid(pv)
+        pv = update_system(c, time.time() - last_time)
+
+        last_time = time.time()
+
+    # Check if system has converged
+    assert abs(pv - 5) < 0.1
+
+
 def test_error_map():
     import math