Discrete-time Controlled Invariant Set¶

An similar example is available in a codeocean capsule.

Introduction¶

This example considers the linear control system already introduced in the Example 2 of [LTJ20]; see also Example 2 of [LRJ20]:

$\begin{aligned} x_{k+1} & = x_k + u_k / 2\\ u_{k+1} & = u_k' \end{aligned}$ with

$(x_k, u_k) \in [-1, 1]^2$ .

The system is $x_{k+1} = Ax_k + Bu_k$ where $B = (0, 1)$ and

$A = \begin{bmatrix} 1 & 1/2\\ 0 & 0 \end{bmatrix}.$ As shown in Example 4 of [LTJ18], a set

$S \subseteq [-1, 1]^2$ is controlled invariant for this system if

$\begin{bmatrix} 1 & 1/2 \end{bmatrix} S \subseteq \begin{bmatrix} 1 & 0 \end{bmatrix} S$

[LRJ20] Legat, Benoît, Saša V. Raković, and Raphaël M. Jungers. Piecewise semi-ellipsoidal control invariant sets. IEEE Control Systems Letters 5.3 (2020): 755-760.

[LTJ18] B. Legat, P. Tabuada and R. M. Jungers. Computing controlled invariant sets for hybrid systems with applications to model-predictive control. 6th IFAC Conference on Analysis and Design of Hybrid Systems ADHS (2018).

We need to pick an LP and an SDP solver, see here for a list of available ones.

In [1]:

using SetProg
import GLPK
lp_solver = optimizer_with_attributes(GLPK.Optimizer, MOI.Silent() => true)
import CSDP
sdp_solver = optimizer_with_attributes(CSDP.Optimizer, MOI.Silent() => true)
A = [1.0 0.5]
E = [1.0 0.0]
using Polyhedra
lib = DefaultLibrary{Float64}(lp_solver)
h = HalfSpace([1, 0], 1.0) ∩ HalfSpace([-1, 0], 1) ∩ HalfSpace([0, 1], 1) ∩ HalfSpace([0, -1], 1)
□ = polyhedron(h, lib) # [0, 1]^2
v = convexhull([1.0, 0], [0, 1], [-1, 0], [0, -1])
◇ = polyhedron(v, lib); # polar of [0, 1]^2

Polyhedral template¶

Fixed point approach¶

This section shows that the maximal control invariant set of this simple control system is polyhedral. Moreover, this polyhedron is obtained after only one fixed point iteration of the standard viability kernel algorithm. We implement the fixed point iteration with the following function:

In [2]:

function fixed_point_iteration(set::Polyhedron)
    new_set = set ∩ (A \ (E * set))
    removehredundancy!(new_set)
    return new_set
end

Out[2]:

fixed_point_iteration (generic function with 1 method)

We start with $[-1, 1]^2$ and obtain the polytope mci after one iteration.

In [3]:

mci = fixed_point_iteration(□)

Out[3]:

Polyhedron DefaultPolyhedron{Float64, Polyhedra.Intersection{Float64, Vector{Float64}, Int64}, Polyhedra.Hull{Float64, Vector{Float64}, Int64}}:
6-element iterator of HalfSpace{Float64, Vector{Float64}}:
 HalfSpace([1.0, 0.0], 1.0)
 HalfSpace([-1.0, 0.0], 1.0)
 HalfSpace([0.0, 1.0], 1.0)
 HalfSpace([0.0, -1.0], 1.0)
 HalfSpace([1.0, 0.5], 1.0)
 HalfSpace([-0.5, -0.25], 0.5)

One additional iteration gives the same polytope, showing that this polytope is control invariant.

In [4]:

fixed_point_iteration(mci)

Out[4]:

Polyhedron DefaultPolyhedron{Float64, Polyhedra.Intersection{Float64, Vector{Float64}, Int64}, Polyhedra.Hull{Float64, Vector{Float64}, Int64}}:
6-element iterator of HalfSpace{Float64, Vector{Float64}}:
 HalfSpace([1.0, 0.0], 1.0)
 HalfSpace([-1.0, 0.0], 1.0)
 HalfSpace([0.0, 1.0], 1.0)
 HalfSpace([0.0, -1.0], 1.0)
 HalfSpace([-1.0, -0.5], 1.0)
 HalfSpace([0.5, 0.25], 0.5)

We plot this polytope mci in yellow below along with the safe set $[-1, 1]^2$ in green.

In [5]:

using Plots
function hexcolor(rgb::UInt32)
    r = ((0xff0000 & rgb) >> 16) / 255
    g = ((0x00ff00 & rgb) >>  8) / 255
    b = ((0x0000ff & rgb)      ) / 255
    Plots.RGBA(r, g, b)
end # Values taken from http://www.toutes-les-couleurs.com/code-couleur-rvb.php
lichen = hexcolor(0x85c17e)
canard = hexcolor(0x048b9a)
aurore = hexcolor(0xffcb60)

plot(ratio=:equal, tickfont=Plots.font(12))
plot!(□, color=lichen)
plot!(mci, color=aurore)

Out[5]:

We also plot their respective polars. Note that the inclusion is reversed in the polar space.

In [6]:

polar_mci = polar(mci)
plot(ratio=:equal, tickfont=Plots.font(12))
plot!(polar_mci, color=aurore)
plot!(◇, color=lichen)

function maximal_invariant(template, heuristic::Function)
    solver = if template isa Polytope
        lp_solver
    else
        sdp_solver
    end
    model = Model(solver)
    @variable(model, S, template)
    @constraint(model, S ⊆ □)
    @constraint(model, A * S ⊆ E * S)
    @objective(model, Max, heuristic(volume(S)))
    optimize!(model)
    @show solve_time(model)
    @show termination_status(model)
    @show objective_value(model)
    return value(S)
end
function primal_plot(set; npoints=256, args...)
    plot(ratio=:equal, tickfont=Plots.font(12); args...)
    plot!(□, color=lichen)
    plot!(mci, color=aurore)
    plot!(set, color=canard, npoints=npoints)
end
function polar_plot(set; npoints=256, args...)
    plot(ratio=:equal, tickfont=Plots.font(12); args...)
    plot!(SetProg.Sets.polar(set), color=canard, npoints=npoints)
    plot!(polar_mci, color=aurore)
    plot!(◇, color=lichen)
end

Out[6]:

polar_plot (generic function with 1 method)

Linear programming approach¶

As introduced in [R21], given a fixed polyhedral conic partition of the state-space, we can search over all polyhedra for which the gauge or support function is linear over each piece of the partition. The partition can be defined by a polytope as described in [Eq. (4.6), R21]. In this example, because the sets are represented with the support function, the pieces will be used for the support function/polar set and the pieces of the gauge function/primal will depend on the value of the support function on each piece.

[R21] Raković, S. V. Control Minkowski–Lyapunov functions Automatica, Elsevier BV, 2021, 128, 109598

Let's start with setting the partition using the cross polytope ◇.

In [7]:

sol_polytope_◇ = maximal_invariant(Polytope(symmetric=true, piecewise=◇), L1_heuristic)
SetProg.Sets.print_support_function(sol_polytope_◇)

solve_time(model) = 5.4836273193359375e-5
termination_status(model) = MathOptInterface.OPTIMAL
objective_value(model) = 1.8856180831641272
h(S, x) =
          x[1]
              if x[2] ≥ 0, x[1] ≥ 0
          x[1]
              if -x[2] ≥ 0, x[1] ≥ 0
          -x[1]
              if -x[1] ≥ 0, x[2] ≥ 0
          -x[1]
              if -x[1] ≥ 0, -x[2] ≥ 0

We can see that the polar set is unbounded so the primal set has zero volume. More precisely, the polar is $[-1, 1] \times [-\infty, \infty]$ and the primal set is $[-1, 1] \times \{0\}$ .

Let's now try setting the partition using the square □.

In [8]:

sol_polytope_□ = maximal_invariant(Polytope(symmetric=true, piecewise=□), L1_heuristic)
SetProg.Sets.print_support_function(sol_polytope_□)

solve_time(model) = 4.601478576660156e-5
termination_status(model) = MathOptInterface.OPTIMAL
objective_value(model) = 2.6666666666666665
h(S, x) =
          x[1]
              if -x[2] + x[1] ≥ 0, x[2] + x[1] ≥ 0
          -x[1]
              if -x[2] - x[1] ≥ 0, x[2] - x[1] ≥ 0
          x[2]
              if x[2] - x[1] ≥ 0, x[2] + x[1] ≥ 0
          -x[2]
              if -x[2] - x[1] ≥ 0, -x[2] + x[1] ≥ 0

The primal plot is below:

In [9]:

primal_plot(sol_polytope_□, xlim=(-1.05, 1.05), ylim=(-1.05, 1.05))

Out[9]:

And the polar plot is below:

In [10]:

polar_plot(sol_polytope_□, xlim=(-1.05, 1.05), ylim=(-1.05, 1.05))

Out[10]:

Let's now use a partition with 8 pieces.

In [11]:

pieces8 = convexhull(□, 1.25 * ◇)
sol_polytope_8 = maximal_invariant(Polytope(symmetric=true, piecewise=pieces8), L1_heuristic)
SetProg.Sets.print_support_function(sol_polytope_8)

solve_time(model) = 0.0001270771026611328
termination_status(model) = MathOptInterface.OPTIMAL
objective_value(model) = 2.4636787533634337
h(S, x) =
          x[1]
              if 0.4*x[2] ≥ 0, -0.6*x[2] + 0.6*x[1] ≥ 0
          -x[2] + x[1]
              if -0.4*x[2] ≥ 0, 0.6*x[2] + 0.6*x[1] ≥ 0
          x[2]
              if 0.6*x[2] - 0.6*x[1] ≥ 0, 0.4*x[1] ≥ 0
          x[2] - x[1]
              if -0.4*x[1] ≥ 0, 0.6*x[2] + 0.6*x[1] ≥ 0
          x[2] - x[1]
              if -0.6*x[2] - 0.6*x[1] ≥ 0, x[2] ≥ 0
          -x[1]
              if -x[2] ≥ 0, 0.6*x[2] - 0.6*x[1] ≥ 0
          -x[2] + x[1]
              if -0.6*x[2] - 0.6*x[1] ≥ 0, 0.4*x[1] ≥ 0
          -x[2]
              if -0.6*x[2] + 0.6*x[1] ≥ 0, -0.4*x[1] ≥ 0

The primal plot is below:

In [12]:

primal_plot(sol_polytope_8, xlim=(-1.05, 1.05), ylim=(-1.05, 1.05))

Out[12]:

And the polar plot is below:

In [13]:

polar_plot(sol_polytope_8, xlim=(-1.05, 1.05), ylim=(-1.05, 1.05))

Out[13]:

We can use the polar of the polytope resulting from the first fixed point iteration to generate a refined conic partition. With this partition, the maximal piecewise semi-ellipsoidal control invariant set matches the maximal control invariant set.

In [14]:

sol_polytope_mci = maximal_invariant(Polytope(symmetric=true, piecewise=polar_mci), L1_heuristic)
SetProg.Sets.print_support_function(sol_polytope_mci)

solve_time(model) = 7.700920104980469e-5
termination_status(model) = MathOptInterface.OPTIMAL
objective_value(model) = 3.1095432575413913
h(S, x) =
          x[1]
              if x[2] ≥ 0, -x[2] + 0.5*x[1] ≥ 0
          -x[2] + x[1]
              if -x[2] ≥ 0, x[1] ≥ 0
          x[2] + 0.5*x[1]
              if x[2] - 0.5*x[1] ≥ 0, x[1] ≥ 0
          x[2] - x[1]
              if -x[1] ≥ 0, x[2] ≥ 0
          -x[1]
              if -x[2] ≥ 0, x[2] - 0.5*x[1] ≥ 0
          -x[2] - 0.5*x[1]
              if -x[1] ≥ 0, -x[2] + 0.5*x[1] ≥ 0

The primal plot is below:

In [15]:

primal_plot(sol_polytope_mci)

Out[15]:

And the polar plot is below:

In [16]:

polar_plot(sol_polytope_mci)

Out[16]:

Ellipsoidal template¶

We now compute the maximal ellipsoidal control invariant set. Here we can either maximize its volume (which corresponds to $\log(\det(Q))$ or $\sqrt[n]{\det(Q)}$ in the objective function) or minimize the sum of the squares of the length of its semi-axes of the polar (which corresponds to the trace of $Q$ in the objective function).

We can see below that the control invariant set of maximal volume has support function

$h^2(S, x) = x_1^2 - \frac{1}{2} x_1x_2 + x_2^2.$

In [17]:

sol_ell_vol = maximal_invariant(Ellipsoid(symmetric=true, dimension=2), nth_root)
SetProg.Sets.print_support_function(sol_ell_vol)

solve_time(model) = 0.00457000732421875
termination_status(model) = MathOptInterface.OPTIMAL
objective_value(model) = 0.9682458357350057
h(S, x) = x[2]^2 - 0.5*x[1]*x[2] + x[1]^2

We can see below the ellipsoid in blue along with the maximal control invariant set in yellow and the safe set in green.

In [18]:

primal_plot(sol_ell_vol)

Out[18]:

Below is are the corresponding sets in the polar space.

In [19]:

polar_plot(sol_ell_vol)

Out[19]:

We can see below that the control invariant set of minimal sum of squares of the length of the semi-axes of the polar has support function

$h^2(S, x) = x_1^2 - \alpha x_1x_2 + x_2^2.$ with

$\alpha \approx 1.377456$ . Note that the sum of the squares of the length of the semi-axes of the polar is equal to the integral of

$h^2(S, x)$ over the hypercube

$[-1, 1]^2$ hence the use of the L1_heuristic(vol, ones(2)) as objective which means taking the integral over the hyperrectangle with vertex ones(2) = [1, 1].

In [20]:

sol_ell_L1 = maximal_invariant(Ellipsoid(symmetric=true, dimension=2), vol -> L1_heuristic(vol, ones(2)))
SetProg.Sets.print_support_function(sol_ell_L1)

solve_time(model) = 0.0017478466033935547
termination_status(model) = MathOptInterface.OPTIMAL
objective_value(model) = 0.666666665518142
h(S, x) = x[2]^2 - 1.204768*x[1]*x[2] + x[1]^2

The primal plot is below:

In [21]:

primal_plot(sol_ell_L1)

Out[21]:

And the polar plot is below:

In [22]:

polar_plot(sol_ell_L1)

Out[22]:

Piecewise semi-ellipsoidal template¶

We now study the maximal piecewise semi-ellipsoidal control invariant sets of a given conic partition. The volume is not directly maximized. Instead, for each cone, we compute the sum $s$ of the normalized rays and consider the polytope obtained by intersecting the cone with the halfspace $s^\top x \le \|s\|_2^2$ . We integrate the quadratic form corresponding to this cone, i.e. $h^2(S, x)$ over the polytope. The sum of the integrals over each polytope is the objective function we use. This can be seen as the generalization of the sum of the squares of the semi-axes of the polar of the ellipsoid.

Note that the constraint (29) of Program 1 of [LRJ20] is implemented with Proposition 2 of [LRJ20] for all results of this example.

The conic partition are obtained by considering the conic hull of each facets of a given polytope. We first consider the conic partition corresponding to the polar of the safe set $[-1, 1]^2$ . This gives the four quadrants as cones of the conic partition. The maximal piecewise semi-ellipsoidal control invariant set with this partition has the following support function:

$h^2(S, x) = \begin{cases} (x_1 - x_2)^2 & \text{ if }x_1x_2 \le 0,\\ x_1^2 - x_1x_2/2 + x_2^2 & \text{ if }x_1x_2 \ge 0. \end{cases}$ For the cones

$x_1x_2 \le 0$ , the semi-ellipsoid matches the safe set and the maximal control invariant set. For the cones

$x_1x_2 \ge 0$ , the semi-ellipsoid matches the maximal volume control invariant ellipsoid. This illustrates one key feature of piecewise semi-ellipsoidal sets, they can combines advantages of both polyhedra and ellipsoids. It can be polyhedral on the directions where the maximal control invariant set is polyhedral and be ellipsoidal on the directions where the maximal control invariant set is smooth or requires many halfspaces in its representation.

In [23]:

sol_piece_◇ = maximal_invariant(Ellipsoid(symmetric=true, piecewise=◇), L1_heuristic)
SetProg.Sets.print_support_function(sol_piece_◇)

solve_time(model) = 0.005764961242675781
termination_status(model) = MathOptInterface.OPTIMAL
objective_value(model) = 3.1666666550367832
h(S, x) =
          x[2]^2 - 0.5*x[1]*x[2] + x[1]^2
              if x[2] ≥ 0, x[1] ≥ 0
          x[2]^2 - 2.0*x[1]*x[2] + x[1]^2
              if -x[2] ≥ 0, x[1] ≥ 0
          x[2]^2 - 2.0*x[1]*x[2] + x[1]^2
              if -x[1] ≥ 0, x[2] ≥ 0
          x[2]^2 - 0.5*x[1]*x[2] + x[1]^2
              if -x[1] ≥ 0, -x[2] ≥ 0

The primal plot is below:

In [24]:

primal_plot(sol_piece_◇, xlim=(-1.05, 1.05), ylim=(-1.05, 1.05))

Out[24]:

And the polar plot is below:

In [25]:

polar_plot(sol_piece_◇, xlim=(-1.05, 1.05), ylim=(-1.05, 1.05))

Out[25]:

Let's now try setting the partition with the square □.

In [26]:

sol_piece_□ = maximal_invariant(Ellipsoid(symmetric=true, piecewise=□), L1_heuristic)
SetProg.Sets.print_support_function(sol_piece_□)

solve_time(model) = 0.0057239532470703125
termination_status(model) = MathOptInterface.OPTIMAL
objective_value(model) = 2.666666664667396
h(S, x) =
          x[2]^2 - 1.52044*x[1]*x[2] + x[1]^2
              if -x[2] + x[1] ≥ 0, x[2] + x[1] ≥ 0
          x[2]^2 - 1.52044*x[1]*x[2] + x[1]^2
              if -x[2] - x[1] ≥ 0, x[2] - x[1] ≥ 0
          x[2]^2 - 1.52044*x[1]*x[2] + x[1]^2
              if x[2] - x[1] ≥ 0, x[2] + x[1] ≥ 0
          x[2]^2 - 1.52044*x[1]*x[2] + x[1]^2
              if -x[2] - x[1] ≥ 0, -x[2] + x[1] ≥ 0

The primal plot is below:

In [27]:

primal_plot(sol_piece_□, xlim=(-1.05, 1.05), ylim=(-1.05, 1.05))

Out[27]:

And the polar plot is below:

In [28]:

polar_plot(sol_piece_□, xlim=(-1.6, 1.6), ylim=(-1.6, 1.6))

Out[28]:

Let's now try with the partition of 8 pieces.

In [29]:

sol_piece_8 = maximal_invariant(Ellipsoid(symmetric=true, piecewise=pieces8), L1_heuristic)
SetProg.Sets.print_support_function(sol_piece_8)

solve_time(model) = 0.011782169342041016
termination_status(model) = MathOptInterface.OPTIMAL
objective_value(model) = 2.2831717475906466
h(S, x) =
          1.849901*x[2]^2 - 0.92495*x[1]*x[2] + x[1]^2
              if 0.4*x[2] ≥ 0, -0.6*x[2] + 0.6*x[1] ≥ 0
          x[2]^2 - 2.0*x[1]*x[2] + x[1]^2
              if -0.4*x[2] ≥ 0, 0.6*x[2] + 0.6*x[1] ≥ 0
          x[2]^2 + 0.774852*x[1]*x[2] + 0.150099*x[1]^2
              if 0.6*x[2] - 0.6*x[1] ≥ 0, 0.4*x[1] ≥ 0
          x[2]^2 - 2.0*x[1]*x[2] + x[1]^2
              if -0.4*x[1] ≥ 0, 0.6*x[2] + 0.6*x[1] ≥ 0
          x[2]^2 - 2.0*x[1]*x[2] + x[1]^2
              if -0.6*x[2] - 0.6*x[1] ≥ 0, x[2] ≥ 0
          1.849901*x[2]^2 - 0.92495*x[1]*x[2] + x[1]^2
              if -x[2] ≥ 0, 0.6*x[2] - 0.6*x[1] ≥ 0
          x[2]^2 - 2.0*x[1]*x[2] + x[1]^2
              if -0.6*x[2] - 0.6*x[1] ≥ 0, 0.4*x[1] ≥ 0
          x[2]^2 + 0.774852*x[1]*x[2] + 0.150099*x[1]^2
              if -0.6*x[2] + 0.6*x[1] ≥ 0, -0.4*x[1] ≥ 0

The primal plot is below:

In [30]:

primal_plot(sol_piece_8, xlim=(-1.05, 1.05), ylim=(-1.05, 1.05))

Out[30]:

And the polar plot is below:

In [31]:

polar_plot(sol_piece_8, xlim=(-1.1, 1.1), ylim=(-1.05, 1.05))

Out[31]:

We can use the polar of the polytope resulting from the first fixed point iteration to generate a refined conic partition. With this partition, the maximal piecewise semi-ellipsoidal control invariant set matches the maximal control invariant set. The support function is

$h^2(S, x) = \begin{cases} (x_1 - x_2)^2 & \text{ if }x_1x_2 \le 0,\\ x_1^2 & \text{ if }x_2(x_1-2x_2) \ge 0,\\ (x_1/2 + x_2)^2 & \text{ if }x_1(2x_2-x_1) \ge 0.\\ \end{cases}$

In [32]:

sol_piece_mci = maximal_invariant(Ellipsoid(symmetric=true, piecewise=polar_mci), L1_heuristic)
SetProg.Sets.print_support_function(sol_piece_mci)

solve_time(model) = 0.009195089340209961
termination_status(model) = MathOptInterface.OPTIMAL
objective_value(model) = 2.990943448940364
h(S, x) =
          x[1]^2
              if x[2] ≥ 0, -x[2] + 0.5*x[1] ≥ 0
          x[2]^2 - 2.0*x[1]*x[2] + x[1]^2
              if -x[2] ≥ 0, x[1] ≥ 0
          x[2]^2 + x[1]*x[2] + 0.25*x[1]^2
              if x[2] - 0.5*x[1] ≥ 0, x[1] ≥ 0
          x[2]^2 - 2.0*x[1]*x[2] + x[1]^2
              if -x[1] ≥ 0, x[2] ≥ 0
          x[1]^2
              if -x[2] ≥ 0, x[2] - 0.5*x[1] ≥ 0
          x[2]^2 + x[1]*x[2] + 0.25*x[1]^2
              if -x[1] ≥ 0, -x[2] + 0.5*x[1] ≥ 0

The primal plot is below:

In [33]:

primal_plot(sol_piece_mci)

Out[33]:

And the polar plot is below:

In [34]:

polar_plot(sol_piece_mci)

Out[34]:

Polyset template¶

We now use the polyset templates. As details in [LRJ20], for inclusion constraints of the form A * S ⊆ E * S, the polar representation is used so we need the set to be convex. For this, we set the argument convex=true. We directly try degree=4 because degree=2 would simply give the same as sol_ell_L1.

In [35]:

sol4 = maximal_invariant(PolySet(symmetric=true, degree=4, convex=true), vol -> L1_heuristic(vol, ones(2)))
SetProg.Sets.print_support_function(sol4)

solve_time(model) = 0.0025229454040527344
termination_status(model) = MathOptInterface.OPTIMAL
objective_value(model) = 0.39999999806931175
h(S, x) = x[2]^4 - 2.992549*x[1]*x[2]^3 + 6.0*x[1]^2*x[2]^2 - 2.992579*x[1]^3*x[2] + x[1]^4

The primal plot is below:

In [36]:

primal_plot(sol4)

Out[36]:

And the polar plot is below:

In [37]:

polar_plot(sol4)

Out[37]:

Let's now try degree 6.

In [38]:

sol6 = maximal_invariant(PolySet(symmetric=true, degree=6, convex=true), vol -> L1_heuristic(vol, ones(2)))
SetProg.Sets.print_support_function(sol6)

solve_time(model) = 0.004295825958251953
termination_status(model) = MathOptInterface.OPTIMAL
objective_value(model) = 0.2857142833526061
h(S, x) = x[2]^6 - 5.206527*x[1]*x[2]^5 + 15.0*x[1]^2*x[2]^4 - 17.355089*x[1]^3*x[2]^3 + 15.0*x[1]^4*x[2]^2 - 5.206526*x[1]^5*x[2] + x[1]^6

The primal plot is below:

In [39]:

primal_plot(sol6)

Out[39]:

And the polar plot is below:

In [40]:

polar_plot(sol6)

Out[40]:

This notebook was generated using Literate.jl.