add hybrid integrator option

README.md

@@ -195,4 +195,73 @@ The fixed-point controller behaves roughly the same in this case, but artifacts
 
 ## See also
 - [TrajectoryLimiters.jl](https://github.com/baggepinnen/TrajectoryLimiters.jl) To generate dynamically feasible reference trajectories with bounded velocity and acceleration given an instantaneous reference $r(t)$ which may change abruptly.
-- [SymbolicControlSystems.jl](https://github.com/JuliaControl/SymbolicControlSystems.jl) For C-code generation of LTI systems.
+- [SymbolicControlSystems.jl](https://github.com/JuliaControl/SymbolicControlSystems.jl) For C-code generation of LTI systems.
+
+
+
+## HIGS PID
+If the additional keyword argument `Kh` is provided to the constructor of `DiscretePID`, a "hybrid" integrator (HIGS) element is connected in series with the standard integrator. The HIGS integrator switches to a proportional gain when a certain condition fails to hold, increasing the phase of the integrator for high frequencies. See
+> "Hybrid integrator design for enhanced tracking in motion control" D.A. Deenen M.F. Heertjes W.P.M.H. Heemels H. Nijmeijer
+
+and 
+
+> "A solution to gain loss in hybrid integrator-gain systems", M.F. Heertjes S.J.A.M. Van den Eijnden W.P.M.H. Heemels H. Nijmeijer
+
+for more details. The HIGS integrator, similarly to a Clegg integrator, is a nonlinear controller that improves upon the fundamental limitations of linear controllers imposed by Bode's sensitivity integral theorem.
+
+Below, we compare a standard PI controller to a HIGS PI controller. The simulation starts with a reference step of magnitude 1, followed by a disturbance step of magnitude -1 at $t = 25$.
+```julia
+using DiscretePIDs, ControlSystemsBase, Plots, Printf
+Tf = 50   # Simulation time
+K  = 1.0  # Proportional gain
+Ti = 5    # Integral time
+Kh = 1    # Hybrid integrator gain (when in gain mode)
+Ts = 0.01 # sample time
+
+P   = c2d(ss(tf(1, [1, 1.2, 0])), Ts) # Process to be controlled, discretized using zero-order hold
+
+fig = plot(layout=(2, 1), legend=:bottomright)
+
+
+for pid in [DiscretePID(; K, Ts, Ti, b=0.7), DiscretePID(; K, Ts, Ti=0.7Ti, b=0.8, Kh)]
+
+    ctrl = function(x,t)
+        y = (P.C*x)[] # measurement
+        d = -(t > 25)         # disturbance
+        r = 1         # reference
+        u = pid(r, y)
+        u + d # Plant input is control signal + disturbance
+    end
+
+    res = lsim(P, ctrl, Tf)
+    rms = @sprintf("%4.2e", sqrt(mean(abs2, res.y[end] .- 1)))
+    plot!(res, plotu=true, label="Hybid = $(pid.Kh > 0) RMS: $rms")
+end
+fig
+```
+
+### Paper demo
+```julia
+using DiscretePIDs, ControlSystemsBase, Plots
+Tf = 2π   # Simulation time
+K  = 1.0    # Proportional gain
+Ti = 1    # Integral time
+Ki = 1.5    # Integral gain
+Ts = 0.01 # sample time
+
+ts = range(0, step=Ts, stop=Tf)
+es = @. sin(ts)+0.5sin(3ts)
+fig = plot(ts, repeat(Ki.*es, 1, 4), lab="ke", l=(2), layout=4)
+
+for (i, int) in enumerate([Sector(), Clegg(), Clamp()])
+    pid = HIGSPI(; K, Ts, Ti, Ki, integrator=int)
+    res = map(ts) do t
+        e = sin(t)+0.5sin(3t)         # reference
+        DiscretePIDs.integration!(pid, e)
+        pid.I
+    end
+
+    plot!(ts, res, label=string(int), l=:dash, sp=i)
+end
+fig
+```

src/DiscretePIDs.jl

@@ -13,7 +13,9 @@ mutable struct DiscretePID{T} <: Function
     "Integral time"
     Ti::T 
     "Derivative time"
-    Td::T 
+    Td::T
+    "Hybrid integrator gain"
+    Kh::T
     "Reset time"
     Tt::T 
     "Maximum derivative gain"
@@ -33,7 +35,9 @@ mutable struct DiscretePID{T} <: Function
     "Integral state"
     I::T 
     "Derivative state"
-    D::T 
+    D::T
+    "Hybrid integrator state"
+    Ih::T
     "Last measurement signal"
     yold::T 
 end
@@ -77,6 +81,7 @@ function DiscretePID(;
     K::T  = 1f0,
     Ti = false,
     Td = false,
+    Kh = false,
     Tt = Ti > 0 && Td > 0 ? typeof(K)(√(Ti*Td)) : typeof(K)(10),
     N  = typeof(K)(10),
     b  = typeof(K)(1),
@@ -85,6 +90,7 @@ function DiscretePID(;
     Ts,
     I    = zero(typeof(K)),
     D    = zero(typeof(K)),
+    Ih   = zero(typeof(K)),
     yold = zero(typeof(K)),
 ) where T
     if Ti > 0
@@ -106,9 +112,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, Kh, Tt, N, b, 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, Kh, Tt, N, b, umin, umax, Ts, bi, ar, bd, ad, I, Ih, D, yold))...)
 end
 
 """
@@ -123,6 +129,9 @@ function set_K!(pid::DiscretePID, K, r, y)
     if pid.Ti > 0
         pid.bi = K * pid.Ts / pid.Ti
         pid.I = pid.I + Kold*(pid.b*r - y) - K*(pid.b*r - y)
+        if pid.Kh > 0
+            pid.Ih = pid.Ih + Kold*(pid.b*r - y) - K*(pid.b*r - y)
+        end
     end
 end
 
@@ -165,11 +174,24 @@ function calculate_control!(pid::DiscretePID{T}, r0, y0, uff0=0) where T
     r = T(r0)
     y = T(y0)
     uff = T(uff0)
+    e = r - y
     P = pid.K * (pid.b * r - y)
     pid.D = pid.ad * pid.D - pid.bd * (y - pid.yold)
-    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)
+    if pid.Kh > 0 # HIGS integrator
+        v = P + pid.I + abs(1+4im/π)*pid.K/pid.Ti*pid.Ih + pid.D + uff
+        u = clamp(v, pid.umin, pid.umax)
+        pid.Ih = pid.Ih + pid.bi * e
+        k = pid.K*pid.Kh
+        ke = k*e
+        if (pid.Ih > ke && ke > 0) || (pid.Ih < ke && ke < 0)
+            pid.Ih = ke
+        end
+        pid.I = pid.I + pid.bi * pid.Ih + pid.ar * (u - v)
+    else # Standard integrator
+        v = P + pid.I + pid.D + uff
+        u = clamp(v, pid.umin, pid.umax)
+        pid.I = pid.I + pid.bi * e + pid.ar * (u - v)
+    end
     pid.yold = y
     return u
 end
@@ -184,5 +206,98 @@ function Base.show(io::IO, ::MIME"text/plain", pid::DiscretePID)
     println(io, ")")
 end
 
+# ==============================================================================
+## HIGSPID
+# ==============================================================================
+export HIGSPI, Sector, Standard, Clegg, Clamp
+mutable struct HIGSPI{T, A} <: Function
+    "Proportional gain"
+    K::T 
+    "Integral time"
+    Ti::T 
+    Ki::T
+    "Sampling period"
+    const Ts::T 
+    "Integral state"
+    I::T 
+    "Last measurement signal"
+    eold::T
+    integrator::A
+end
+
+abstract type Integrator end
+struct Sector <: Integrator end
+struct Standard <: Integrator end
+struct Clegg <: Integrator end
+struct Clamp <: Integrator end
+
+function integration!(pid::HIGSPI{T, Sector}, e) where T
+    w = pid.K*pid.Ts/pid.Ti
+    k = pid.K*pid.Ki
+    eold = pid.eold
+    u = pid.I
+
+    ei = e
+    int = e > 0 && u < k*ei || e < 0 && k*ei < u
+    pid.I = if int
+        if e > 0
+            max(pid.I + w * e, zero(T))
+        else
+            min(pid.I + w * e, zero(T))
+        end
+    else
+        k * e
+    end
+end
+
+
+function integration!(pid::HIGSPI{<:Any, Standard}, e)
+    w = pid.K*pid.Ts/pid.Ti
+    pid.I = pid.I + w * e
+end
+
+function integration!(pid::HIGSPI{T, Clegg}, e) where T
+    w = pid.K*pid.Ts/pid.Ti
+    pid.I = if e*pid.eold >= 0 # e*ui ≥ 0
+        pid.I + w * e
+    else
+        zero(T)#k*e
+    end
+end
+
+# Note, this kind of integrator element only really works well when the plant contains an integrator as well. For, e.g., a first-order plant, there will be a stationary error with this integrator alone. 
+function integration!(pid::HIGSPI{<:Any, Clamp}, e)
+    # This implmentation is mitivated by Fig 3 in https://heemels.tue.nl/content/papers/DeeHee_WA17a.pdf
+    w = pid.K*pid.Ts/pid.Ti
+    pid.I = pid.I + w * e
+    k = pid.K*pid.Ki
+    ke = k*e
+    if (pid.I > ke && ke > 0) || (pid.I < ke && ke < 0)
+        pid.I = ke
+    end
+end
+
+
+function HIGSPI(;K::T=1, Ti=1, Ki=1, Ts, I=zero(Ts), yold=zero(Ts), integrator=Sector()) where T
+    Ti ≥ 0 || throw(ArgumentError("Ti must be positive"))
+    HIGSPI(T(K), T(Ti), T(Ki), T(Ts), T(I), T(yold), integrator)
+end
+
+function calculate_control!(pid::HIGSPI{T}, r0, y0, uff0=0) where T
+    r = T(r0)
+    y = T(y0)
+    uff = T(uff0)
+    e = (r - y)
+    P = pid.K * e
+    u = P + pid.I + uff
+
+    integration!(pid, e)
+
+    pid.eold = e
+    return u
+end
+
+(pid::HIGSPI)(args...) = calculate_control!(pid, args...)
+
 
 end