#
# Combining paradigms: the algebraic Riccati equation
# ===================================================
#
# Run in batch: maple -q 02_riccati_numeric_then_certify.mpl
# Needs the LACE toolbox (with(LACE) must work).
#
# The continuous algebraic Riccati equation (CARE)
#
#     A^T X + X A - X B B^T X + Q = 0
#
# is solved every day in floating point (care in Matlab, Kleinman's iteration). We show
# three ways of using exact and certified tools around such a solver:
#
# 1. certify the floating-point solution: a Kantorovich test proves that an exact
#    solution lies in a tiny ball around it;
# 2. refine it to any precision by interval Newton;
# 3. solve exactly: compute all the real solutions, and prove that the one found is the
#    only stabilizing one.
#
# The example: the system dx/dt = A x + B u with one input, where
#
#     A = [0  1]      B = [0]
#         [2 -1],         [1].
#
# Without control it is unstable: the eigenvalues of A are 1 and -2. We look for the
# control u that minimizes the integral over [0, infinity) of x^T x + u^2 (weights Q = I
# on the state, R = 1 on the input). It is u = -B^T X x, where X is the solution of the
# CARE that makes the closed loop A - B B^T X stable.

with(LinearAlgebra):
with(LACE):
interface(rtablesize = 20):

A := Matrix([[0, 1], [2, -1]]):
B := Matrix([[0], [1]]):
Q := IdentityMatrix(2):

# 1. Floating point: Kleinman's iteration
# ---------------------------------------
#
# Kleinman's method improves a feedback u = -K x step by step. A gain is the row vector
# K of this feedback; it is stabilizing when the closed loop dx/dt = (A - B K) x is
# stable, that is when the eigenvalues of A - B K have negative real parts. The method
# needs one to start. Here K0 = (3 1) gives A - B K0 = [[0, 1], [-1, -2]], of
# characteristic polynomial (s+1)^2: both eigenvalues are -1.
#
# At each step, with A_k = A - B K_k (the closed loop), the feedback u = -K_k x gives
# dx/dt = A_k x and the cost integral of x^T (Q + K_k^T K_k) x = x0^T X_{k+1} x0, with
# X_{k+1} the integral over [0, infinity) of exp(A_k^T t) (Q + K_k^T K_k) exp(A_k t),
# which converges because A_k is stable. Differentiating under the integral, X_{k+1} is
# the solution of the Lyapunov equation
#
#     A_k^T X_{k+1} + X_{k+1} A_k + Q + K_k^T K_k = 0,
#
# which is linear in X_{k+1} (no X B B^T X term). Maple solves it in hardware floats
# with LinearAlgebra:-LyapunovSolve (a Bartels-Stewart method, through the Schur form
# of A_k), where the Julia version solved the Kronecker form of the same linear system.
# The next gain is K_{k+1} = B^T X_{k+1}. It is again stabilizing: by a theorem of
# Kleinman (1968), when K0 is stabilizing and Q is positive definite, every K_k is
# stabilizing, and the iterates converge quadratically to the stabilizing solution of
# the CARE. On the example, the first step gives, exactly, X_1 = [[10, 5], [5, 3]] and
# K_1 = (5 3), and A - B K_1 has characteristic polynomial (s+1)(s+3).

kleinman := proc(A, B, Q, K0, iters := 30)
    local K, Ak, X, sc, i;
    K := K0;
    for i to iters do
        Ak := A - B . K;
        # LyapunovSolve(M, C) returns X, scale with M X + X M^T = scale C
        X, sc := LyapunovSolve(Transpose(Ak), -(Q + Transpose(K) . K));
        X := X / sc;
        K := Transpose(B) . X;
    end do;
    X
end proc:

hw := M -> Matrix(M, datatype = float[8]):
K0 := hw(Matrix([[3, 1]])):                  # closed loop (s+1)^2
Xf := kleinman(hw(A), hw(B), hw(Q), K0):
printf("Kleinman solution Xf:\n");
print(Xf);

# The residual is at the level of rounding errors (Frobenius norm):

res := Norm(Transpose(A) . Xf + Xf . A - Xf . B . Transpose(B) . Xf + Q, Frobenius):
printf("residual ||A^T Xf + Xf A - Xf B B^T Xf + Q|| = %.6e\n", res);

# A small residual is not a proof: it says that Xf solves a nearby equation, not that an
# exact solution is nearby.

# 2. The CARE as a polynomial system
# ----------------------------------
#
# With X = [[x1, x2], [x2, x3]], the CARE is a system of three quadratic equations in
# three unknowns, with rational coefficients.

vars := [x1, x2, x3]:
X := Matrix([[x1, x2], [x2, x3]]):
E := map(expand, Transpose(A) . X + X . A - X . B . Transpose(B) . X + Q):
sys := [E[1, 1], E[1, 2], E[2, 2]]:
printf("CARE as a polynomial system:\n");
print(sys);

# 3. Certify: the Kantorovich test
# --------------------------------
#
# At the point Xf, LACE computes in interval arithmetic the constants of Kantorovich's
# theorem: 2 B0 (twice the length of the Newton step, B0 is also written eta) and C
# (written mu in LACE). If C <= 1, there is exactly one solution of the system in the
# ball of radius 2 B0 around the point. The floats of Xf are read exactly, as rationals.

pt := map(exact_rational, [Xf[1, 1], Xf[1, 2], Xf[2, 2]]):
r, C := op(kantorovich_test(sys, vars, pt, 63, 63, 30, verbose = 0)):
printf("2 B0 in [%.10e, %.10e]\n", r[1], r[2]);
printf("C    in [%.10e, %.10e]\n", C[1], C[2]);
printf("certified (C < 1): %a\n", evalb(C[2] < 1));

# C is far below 1: the floating-point solution is within about 4e-15 of an exact
# solution, and this is now a theorem. The test costs one evaluation of the system, of
# its Jacobian and of a bound on its second derivatives at one point.

# 4. Refine: interval Newton
# --------------------------
#
# Interval Newton contracts a box known to contain the solution. The Kantorovich ball
# gives one: the box of half-width 2 B0 around the point. (Started from the point
# itself, the iteration only computes a Newton step, which is not an enclosure.) Here
# 200 bits, with a working precision of 256 bits.

box := [seq([x - r[2], x + r[2]], x in pt)]:
status, Xbox := op(1 .. 2, interval_newton(sys, vars, box, 256, 256, 200, 200, 10,
                                            refine = 1, verbose = 0)):
printf("status = %d, log2 of the widths = %a\n", status,
       [seq(evalf[17](log[2](b[2] - b[1])), b in Xbox)]);

# The status 1 means that the box is proved to contain a zero (some Newton step mapped
# the box into its interior) and that the target precision is reached. The exact
# solution is known here, x2 = 2 + sqrt(5), x3 = sqrt(5), x1 = x2 x3 + x2 - 2 x3 =
# 7 + sqrt(5), and it is in the boxes. The comparison is exact: for a rational q,
# q < sqrt(5) if and only if q < 0 or q^2 < 5.

lt_sqrt5 := q -> evalb(q < 0 or q^2 < 5):         # q < sqrt(5)
gt_sqrt5 := q -> evalb(q > 0 and q^2 > 5):        # q > sqrt(5)
inside := (b, c) -> lt_sqrt5(b[1] - c) and gt_sqrt5(b[2] - c):   # c + sqrt(5) in (lo, hi)
printf("exact solution (7+sqrt5, 2+sqrt5, sqrt5) in the boxes: %a\n",
       inside(Xbox[1], 7) and inside(Xbox[2], 2) and inside(Xbox[3], 0));

# 5. Solve exactly: all the solutions
# -----------------------------------
#
# The Kantorovich test is local: it says nothing about the other solutions. Exact
# solving (a rational univariate representation, then certified isolation, in LACE)
# gives all the real solutions in boxes:

sols := real_solutions(sys, vars, prec = 30):
printf("%d real solutions (one box per solution, bounds to 30 digits):\n", nops(sols));
for s in sols do
    for b in s do printf("  [%a, %a]", evalf[30](b[1]), evalf[30](b[2])) end do;
    printf("\n");
end do:

# The closed loop A - B B^T X has characteristic polynomial s^2 + (1 + x3) s + (x2 - 2).
# Its coefficients are known as intervals at every solution. For interval
# coefficients we use Kharitonov's theorem (1978): every polynomial of the
# family is Hurwitz stable (all roots with negative real part) if and only if four
# particular polynomials, built from the bounds, are; each one is tested exactly by
# Maple's PolynomialTools:-Hurwitz (Routh-Hurwitz on rational coefficients).
# Conversely, when a coefficient is <= 0 on its whole interval (leading one positive),
# no polynomial of the family is stable. Otherwise the answer is FAIL (undecided).

hurwitz_interval := proc(c)      # c: intervals [lo, hi], highest degree first
    local d, lo, hi, pat, k, s, i;
    if ormap(b -> b[2] <= 0, c) then return false end if;
    d := nops(c) - 1;
    lo := ListTools:-Reverse(map(b -> b[1], c));          # constant term first
    hi := ListTools:-Reverse(map(b -> b[2], c));
    for pat in [[1, 1, 2, 2], [2, 2, 1, 1], [1, 2, 2, 1], [2, 1, 1, 2]] do
        k := add([lo[i], hi[i]][pat[irem(i - 1, 4) + 1]] * s^(i - 1), i = 1 .. d + 1);
        if PolynomialTools:-Hurwitz(k, s) <> true then return FAIL end if;
    end do;
    true
end proc:

closed_loop := x -> [[1, 1], 1 +~ x[3], x[2] -~ 2]:      # highest degree first
stabilizing := [seq(hurwitz_interval(closed_loop(x)), x in sols)]:
printf("stabilizing: %a\n", stabilizing);

# Exactly one solution is stabilizing, and it is the one certified in step 3: its box
# lies in the Kantorovich ball of radius 2 B0 around the floating-point point.

xs := sols[ListTools:-Search(true, stabilizing)]:
printf("stabilizing box inside the Kantorovich ball: %a\n",
       andmap(i -> max(abs(xs[i][1] - pt[i]), abs(xs[i][2] - pt[i])) < r[1], [1, 2, 3]));

# 6. Scaling
# ----------
#
# How do the certificate (step 3) and exact solving (step 5) behave when the size grows?
# We build larger systems in the same way as the example, dx/dt = A x + B u with one
# input, of dimension n:
#
# - A has ones above the diagonal, last row (-a_1, ..., -a_n), and zeros elsewhere, so
#   that its characteristic polynomial is s^n + a_n s^(n-1) + ... + a_2 s + a_1; we take
#   a_k = (-1)^k (1 + k mod 3), arbitrary values that make A unstable (a_1 = -2 < 0);
# - B = (0, ..., 0, 1)^T, Q = I, R = 1;
# - the initial gain K0 is chosen so that all the eigenvalues of A - B K0 are -1:
#   K0 = (binomial(n, n) - a_1, ..., binomial(n, 1) - a_n), so that A - B K0 has
#   characteristic polynomial (s+1)^n.
#
# The unknown X is a symmetric n x n matrix: n(n+1)/2 unknowns, and as many quadratic
# equations. For each n the table gives the Kantorovich constant C at the Kleinman
# solution (upper bound; certified when C <= 1), the time of Kleinman plus the test
# (t_num_cert, in seconds), the number of real solutions of the CARE and the time to
# compute them all (t_exact). Times are wall-clock times.

care := proc(a)
    local n, A, B, X, E, xs, i, j;
    n := nops(a);
    A := Matrix(n, n, (i, j) -> `if`(j = i + 1, 1, 0));
    A[n, 1 .. n] := Vector[row](-a);
    B := Matrix(n, 1, {(n, 1) = 1});
    xs := [seq(seq(x || i || _ || j, j = i .. n), i = 1 .. n)];
    X := Matrix(n, n, (i, j) -> x || (min(i, j)) || _ || (max(i, j)));
    E := map(expand, Transpose(A) . X + X . A - X . B . Transpose(B) . X
                     + IdentityMatrix(n));
    [seq(seq(E[i, j], j = i .. n), i = 1 .. n)], xs, hw(A), hw(B)
end proc:

compare := proc(n)
    local a, sys, xs, Af, Bf, K0, t0, Xf, pt, kt, t_num, sols, t_exact, i, j, k;
    a := [seq((-1)^k * (1 + irem(k, 3)), k = 1 .. n)];
    sys, xs, Af, Bf := care(a);
    K0 := hw(Matrix([[seq(binomial(n, n - k + 1) - a[k], k = 1 .. n)]]));
    t0 := time[real]();
    Xf := kleinman(Af, Bf, hw(IdentityMatrix(n)), K0);
    pt := map(exact_rational, [seq(seq(Xf[i, j], j = i .. n), i = 1 .. n)]);
    kt := kantorovich_test(sys, xs, pt, 63, 63, 10, verbose = 0);
    t_num := time[real]() - t0;
    t0 := time[real]();
    sols := real_solutions(sys, xs, prec = 20);
    t_exact := time[real]() - t0;
    [n, nops(sys), evalf(kt[2][2]), t_num, nops(sols), t_exact]
end proc:

compare(2):        # warm-up, as in the Julia version
printf("%3s %9s %12s %12s %15s %10s\n", "n", "unknowns", "C", "t_num_cert",
       "real_solutions", "t_exact");
for n from 2 to 9 do
    line := compare(n);
    printf("%3d %9d %12.4e %12.4f %15d %10.4f\n", op(line));
end do:

# On this structured problem, exact solving of the whole system is competitive with the
# certification of a single solution up to n = 8 or so: the Kantorovich test bounds
# second derivatives over n(n+1)/2 unknowns, and its cost grows quickly too. Beyond,
# exact solving becomes the more expensive one: about 6 s for n = 10 (64 real
# solutions) on a laptop, nine times more than for n = 9, while the number of real
# solutions doubles every two orders. The Kleinman point itself is not reproducible to
# the last bit: LyapunovSolve calls threaded floating-point libraries, and from one run
# to the next C may change in its leading digits for some n (seen for n = 4), while
# staying far below 1. The constant C also grows with n and would eventually
# exceed 1 at 63 bits, which calls for a more precise point (interval Newton) or the
# preconditioned test (flags = {precond}, section 7).

# 7. A preconditioned Kantorovich test
# -------------------------------------
#
# Beyond n = 11, the test fails at the Kleinman point: C > 1. The point is not worse than
# before, but the constant of the test depends on how the equations are scaled. With
# flags = {precond}, LACE applies the test to the system multiplied by an approximate
# inverse of its Jacobian matrix at the point, which does not change its solutions; the
# second derivatives of the equations are then combined before taking absolute values, so
# that their cancellations are kept. For each n: C and the time of the test, without and
# with preconditioning (only the numerical part, no exact solving).

kanto_compare := proc(n)
    local a, sys, xs, Af, Bf, K0, Xf, pt, t0, k0, t_0, k1, t_1, i, j, k;
    a := [seq((-1)^k * (1 + irem(k, 3)), k = 1 .. n)];
    sys, xs, Af, Bf := care(a);
    K0 := hw(Matrix([[seq(binomial(n, n - k + 1) - a[k], k = 1 .. n)]]));
    Xf := kleinman(Af, Bf, hw(IdentityMatrix(n)), K0);
    pt := map(exact_rational, [seq(seq(Xf[i, j], j = i .. n), i = 1 .. n)]);
    t0 := time[real]();
    k0 := kantorovich_test(sys, xs, pt, 63, 63, 10, verbose = 0);
    t_0 := time[real]() - t0;
    t0 := time[real]();
    k1 := kantorovich_test(sys, xs, pt, 63, 63, 10, verbose = 0, flags = {precond});
    t_1 := time[real]() - t0;
    [n, evalf(k0[2][2]), t_0, evalf(k1[2][2]), t_1]
end proc:

printf("\n%3s %12s %8s %12s %10s\n", "n", "C", "t", "C_precond", "t_precond");
for n from 10 to 14 do
    printf("%3d %12.4e %8.2f %12.4e %10.2f\n", op(kanto_compare(n)));
end do:

# Without preconditioning the test fails from n = 12 (C ~ 4); with it, the same points
# are certified up to n = 13 (C ~ 0.12), for less than twice the cost of the test. At
# n = 14 both fail: the point itself must then be refined. The values differ from those of
# the Julia worksheet (C = 16 at n = 12, three times the cost) because the Kleinman points
# differ in their last bits (LyapunovSolve of Maple against the Julia solver); the
# conclusion is the same.

# Summary
# -------
#
# | Step                            | Tool            | What it gives                          |
# |---------------------------------|-----------------|----------------------------------------|
# | Kleinman in hardware floats     | LinearAlgebra   | a fast approximation, no guarantee     |
# | Kantorovich test                | LACE            | proof: an exact solution within 2 B0   |
# | Interval Newton                 | LACE            | that solution to any precision         |
# | Exact solving + interval        | LACE,           | all solutions, and uniqueness of the   |
# | Routh-Hurwitz (Kharitonov)      | PolynomialTools | stabilizing one                        |
#
# The certificate is local and its cost is polynomial in the size; exact solving is
# global and its cost grows with the number of solutions. Use the first to trust a
# numerical result, the second to know that nothing else exists.

LACE:-finish():