add support for externally sourced derivative

Author: baggepinnen

Project.toml

@@ -1,7 +1,7 @@
 name = "DiscretePIDs"
 uuid = "c1363496-6848-4723-8758-079b737f6baf"
 authors = ["Fredrik Bagge Carlson"]
-version = "0.1.6"
+version = "0.1.7"
 
 [deps]
 Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7"

README.md

@@ -34,6 +34,12 @@ or
 u = calculate_control!(pid, r, y, uff)
 ```
 
+The derivative term is by default computed by filtering the measurement $y$, but it can also be sourced externally by setting the keyword argument `yd`:
+```julia
+u = calculate_control!(pid, r, y, uff; yd)
+```
+When `yd` is provided, no filtering is applied by the PID controller, i.e., $N$ is ignored. This is useful when the derivative is computed externally, e.g., from a velocity sensor or an observer.
+
 The parameters $K$, $T_i$, and $T_d$ may be updated using the functions `set_K!`, `set_Ti!`, and `set_Td!`, respectively.
 
 The numeric type used by the controller (the `T` in `DiscretePID{T}`) is determined by the types of the parameters. To use a custom number type, e.g., a fixed-point number type, simply pass the parameters as that type, see example below. The controller will automatically convert measurements and references to this type before performing the control calculations.

src/DiscretePIDs.jl

@@ -160,18 +160,24 @@ end
 
 
 """
-    u = calculate_control!(pid::DiscretePID, r, y, uff=0)
+    u = calculate_control!(pid::DiscretePID, r, y, uff=0; yd = nothing)
     (pid)(r, y, uff=0) # Alternative syntax
 
 Calculate the control output from the PID controller when `r` is the reference (set point), `y` is the latest measurement and `uff` is the feed-forward contribution.
 If the type of the input arguments differ from the numeric type used by the PID controller, they will be converted before computations are performed.
+
+The derivative term is by default computed by filtering the measurement `y`, but it can also be provided directly by setting the optional keyword argument `yd`. When `yd` is set, no filtering is applied by the PID controller, i.e., `N` is ignored.
 """
-function calculate_control!(pid::DiscretePID{T}, r0, y0, uff0=0) where T
+function calculate_control!(pid::DiscretePID{T}, r0, y0, uff0=0; yd=nothing) where T
     r = T(r0)
     y = T(y0)
     uff = T(uff0)
     P = pid.K * (pid.b * r - y)
-    pid.D = pid.ad * pid.D - pid.bd * (y - pid.yold)
+    if yd === nothing
+        pid.D = pid.ad * pid.D - pid.bd * (y - pid.yold)
+    else
+        pid.D = - pid.K * pid.Td * T(yd)
+    end
     v = P + pid.I + pid.D + uff
     u = clamp(v, pid.umin, pid.umax)
     pid.I = pid.I + pid.bi * (r - y) + pid.ar * (u - v)

test/runtests.jl

@@ -92,6 +92,40 @@ res2 = lsim(P, ctrl, Tf)
 @test res.y ≈ res2.y rtol=0.02
 # plot([res, res2])
 
+## PID control with external derivative (yd keyword)
+# Compare internal filtered derivative vs externally provided derivative
+pid_internal = DiscretePID(; K = 1.0*K, Ts, Ti, Td = 0.8*Td)
+pid_external = DiscretePID(; K = 1.0*K, Ts, Ti, Td = 0.8*Td)
+
+yold_ext = 0.0
+ctrl_internal = function(x, t)
+    y = (P.C*x)[]
+    d = 1
+    r = 0
+    u = pid_internal(r, y)
+    u + d
+end
+
+ctrl_external = function(x, t)
+    global yold_ext
+    y = (P.C*x)[]
+    d = 1
+    r = 0
+    yd = (y - yold_ext) / Ts  # Compute raw derivative of -y
+    yold_ext = y
+    u = calculate_control!(pid_external, r, y; yd)
+    u + d
+end
+
+res_internal = lsim(P, ctrl_internal, Tf)
+reset_state!(pid_internal)
+reset_state!(pid_external)
+yold_ext = 0.0
+res_external = lsim(P, ctrl_external, Tf)
+
+# External derivative is unfiltered, so allow larger tolerance
+@test res_internal.y ≈ res_external.y rtol=0.1
+
 reset_state!(pid)
 res3 = lsim(P, ctrl, Tf)
 @test res3.y == res2.y