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

Discussion:
shenxiangzhuang: Hi! I think this option parameter is good for unnecessary misunderstanding about the implementation, and I will be happy that this PR can be merged as early as possible. Would you mind that I solve the conflict and add new PR to merge? @m-lundberg 
shenxiangzhuang: > I apologize for the really late review. If you are still interested in getting this merged I am approving your changes now.
> 
> You will have to resolve some merge conflicts though unfortunately. Please, if you could rebase your changes on top of the latest main that would be great. Let me know if you're not sure how to do this and I can help.



> Hi! I think this option parameter is good for unnecessary misunderstanding about the implementation, and I will be happy that this PR can be merged as early as possible. Would you mind that I solve the conflict and add new PR to merge? @m-lundberg

Or if @kmodexc can notice the message and solve the conflict would be better.
kmodexc: Sry im doing that in the next days. Totally forgot about it.
kmodexc: done
kmodexc: @m-lundberg 

kmodexc: Im not doing this a second time

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