diff --git a/src/control_affine_system.jl b/src/control_affine_system.jl
index f225314..a4947ce 100644
--- a/src/control_affine_system.jl
+++ b/src/control_affine_system.jl
@@ -10,12 +10,12 @@ Control affine system described by the dynamics ``ẋ = f(x) + g(x)u`` where ``x
 - `f::Function` : drift dynamics
 - `g::Function` : control directions
 """
-struct ControlAffineSystem
+struct ControlAffineSystem{F,G}
     name::String
     n::Int
     m::Int
-    f::Function
-    g::Function
+    f::F
+    g::G
 end
 
 """
diff --git a/src/control_barrier_function.jl b/src/control_barrier_function.jl
index 1c7be19..6525224 100644
--- a/src/control_barrier_function.jl
+++ b/src/control_barrier_function.jl
@@ -10,12 +10,12 @@ Control barrier function (CBF) defining a safe set as its zero superlevel set.
 - `Lfh::Function` : Lie derivative of CBF along drift vector field `f`
 - `Lgh::Function` : Lie derivative of CBF along control directions `g`
 """
-struct ControlBarrierFunction
-    h::Function
-    α::Function
-    ∇h::Function
-    Lfh::Function
-    Lgh::Function
+struct ControlBarrierFunction{Th, Tα, T∇h, TLfh, TLgh}
+    h::Th
+    α::Tα
+    ∇h::T∇h
+    Lfh::TLfh
+    Lgh::TLgh
 end
 
 """
@@ -44,4 +44,4 @@ end
 If no extended class K function provided, default to the identify function.
 """
 ControlBarrierFunction(h::Function, Σ::ControlAffineSystem) =
-    ControlBarrierFunction(h, Σ, r -> r)
+    ControlBarrierFunction(h, Σ, identity)
diff --git a/src/explicit_safety_filter.jl b/src/explicit_safety_filter.jl
index 87636d2..a2689d0 100644
--- a/src/explicit_safety_filter.jl
+++ b/src/explicit_safety_filter.jl
@@ -6,8 +6,8 @@ Controller that uses the closed-form solution to a control barrier function quad
 # Fields
 - `k::Function` : function that computes safe control actions
 """
-struct ExplicitSafetyFilter <: SafetyFilter
-    k::Function
+struct ExplicitSafetyFilter{T} <: SafetyFilter
+    k::T
 end
 
 """
@@ -15,8 +15,7 @@ end
 
 Functors for evaluating explicit safety filter
 """
-(k::ExplicitSafetyFilter)(x) = k.k(x)
-(k::ExplicitSafetyFilter)(x, t) = k.k(x, t)
+(k::ExplicitSafetyFilter)(args...) = k.k(args...)
 
 """
     ExplicitSafetyFilter(cbf::ControlBarrierFunction, Σ::ControlAffineSystem, kd::Function)
@@ -26,23 +25,14 @@ Construct an ExplicitSafetyFilter from a cbf and a desired controller.
 function ExplicitSafetyFilter(
     cbf::ControlBarrierFunction, Σ::ControlAffineSystem, kd::Function
 )
-    try
-        kd(Σ.n == 1 ? rand() : rand(Σ.n), 0.0)
-    catch e
-        if isa(e, MethodError)
-            a(x) = cbf.Lfh(x) + cbf.Lgh(x) * kd(x) + cbf.α(cbf(x))
-            k(x) = kd(x) + λQP(a(x), norm(cbf.Lgh(x))^2) * cbf.Lgh(x)'
-
-            return ExplicitSafetyFilter(k)
-        else
-            return e
-        end
-    else
-        a(x, t) = cbf.Lfh(x) + cbf.Lgh(x) * kd(x, t) + cbf.α(cbf(x))
-        k(x, t) = kd(x, t) + λQP(a(x, t), norm(cbf.Lgh(x))^2) * cbf.Lgh(x)'
-
-        return ExplicitSafetyFilter(k)
+    function k(x, args...)
+        Lgh = cbf.Lgh(x)
+        Lfh = cbf.Lfh(x)
+        kdx = kd(x, args...)
+        a = Lfh + Lgh * kdx + cbf.α(cbf(x))
+        kdx + λQP(a, norm(Lgh)^2) * Lgh'
     end
+    ExplicitSafetyFilter{typeof(k)}(k)
 end
 
 """
diff --git a/src/qp_safety_filter.jl b/src/qp_safety_filter.jl
index 75fc41f..f091f43 100644
--- a/src/qp_safety_filter.jl
+++ b/src/qp_safety_filter.jl
@@ -21,12 +21,12 @@ Functors for evaluating QP-based safety filter
 (k::QPSafetyFilter)(x, t) = k.k(x, t)
 
 """
-    QPSafetyFilter(cbfs::Vector{ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function)
+    QPSafetyFilter(cbfs::Vector{<:ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function)
 
 Construct an QPSafetyFilter from a cbf and a desired controller.
 """
 function QPSafetyFilter(
-    cbfs::Vector{ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function
+    cbfs::Vector{<:ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function
 )
     try
         kd(Σ.n == 1 ? rand() : rand(Σ.n), 0.0) # See if desired controller is time-varying
@@ -43,13 +43,13 @@ end
 
 """
     QPSafetyFilter(
-    cbfs::Vector{ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function, umin, umax
+    cbfs::Vector{<:ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function, umin, umax
 )
 
 Construct an QPSafetyFilter from a cbf and a desired controller.
 """
 function QPSafetyFilter(
-    cbfs::Vector{ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function, umin, umax
+    cbfs::Vector{<:ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function, umin, umax
 )
     try
         kd(Σ.n == 1 ? rand() : rand(Σ.n), 0.0) # See if desired controller is time-varying
@@ -80,12 +80,12 @@ function QPSafetyFilter(
 end
 
 """
-    solve_cbf_qp(x, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function)
+    solve_cbf_qp(x, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function)
 
 Solve CBF-QP using OSQP and JuMP.
 """
 function solve_cbf_qp(
-    x, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function
+    x, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function
 )
     model = Model(OSQP.Optimizer)
     set_silent(model)
@@ -101,7 +101,7 @@ end
 
 """
     solve_cbf_qp(
-        x, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function, umin, umax
+        x, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function, umin, umax
     )
 
 Solve CBF-QP using OSQP and JuMP while satisfying input bounds `umin ≤ u ≤ umax`.
@@ -109,7 +109,7 @@ Solve CBF-QP using OSQP and JuMP while satisfying input bounds `umin ≤ u ≤ u
 function solve_cbf_qp(
     x,
     Σ::ControlAffineSystem,
-    cbfs::Vector{ControlBarrierFunction},
+    cbfs::Vector{<:ControlBarrierFunction},
     kd::Function,
     umin,
     umax,
@@ -129,12 +129,12 @@ function solve_cbf_qp(
 end
 
 """
-    solve_time_varying_cbf_qp(x, t, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function)
+    solve_time_varying_cbf_qp(x, t, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function)
 
 Solve CBF-QP where desired controller is time-varying
 """
 function solve_time_varying_cbf_qp(
-    x, t, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function
+    x, t, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function
 )
     model = Model(OSQP.Optimizer)
     set_silent(model)
@@ -149,7 +149,7 @@ function solve_time_varying_cbf_qp(
 end
 
 """
-    solve_time_varying_cbf_qp(x, t, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function, umin, umax)
+    solve_time_varying_cbf_qp(x, t, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function, umin, umax)
 
 Solve CBF-QP where desired controller is time-varying with input bounds.
 """
@@ -157,7 +157,7 @@ function solve_time_varying_cbf_qp(
     x,
     t,
     Σ::ControlAffineSystem,
-    cbfs::Vector{ControlBarrierFunction},
+    cbfs::Vector{<:ControlBarrierFunction},
     kd::Function,
     umin,
     umax,
diff --git a/src/tunable_qp_safety_filter.jl b/src/tunable_qp_safety_filter.jl
index 6b125f7..b3e058c 100644
--- a/src/tunable_qp_safety_filter.jl
+++ b/src/tunable_qp_safety_filter.jl
@@ -21,7 +21,7 @@ Functors for evaluating QP-based safety filter
 (k::TunableQPSafetyFilter)(x, t) = k.k(x, t)
 
 """
-    TunableQPSafetyFilter(cbfs::Vector{ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function; tunable=false)
+    TunableQPSafetyFilter(cbfs::Vector{<:ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function; tunable=false)
 
 Construct an TunableQPSafetyFilter from a cbf and a desired controller.
 
@@ -29,7 +29,7 @@ Construct an TunableQPSafetyFilter from a cbf and a desired controller.
 - `tunable::Bool` : boolean to decide if coefficients on extended class K functions should be decision variables
 """
 function TunableQPSafetyFilter(
-    cbfs::Vector{ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function
+    cbfs::Vector{<:ControlBarrierFunction}, Σ::ControlAffineSystem, kd::Function
 )
     try
         kd(Σ.n == 1 ? rand() : rand(Σ.n), 0.0) # See if desired controller is time-varying
@@ -55,12 +55,12 @@ TunableQPSafetyFilter(cbf::ControlBarrierFunction, Σ::ControlAffineSystem, kd::
     TunableQPSafetyFilter([cbf], Σ, kd)
 
 """
-    solve_tunable_cbf_qp(x, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function)
+    solve_tunable_cbf_qp(x, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function)
 
 Solve CBF-QP where coefficients on extended class K functions are decision variables
 """
 function solve_tunable_cbf_qp(
-    x, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function
+    x, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function
 )
     model = Model(OSQP.Optimizer)
     set_silent(model)
@@ -78,12 +78,12 @@ function solve_tunable_cbf_qp(
 end
 
 """
-    solve_time_varying_tunable_cbf_qp(x, t, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function)
+    solve_time_varying_tunable_cbf_qp(x, t, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function)
 
 Solve CBF-QP where coefficients on extended class K functions are decision variables and desired controller depends on time
 """
 function solve_time_varying_tunable_cbf_qp(
-    x, t, Σ::ControlAffineSystem, cbfs::Vector{ControlBarrierFunction}, kd::Function
+    x, t, Σ::ControlAffineSystem, cbfs::Vector{<:ControlBarrierFunction}, kd::Function
 )
     model = Model(OSQP.Optimizer)
     set_silent(model)