Figure 3.5:

Bounds on tightened constraint set \bar {\mathbb {Z}} for varying N. Bounds are \left | x_1 \right | \le \chi _1, \left | x_2 \right | \le \chi _2, and \left | u \right | \le \mu .

Code for Figure 3.5

Text of the GNU GPL.

tightenconstraints.py

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
# theta = tightenconstraints(Z, A, B, K, W, N)
#
# Tightens state and input constraints for a box W.
#
# Z should be a struct with fields G, H, and psi to define the feasible set Z as
#
#    Gx + Hu <= psi
#
# A, B, and K should be the system model and controller gain. W should be a
# vector defining the maximum absolute values for w. The system evolves as
#
#    x^+ = Ax + Bu + w,     u = v + Kx,     -W <= w <= W
#
# This means that W must be a symmetric box in R^n (we make this restriction
# because optimization becomes particularly easy).
#
# Returns theta such that the tighter constraints
#
#    Gx + H(v + Kx) <= psi - theta
#
# are satisfied for any N realizations of W.
import numpy as np

def tightenconstraints(Z, A, B, K, W, N):
    """
    Tightens state and input constraints for a box W.

    Z should be a struct with fields G, H, and psi to define the feasible set Z as

       Gx + Hu <= psi

    A, B, and K should be the system model and controller gain. W should be a
    vector defining the maximum absolute values for w. The system evolves as

       x^+ = Ax + Bu + w,     u = v + Kx,     -W <= w <= W

    This means that W must be a symmetric box in R^n (we make this restriction
    because optimization becomes particularly easy).

    Returns theta such that the tighter constraints

       Gx + H(v + Kx) <= psi - theta

    are satisfied for any N realizations of W.
    """

    if not isinstance(Z, dict) or not all(field in Z for field in ['G', 'H']):
        raise ValueError('Invalid input for Z!')

    C = Z['G'] + Z['H'] @ K
    A = A + B @ K
    theta = np.zeros(C.shape[0])
    Ak = np.eye(A.shape[0])
    W = np.abs(W).flatten()

    for i in range(N+1):
        theta = theta + np.abs(C @ Ak) @ W
        Ak = A @ Ak

    return theta

main.py

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
# [depends] functions/tightenconstraints.py
# [makes] calcalpha.mat
# Example Constraint tightening for a linear system.
import numpy as np
import scipy.io
import matplotlib.pyplot as plt
import os
import sys

sys.path.append('functions/')
from hyperrectangle import hyperrectangle
from tightenconstraints import tightenconstraints

# State space matrices
A = np.array([[1, 1], [0, 1]])
B = np.array([[0], [1]])
K = np.array([[-0.4, -1.2]])

# State constraints. u is unconstrained.
xmax = 1
umax = 1
Z = {}
GH, Z['psi'] = hyperrectangle(np.array([[-xmax], [-xmax], [-umax]]),
                             np.array([[xmax], [xmax], [umax]]))
Z['G'] = GH[:,:2]
Z['H'] = GH[:,2:3] # Make H 2D array

# Disturbance set
def infnorm(x):
    return np.max(np.abs(x))

wmax = 0.1
W = np.array([[wmax, wmax, -wmax, -wmax],
              [wmax, -wmax, -wmax, wmax]])
normW = infnorm(W)

KW = K @ W
normKW = infnorm(KW)

# For each value of N, see how far you need to squeeze.
Nmax = 25
AkW = [None] * (Nmax + 1)
alphaW = [None] * (Nmax + 1)
normZbars = np.full((3, Nmax + 1), np.nan)
Nkeep = [2, 3, 4, 5, 6, 7, 8, 10, 15]
Ak = np.eye(A.shape[0])
alphas = np.full(Nmax + 1, np.nan)
thetas = [None] * (Nmax + 1)

for n in range(Nmax + 1):
    AkW[n] = Ak @ W

    normAkW = infnorm(AkW[n])
    normKAkW = infnorm(K @ AkW[n])

    alphas[n] = max(normAkW/normW, normKAkW/normKW)
    alphaW[n] = alphas[n] * W

    if alphas[n] < 1:
        thetas[n] = tightenconstraints(Z, A, B, K, wmax*np.array([[1], [1]]), n + 1)
    else:
        thetas[n] = np.full_like(Z['psi'], np.nan)

    psibar = Z['psi'] - thetas[n]/(1 - alphas[n] + np.finfo(float).eps)
    normZbars[:,n] = psibar.flatten()[[0,2,4]]

    Ak = (A + B @ K) @ Ak

if not os.getenv('OMIT_PLOTS') == 'true':
    plt.figure()
    plt.semilogy(range(Nmax+1), alphas, '-ok')
    plt.plot([1, Nmax], [1, 1], '--r')
    plt.xlabel('N')
    plt.ylabel('Minimum \\alpha')

    plt.figure()
    Nplots = int(np.ceil(np.sqrt(len(Nkeep))))
    for i, nkeep in enumerate(Nkeep):
        plt.subplot(Nplots, Nplots, i+1)
        n = nkeep + 1
        Vin = AkW[n]
        Vout = alphaW[n]

        plt.plot(np.append(Vin[0,:], Vin[0,0]),
                 np.append(Vin[1,:], Vin[1,0]), '-or')
        plt.plot(np.append(Vout[0,:], Vout[0,0]),
                 np.append(Vout[1,:], Vout[1,0]), '-sb')
        plt.title(f'N = {n}: \\alpha = {alphas[n]:.3g}')

    plt.figure()
    N = np.arange(Nmax+1)
    plt.plot(N, normZbars[0,:], '-or',
             N, normZbars[1,:], '-og',
             N, normZbars[2,:], '-ob')
    plt.xlabel('N')
    plt.ylabel('Bound')
    plt.legend(['x_1', 'x_2', 'u'])

# Save data
data = {'N': np.arange(Nmax+1),
        'alpha': alphas,
        'normZbar': normZbars}
scipy.io.savemat('calcalpha.mat', data)