Merge pull request #17 from JuliaControl/wd

add derivative set-point weighting (`wd` parameter)

Author: baggepinnen

README.md

@@ -5,7 +5,7 @@
 
 
 This package implements a discrete-time PID controller as an approximation of the continuous-time PID controller given by
-$$U(s) = K \left( bR(s) - Y(s) + \dfrac{1}{sT_i} \left( R(s) - Y(s) \right) - \dfrac{sT_d}{1 + s T_d / N}Y(s) \right) + U_\textrm{ff}(s),$$
+$$U(s) = K \left( bR(s) - Y(s) + \dfrac{1}{sT_i} \left( R(s) - Y(s) \right) + \dfrac{sT_d}{1 + s T_d / N}(w_d R(s) - Y(s)) \right) + U_\textrm{ff}(s),$$
 where
 - $u(t) \leftrightarrow U(s)$ is the control signal
 - $y(t) \leftrightarrow Y(s)$ is the measurement signal
@@ -16,14 +16,15 @@ where
 - $T_d$ is the derivative time
 - $N$ is a parameter that limits the gain of the derivative term at high frequencies, typically ranges from 2 to 20,
 - $b \in [0, 1]$ is a parameter that gives the proportion of the reference signal that appears in the proportional term.
+- $w_d \in [0, 1]$ is a parameter that gives the proportion of the reference signal that appears in the derivative term (default 0).
 
 *Saturation* of the controller output is parameterized by $u_{\min}$ and $u_{\max}$, and the integrator *anti-windup* is parameterized by the tracking time $T_\mathrm{t}$.
 
 ## Usage
 
 Construct a controller by
 ```julia
-pid = DiscretePID(; K = 1, Ti = false, Td = false, Tt = √(Ti*Td), N = 10, b = 1, umin = -Inf, umax = Inf, Ts, I = 0, D = 0, yold = 0)
+pid = DiscretePID(; K = 1, Ti = false, Td = false, Tt = √(Ti*Td), N = 10, b = 1, wd = 0, umin = -Inf, umax = Inf, Ts, I = 0, D = 0, yold = 0)
 ```
 and compute the control signal at a given time using
 ```julia
@@ -219,7 +220,7 @@ K (b r - y + 1/T_i (r - y) - s T_d y/(1 + s T_d / N))
 using the function `K, Ti, Td = parallel2standard(kp, ki, kd)` or, if a filter parameter is included, `K, Ti, Td, N = parallel2standard(kp, ki, kd, Tf)`. This function also accepts a vector of parameters in the same order, in which case a vector is returned.
 
 ## Details
-- The derivative term only acts on the (filtered) measurement and not the command signal. It is thus safe to pass step changes in the reference to the controller. The parameter $b$ can further be set to zero to avoid step changes in the control signal in response to step changes in the reference.
+- The derivative term by default only acts on the (filtered) measurement and not the command signal. It is thus safe to pass step changes in the reference to the controller. Set `wd = 1` to let the derivative act on the error `r-y` instead. The parameter $b$ can further be set to zero to avoid step changes in the control signal in response to step changes in the reference.
 - Bumpless transfer when updating `K` is realized by updating the state `I`. See the docs for `set_K!` for more details.
 - The total control signal $u(t)$ (PID + feedforward) is limited by the integral anti-windup.
 - The integrator is discretized using a forward difference (no direct term between the input and output through the integral state) while the derivative is discretized using a backward difference. This approximation has the advantage that it is always stable and that the sampled pole goes to zero when $T_d$ goes to zero. Tustin's approximation gives an approximation such that the pole instead goes to $z = −1$ as $T_d$ goes to zero. 

src/DiscretePIDs.jl

@@ -19,7 +19,9 @@ mutable struct DiscretePID{T} <: Function
     "Maximum derivative gain"
     const N::T 
     "Fraction of set point in prop. term"
-    b::T 
+    b::T
+    "Fraction of set point in derivative term"
+    wd::T
     "Low output limit"
     umin::T 
     "High output limit"
@@ -34,12 +36,12 @@ mutable struct DiscretePID{T} <: Function
     I::T 
     "Derivative state"
     D::T 
-    "Last measurement signal"
+    "Last derivative error (wd*r - y)"
     yold::T 
 end
 
 """
-    DiscretePID(; K = 1, Ti = false, Td = false, Tt = √(Ti*Td), N = 10, b = 1, umin = -Inf, umax = Inf, Ts, I = 0, D = 0, yold = 0)
+    DiscretePID(; K = 1, Ti = false, Td = false, Tt = √(Ti*Td), N = 10, b = 1, wd = 0, umin = -Inf, umax = Inf, Ts, I = 0, D = 0, yold = 0)
 
 A discrete-time PID controller with set-point weighting and integrator anti-windup.
 The controller is implemented on the standard form
@@ -48,7 +50,7 @@ u = K \\left( e + \\dfrac{1}{Ti} \\int e dt + T_d \\dfrac{de}{dt} \\right)
 ```
 
 ```math
-U(s) = K \\left( bR(s) - Y(s) + \\dfrac{1}{sT_i} \\left( R(s) Y(s) \\right) - \\dfrac{sT_d}{1 + s T_d / N}Y(s)
+U(s) = K \\left( bR(s) - Y(s) + \\dfrac{1}{sT_i} \\left( R(s) Y(s) \\right) - \\dfrac{sT_d}{1 + s T_d / N}(Y(s) - w_d R(s))
 ```
 
 Call the controller like this
@@ -64,12 +66,13 @@ u = calculate_control!(pid, r, y, uff) # Equivalent to the above
 - `Tt`: Reset time for anti-windup
 - `N`: Maximum derivative gain
 - `b`: Fraction of set point in proportional term
+- `wd`: Fraction of set point in derivative term (default 0)
 - `umin`: Low output limit
 - `umax`: High output limit
 - `Ts`: Sampling period
 - `I`: Integral part
 - `D`: Derivative part
-- `yold`: Last measurement signal
+- `yold`: Last derivative error (wd*r - y)
 
 See also [`calculate_control!`](@ref), [`set_K!`](@ref), [`set_Ti!`](@ref), [`set_Td!`](@ref), [`reset_state!`](@ref).
 """
@@ -80,6 +83,7 @@ function DiscretePID(;
     Tt = Ti > 0 && Td > 0 ? typeof(K)(√(Ti*Td)) : typeof(K)(10),
     N  = typeof(K)(10),
     b  = typeof(K)(1),
+    wd = zero(typeof(K)),
     umin = typemin(typeof(K)),
     umax = typemax(typeof(K)),
     Ts,
@@ -96,6 +100,7 @@ function DiscretePID(;
     Td ≥ 0 || throw(ArgumentError("Td must be positive"))
     N ≥ 0 || throw(ArgumentError("N must be positive"))
     0 ≤ b ≤ 1 || throw(ArgumentError("b must be ∈ [0, 1]"))
+    0 ≤ wd ≤ 1 || throw(ArgumentError("wd must be ∈ [0, 1]"))
     umax > umin || throw(ArgumentError("umax must be greater than umin"))
 
     if Ti > 0
@@ -106,9 +111,9 @@ function DiscretePID(;
     ad = Td / (Td + N * Ts)
     bd = K * N * ad
 
-    T2 = promote_type(typeof.((K, Ti, Td, Tt, N, b, umin, umax, Ts, bi, ar, bd, ad, I, D, yold))...)
+    T2 = promote_type(typeof.((K, Ti, Td, Tt, N, b, wd, umin, umax, Ts, bi, ar, bd, ad, I, D, yold))...)
 
-    DiscretePID(T2.((K, Ti, Td, Tt, N, b, umin, umax, Ts, bi, ar, bd, ad, I, D, yold))...)
+    DiscretePID(T2.((K, Ti, Td, Tt, N, b, wd, umin, umax, Ts, bi, ar, bd, ad, I, D, yold))...)
 end
 
 """
@@ -173,15 +178,16 @@ function calculate_control!(pid::DiscretePID{T}, r0, y0, uff0=0; yd=nothing) whe
     y = T(y0)
     uff = T(uff0)
     P = pid.K * (pid.b * r - y)
+    e = pid.wd * r - y  # weighted error for derivative
     if yd === nothing
-        pid.D = pid.ad * pid.D - pid.bd * (y - pid.yold)
+        pid.D = pid.ad * pid.D + pid.bd * (e - 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)
-    pid.yold = y
+    pid.yold = e  # store weighted error for next derivative calculation
     return u
 end
 

test/runtests.jl

@@ -164,6 +164,35 @@ res2 = lsim(P, ctrl, Tf)
 # @test res.y ≈ res2.y rtol=0.01
 
 
+## PID control with derivative set-point weighting (wd parameter)
+# Compare wd=0 (derivative only on -y) vs wd=1 (derivative on r-y)
+Tf = 30
+Ti = 1
+Td = 1
+pid_wd0 = DiscretePID(; K, Ts, Ti, Td, wd=0)
+pid_wd1 = DiscretePID(; K, Ts, Ti, Td, wd=1)
+
+ctrl_wd0 = function(x, t)
+    y = (P.C*x)[]
+    r = (t >= 5)  # Step in reference
+    pid_wd0(r, y)
+end
+
+ctrl_wd1 = function(x, t)
+    y = (P.C*x)[]
+    r = (t >= 5)  # Step in reference
+    pid_wd1(r, y)
+end
+
+res_wd0 = lsim(P, ctrl_wd0, Tf)
+res_wd1 = lsim(P, ctrl_wd1, Tf)
+
+# With wd=1, step in r causes derivative kick; with wd=0 it doesn't
+# So the control signals should differ, especially around t=5
+@test res_wd0.y != res_wd1.y  # Results should be different
+@test maximum(abs.(res_wd1.u)) > 5*maximum(abs.(res_wd0.u))  # wd=1 has larger control signal due to derivative kick
+
+
 ## PID control with bumpless transfer
 # Here we simulate a load disturbance instead since the discrete PID does not differentiate r, while the ControlSystemsBase.pid does.
 Tf = 10