Safety Filters and Control Barrier Functions

Safety filters are control techniques that modify nominal control inputs to ensure system safety while minimizing deviation from desired behavior. They are based on Control Barrier Functions (CBF) and provide formal safety guarantees through quadratic programming (QP) optimization.

Theory

Control Barrier Functions

Consider a nonlinear control-affine system:

\[\dot{x} = f(x) + g(x)u\]

where $x \in \mathbb{R}^{n_x}$ is the state and $u \in \mathbb{R}^{n_u}$ is the control input.

A Control Barrier Function (CBF) $h(x)$ defines a safe set $\mathcal{C} = \{x : h(x) \geq 0\}$. The goal is to ensure the system remains in this safe set for all time. For systems with relative degree 1 (i.e., the input appears directly in the derivative of $h$), the safety condition is:

\[\dot{h}(x) = L_f h(x) + L_g h(x) u \geq -\alpha(h(x))\]

where $L_f h$ and $L_g h$ are Lie derivatives, and $\alpha$ is a class-$\mathcal{K}$ function (typically $\alpha(h) = h$ for simplicity).

Safety Filter Design

The safety filter solves the following quadratic program at each time step:

\[\begin{aligned} u^* = \arg\min_u \quad & \|u - u_{\text{nom}}\|^2 \\ \text{subject to} \quad & L_f h(x) + L_g h(x) u \geq -\alpha(h(x)) \\ & u_{\min} \leq u \leq u_{\max} \end{aligned}\]

This formulation finds the control input closest to the nominal control $u_{\text{nom}}$ while ensuring safety constraints are satisfied.

Exponential Control Barrier Functions

For systems with relative degree $\nu > 1$, standard CBFs cannot be directly applied. Exponential Control Barrier Functions (ECBF) extend CBFs to handle higher relative degrees by introducing a coordinate transformation:

\[z = T(x) = \begin{bmatrix} h(x) \\ L_f h(x) \\ \vdots \\ L_f^{\nu-1} h(x) \end{bmatrix}\]

The ECBF constraint becomes:

\[L_f^\nu h(x) + L_g L_f^{\nu-1} h(x) u \geq -K_\alpha z\]

where $K_\alpha$ is a gain vector chosen to ensure that $h$ decreases no faster than an exponentially decaying system with specified poles. This can be designed using pole placement techniques.

For more details on the theoretical foundations, see Ames et al., "Control Barrier Functions: Theory and Applications".

Connection between exponential CBFs and feedback linearization

The condition relating the decay of the barrier function to the pole placement of a linear system is closely related to feedback linearization. We can view this condition as making sure that the barrier function $h$ does not decay faster than the corresponding feedback linearizing controller can take over to keep it positive. If we place the poles of this linearly decaying system too far to the left, we allow ourselves to get closer to the barrier boundary before the safety filter intervenes, but when it does intervene, it will do so more aggressively. Note, the safety filter cannot account for potential future violations of actuator constraints, so an aggressive tuning may yield an infeasible QP if the nominal controller is heading towards the barrier boundary too fast. If actuator constraints become problematic, consider using, e.g., an MPC controller that natively considers future constraints, or design a barrier function that encodes a recovery procedure and use a safety filter to ensure that the system can always execute this recovery procedure. While such a recovery barrier function involves online future prediction much like MPC, it is typically less computationally costly since it does not perform any optimization over the future trajectory.

Implementation in DyadControlSystems

DyadControlSystems provides the following functions for safety filter design:

safetyfilter: Creates a basic safety filter for systems with relative degree 1
exponential_safetyfilter: Creates an exponential safety filter for systems with arbitrary relative degree
simulate: Simulates a system with safety filter and nominal controller
SafetyFilterSolution: Solution structure with automatic plotting support

Both filter types return callable structs that can be used as u_safe = filter(x, u_nom, p, t).

Examples

Example 1: Obstacle Avoidance

This example demonstrates a basic safety filter for obstacle avoidance in 2D space. The system is a double integrator where we directly control velocity, and we want to avoid a circular obstacle while reaching the origin.

using DyadControlSystems
using Symbolics
using LinearAlgebra
using Plots

# Define symbolic variables
@variables x1 x2 u[1:2]
x = [x1; x2]
u = collect(u)

# Simple dynamics: ẋ = u (direct velocity control)
f = [0; 0]
g = diagm([1, 1])

# Obstacle parameters
xo = [-1.0, 1.0]  # Obstacle center
ro = 0.4          # Obstacle radius

# Barrier function: h(x) = ||x - xo||² - ro² ≥ 0 keeps us outside the circle
h = norm(x - xo)^2 - ro^2

# Create safety filter
umin = -1
umax = 1
filter = safetyfilter(f, g, h, x; umin, umax)

# Nominal controller: simple proportional control to origin
u_nom_func(x, p, t) = -x

# Simulate
x0 = [-2.1, 2.0]  # Start position
sol = simulate(filter, x0, u_nom_func, (0.0, 15.0))

# Plot results
p1 = plot(sol.x[1,:], sol.x[2,:],
    label="Trajectory", lw=2,
    title="Obstacle Avoidance with Safety Filter",
    xlabel="x₁", ylabel="x₂", aspect_ratio=:equal)
scatter!([xo[1]], [xo[2]], label="Obstacle center", ms=8, color=:red)
θ = range(0, 2π, length=100)
plot!(xo[1] .+ ro*cos.(θ), xo[2] .+ ro*sin.(θ),
    label="Obstacle boundary", color=:red, lw=2)
scatter!([x0[1]], [x0[2]], label="Start", ms=6, color=:green)
scatter!([0], [0], label="Goal", ms=6, color=:blue)

p2 = plot(sol, ploth=true, plotu=true, plotx=false, plotactive=true, layout=4)
plot(p1, p2, layout=(1,2), size=(1200, 400))

The trajectory successfully avoids the obstacle while reaching the origin. The safety filter intervenes when necessary to maintain the barrier constraint $h(x) \geq 0$.

Example 2: Exponential Safety Filter with Feedback Linearization

This example combines an exponential safety filter with feedback linearization for a third-order nonlinear system. The system has relative degree 3, requiring the exponential CBF formulation.

using DyadControlSystems
using Symbolics
using Plots

@variables x[1:3] u
x = collect(x)

# Nonlinear system with relative degree 3
f = [
    x[2]
    sin(x[3])
    0
]
g = [0; 0; 1]
h_output = x[1]  # Output for feedback linearization

# First, design a feedback linearizing controller
fl_result = feedbacklin(f, g, h_output, x)
fl_sys = Symbolics.build_function(fl_result; simplify=false)

# Reference tracking controller in linearized coordinates
v_func = function (x, p, t)
    y, yd, ydd = fl_sys.T(x, p, t)  # Transform to linear coordinates
    ref = 1.0  # Step reference
    e = ref - y
    ed = 0 - yd
    edd = 0 - ydd
    5*(e + 1*ed + 1*edd)  # PDD² controller
end

# Safety constraint: keep x[1] < 1.1
h_safety = 1.1 - x[1]

filter = exponential_safetyfilter(f, g, h_safety, x;
    umin=-10, umax=10,
    desired_poles=[-1.0, -1.5, -2.0])

# Nominal controller function (transforms x -> v -> u)
u_nom_fun = function(x, p, t)
    v = v_func(x, p, t)
    fl_sys.u(x, v, p, t)
end

# Simulate with safety filter
x0 = [0.5, 0.0, π/4]
sol_safe = DyadControlSystems.simulate(filter, x0, u_nom_fun, (0, 15))

# Also simulate without safety filter for comparison
sol_unsafe = DyadControlSystems.simulate(fl_sys, x0, v_func, (0, 15))

# Plot comparison
plot(sol_unsafe.t, sol_unsafe.x[1,:], label="Without safety filter", lw=2)
plot!(sol_safe.t, sol_safe.x[1,:], label="With safety filter", lw=2)
hline!([1.0], label="Reference", ls=:dash, color=:green)
hline!([1.1], label="Safety limit", ls=:dash, color=:red)
xlabel!("Time (s)")
ylabel!("x₁")
title!("Exponential Safety Filter with Feedback Linearization")

The exponential safety filter successfully enforces the safety constraint $x_1 < 1.1$ while allowing the system to track the reference as closely as possible.

Example 3: Safety Filter with ModelingToolkit

Safety filters can also be applied to systems defined using ModelingToolkit, enabling physical modeling with safety guarantees. This example models and simulates a simple pendulum with a safety constraint on the angle.

using DyadControlSystems, ModelingToolkit
using ModelingToolkit: t_nounits as t, D_nounits as D
using StaticArrays, Plots

# Define a simple pendulum system
@mtkmodel Pendulum begin
    @parameters begin
        m = 1.0
        l = 1.0
        g = 9.81
        b = 0.1  # Damping
    end
    @variables begin
        θ(t) = -π/6     # Angle
        ω(t) = 0.0     # Angular velocity
        τ(t)           # Control torque
        h1(t)           # Barrier function
        h2(t)           # Barrier function
    end
    @equations begin
        D(θ) ~ ω
        D(ω) ~ (-g/l*sin(θ) - b*ω + τ)/(m*l^2)
        h1 ~ deg2rad(5) - θ  # Safety: keep angle smaller than +5 degrees
        h2 ~ deg2rad(5) + θ  # Safety: keep angle larger than -5 degrees
    end
end

@named pend = Pendulum()
model = complete(pend)

# Create safety filter directly from ModelingToolkit model
inputs = [model.τ]
desired_poles = [[-5, -6], [-5, -6]]  # 2 poles for relative degree 2, two barrier functions
filter_mtk, x_mtk, p_mtk, sys = exponential_safetyfilter(model, inputs, [model.h1, model.h2]; umin=-5, umax=5, desired_poles, split=false)

θiωi = [findfirst(isequal(var), x_mtk) for var in (model.θ, model.ω)] # Indexing helper

# Nominal controller: simple PD control
function u_nom(x, p, t)
    θ, ω  = x[θiωi]
    SA[10*(0 - θ) - 2*ω]  # PD control to origin
end

# Simulate with safety filter
x0 = ModelingToolkit.get_u0(sys, inputs .=> 0)
p0 = ModelingToolkit.get_p(sys, inputs .=> 0)
sol = DyadControlSystems.simulate(filter_mtk, x0, u_nom, (0.0, 5.0); p=p0)
sol_unsafe = DyadControlSystems.simulate(filter_mtk, x0, u_nom, (0.0, 5.0); Ts=0.01, p=p0, safety=false)

# Plot results
plot(sol, lw=2, layout=5, margin=2Plots.mm, titlefontsize=10, size=(800,600))
plot!(sol_unsafe.t, [sol_unsafe.h; sol_unsafe.x; sol_unsafe.u]', l=(:dash, :orange), label="Without safety filter", sp=(1:5)')
title!("Angle (rad)", sp=θiωi[1]+2)
title!("Angular velocity (rad/s)", sp=θiωi[2]+2)
hline!([deg2rad(5)], label="Safety limit", ls=:dash, color=:red, sp=θiωi[1]+2)

Note, the second barrier function, $h_2$, is negative at the initial condition. However, once it enters the safe set, it remains there.

Lyapunov-Based Optimal Control with Safety Constraints

The lyapunov_opt function provides a unified framework for control design that combines:

Quadratic cost minimization: Tracks a reference control while minimizing control effort
Lyapunov stability guarantees: Ensures the closed-loop system is stable
Optional safety constraints: Can incorporate exponential control barrier functions

The controller solves the following optimization problem at each time step:

\[\begin{aligned} \min_{u, \delta} \quad & \|u - u_{\text{ref}}\|^2_H + p_2 \delta^2 + p_1\delta \\ \text{subject to} \quad & L_f V + L_g V \cdot u \leq -\gamma(V(x)) + \delta \\ & L_g L_f^{\nu-1} h \cdot u \geq -L_f^\nu h - K_\alpha z \quad \text{(if safety enabled)} \\ & u_{\min} \leq u \leq u_{\max} \\ & \delta \geq 0 \end{aligned}\]

where:

$V(x)$ is a Lyapunov function (e.g., system energy)
$\gamma$ is a class-$\mathcal{K}$ function controlling the convergence rate
$\delta$ is a slack variable allowing soft Lyapunov constraints
$p_2$ is the quadratic penalty weight for the slack variable
$p_1$ is the linear penalty weight for the slack variable
$h(x)$ are optional barrier functions for safety
$H$ is the weight matrix for control cost
$u_{\text{ref}}$ is an optional nominal control input. This may be used to cause the controller to act like a safety filter, or left out to yield a pure optimization-based Lyapunov controller

Example 4: Pendulum with Lyapunov Control and Safety

This example demonstrates how to combine Lyapunov-based stabilization with safety constraints for a pendulum system. This example builds upon the previous pendulum example, adding a Lyapunov function based on total mechanical energy and comparing performance with and without safety constraints.

using ModelingToolkit
using ModelingToolkit: t_nounits as t, D_nounits as D
using LinearAlgebra

# Define pendulum with Lyapunov function
@mtkmodel PendulumWithLyapunov begin
    @parameters begin
        m = 1.0
        l = 1.0
        g = 9.81
        b = 0.1  # Damping
    end
    @variables begin
        θ(t) = π/6     # Initial angle
        ω(t) = 0.0      # Initial angular velocity
        τ(t)            # Control torque
        V(t)            # Lyapunov function (total energy)
        h1(t)           # Barrier function 1
        h2(t)           # Barrier function 2
    end
    @equations begin
        D(θ) ~ ω
        D(ω) ~ (-g/l*sin(θ) - b*ω + τ)/(m*l^2)
        # V ~ θ^2 + ω^2
        V ~ (1/2)*m*l^2*ω^2 + m*g*l*(1 - cos(θ))  # Total mechanical energy
        h1 ~ deg2rad(20) - θ  # Keep angle < 20 degrees
        h2 ~ deg2rad(8) + θ  # Keep angle > -8 degrees
    end
end

@named pend = PendulumWithLyapunov()
model = complete(pend)

# Create Lyapunov controller WITHOUT safety constraints
H = [1.0;;]  # Weight for control cost
gamma = x -> 5x  # Fast convergence rate
umin = -3
umax = 3
p2_delta = 1  # High penalty on slack variable to enforce Lyapunov constraint
p1_delta = 1
inputs = [model.τ]
soft_safety = true
controller_nosafety, x_ns, p_ns, sys_ns = DyadControlSystems.lyapunov_opt(
    model, inputs, model.V;
    p2_delta, p1_delta, gamma, soft_safety,
    H, umin, umax, split=false)

# Create Lyapunov controller WITH safety constraints
controller_safety, x_s, p_s, sys_s = DyadControlSystems.lyapunov_opt(
    model, inputs, model.V;
    p2_delta, p1_delta, gamma, soft_safety,
    H, umin, umax,
    h=[model.h1, model.h2],  # Add safety constraints
    desired_poles=[[-5, -6], [-5, -6]], split=false)  # Fast safety response

# Get initial conditions and parameters
x0 = ModelingToolkit.get_u0(sys_s, inputs .=> 0)
p0 = ModelingToolkit.get_p(sys_s, inputs .=> 0)

# Reference control: energy-optimal (zero reference minimizes energy)
function u_nom(x, p, t)
    θ, ω  = x[θiωi]
    0*SA[10*(0 - θ) - 2*ω]  # PD control to origin, multiplied by 0 to yield zero nominal control
end

# Simulate both controllers
sol_nosafety = DyadControlSystems.simulate(controller_nosafety, x0, u_nom, (0, 8); p=p0)
sol_safety   = DyadControlSystems.simulate(controller_safety,   x0, u_nom, (0, 8); p=p0)

# Find indices for θ and ω
θi = findfirst(isequal(model.θ), x_s)
ωi = findfirst(isequal(model.ω), x_s)

# Plot comparison
p1 = plot(sol_nosafety.t, rad2deg.(sol_nosafety.x[θi, :]),
    label="Without safety", lw=2, color=:blue,
    xlabel="Time (s)", ylabel="Angle (degrees)",
    title="Pendulum Angle: Lyapunov Control")
plot!(sol_safety.t, rad2deg.(sol_safety.x[θi, :]),
    label="With safety", lw=2, color=:green)
hline!([20, -8], label="Safety limits", ls=:dash, color=:red, lw=1)

p2 = plot(sol_nosafety.t, sol_nosafety.V,
    label="Without safety", lw=2, color=:blue,
    xlabel="Time (s)", ylabel="Lyapunov function V",
    title="Energy Evolution")
plot!(sol_safety.t, sol_safety.V,
    label="With safety", lw=2, color=:green)

p3 = plot(sol_nosafety.t, sol_nosafety.u[1, :],
    label="Without safety", lw=2, color=:blue,
    xlabel="Time (s)", ylabel="Control torque τ",
    title="Control Input")
plot!(sol_safety.t, sol_safety.u[1, :],
    label="With safety", lw=2, color=:green)
hline!([3, -3], label="Control limits", ls=:dot, color=:gray, lw=1)

# Plot barrier functions for safety controller
p4 = plot(sol_safety.t, sol_safety.h[1, :],
    label="h₁ (upper bound)", lw=2, color=:orange,
    xlabel="Time (s)", ylabel="Barrier function",
    title="Safety Constraints")
plot!(sol_safety.t, sol_safety.h[2, :],
    label="h₂ (lower bound)", lw=2, color=:purple)
hline!([0], label="Safety boundary", ls=:dash, color=:red, lw=1)
p5 = plot(sol_nosafety.t, sol_nosafety.delta,
    label="Slack variable δ", lw=2, color=:blue,
    xlabel="Time (s)", ylabel="Slack variable",
    title="Lyapunov Slack Variable")
plot!(p5, sol_safety.t, sol_safety.delta,
    label="Without safety", lw=2,color=:green)
p6 = plot(sol_nosafety.t, sol_nosafety.exitflags,
    label="Exit flag", lw=2, color=:blue,
    xlabel="Time (s)", ylabel="Exit flag",
    title="QP Exit Flag")
plot!(p6, sol_safety.t, sol_safety.exitflags,
    label="With safety", lw=2,color=:green)
plot(p1, p2, p3, p4, p5, p6, size=(1000, 700), xlabel="")

The plots show:

Angle trajectory: The safety-constrained controller respects the limits while the unconstrained controller violates them
Lyapunov function: Both controllers decrease the system energy
Control input: Safety constraints modify the control when approaching boundaries
QP exit flag: Indicates whether an optimal solution was found (1) or if constraint softening was needed (2)

Practical Considerations

When to Use Safety Filters

Safety filters are most effective when:

Hard constraints must be satisfied (collision avoidance, actuator limits)
A nominal controller exists but lacks safety guarantees
Real-time computation is feasible (QP can be solved quickly)
System model is reasonably accurate near constraints

Limitations

Requires accurate model near safety boundaries
QP must be feasible (conflicting constraints due to actuator limitations can cause issues). The safety filter cannot predict future infeasibility due to actuator limitations.
Does not handle stochastic uncertainties directly

Integration with Other Controllers

Safety filters work well with:

Feedback linearization: Apply safety filter to the computed $u$
MPC: Use as a backup safety layer, e.g., when using linear MPC but needing nonlinear safety guarantees

Index

DyadControlSystems.ExponentialSafetyFilter
DyadControlSystems.LyapunovOptimalController
DyadControlSystems.LyapunovOptimalControllerSolution
DyadControlSystems.SafetyFilter
DyadControlSystems.exponential_safetyfilter
DyadControlSystems.get_dynamics
DyadControlSystems.lyapunov_opt
DyadControlSystems.safetyfilter
DyadControlSystems.simulate
DyadControlSystems.simulate

DyadControlSystems.safetyfilter — Function

safetyfilter(f, g, h, x; umax, umin, a=identity, p=[])

Create a SafetyFilter that computes safe control inputs u_opt by solving a quadratic program (QP) that minimizes the deviation from a nominal control input u_nom while ensuring safety constraints defined by a control barrier function h(x).

This function requires $h$ to have relative degree 1 with respect to the system dynamics defined by f and g, that is, the time derivative of $h$ must depend explicitly on the control input u. For barrier functions with higher relative degree, consider using exponential_safetyfilter.

Arguments:

f: Symbolic drift dynamics f(x)
g: Symbolic input dynamics g(x)
h: Symbolic barrier function h(x)
x: Symbolic state variables
umax: Upper bounds on control inputs
umin: Lower bounds on control inputs
a: Barrier function scaling (default: identity). Must be a class K function, i.e., strictly increasing and a(0) = 0.
p: Symbolic parameters (default: [])
soft_safety: Whether to use soft constraints for safety (default: true) Use these arguments to prioritize safety vs. actuator constraints.
soft_input: Whether to use soft constraints for input bounds (default: false)

Returns:

SafetyFilter: A struct that can be called as filter(x, u_nom, p, t) to compute safe control

Example:

filter = safetyfilter(f, g, h, x; umax=10, umin=-10)
u_safe, fval, exitflag, info = filter(x_current, u_nominal, [], 0.0)