from lqr import lqr_riccati, lqr_riccati_integrated
from are import (
are_closed_slowest, are_gains, are_hamiltonian, are_residual, are_residual_rel, are_sign,
are_solve, are_test_system,
)
from schur import hessenberg, schur_advance, schur_norm
# ---- what integrating it backwards costs, on a system with a slow closed loop
SLOW_A = [[-0.001]]
SLOW_B = [[0.001]]
SLOW_W = [[1.0]]
SLOW_R = 1.0
exact_slow = (-0.001 + sqrt(0.001 * 0.001 + 0.001 * 0.001)) / (0.001 * 0.001)
schur_slow = lqr_riccati(SLOW_A, SLOW_B, SLOW_W, SLOW_R)
naive_slow = lqr_riccati_integrated(SLOW_A, SLOW_B, SLOW_W, SLOW_R)
show exact_slow
show schur_slow
show naive_slow
# the order of the demo system
DEMO_N = 6
#| The system the rest of the post works on: sixth order, one input, unstable
#| to start with, and its $A$ is not symmetric.
def demo_system() {
A = are_test_system(DEMO_N, 91.0)
return A
}
DEMO_A = demo_system()
DEMO_B = zeros(DEMO_N, 1)
for i to DEMO_N {
DEMO_B[i, 0] = 0.4 + 0.3 * mod(i, 2.0)
}
DEMO_R = [[0.7]]
DEMO_W = zeros(DEMO_N, DEMO_N)
for i to DEMO_N {
DEMO_W[i, i] = 1.0
}
open_re = zeros(DEMO_N)
open_im = zeros(DEMO_N)
eig_general(DEMO_A, open_re, open_im)
worst_open = -1.0e30
for i to DEMO_N {
worst_open = max(worst_open, open_re[i])
}
show worst_open
DEMO_P = zeros(DEMO_N, DEMO_N)
dim_stable = are_solve(DEMO_A, DEMO_B, DEMO_R, DEMO_W, DEMO_P)
DEMO_K = are_gains(DEMO_B, DEMO_R, DEMO_P)
show dim_stable
show are_residual(DEMO_A, DEMO_B, DEMO_R, DEMO_W, DEMO_P)
show are_residual_rel(DEMO_A, DEMO_B, DEMO_R, DEMO_W, DEMO_P)
show are_closed_slowest(DEMO_A, DEMO_B, DEMO_K)
# ---- the Hamiltonian's spectrum, which is symmetric about the imaginary axis
HAM = are_hamiltonian(DEMO_A, DEMO_B, DEMO_R, DEMO_W)
# the Hamiltonian is 2 DEMO_N square, and has 2 DEMO_N - 1 subdiagonal entries
HAM_N = len(HAM)
NSUB = HAM_N - 1
ham_re = zeros(HAM_N)
ham_im = zeros(HAM_N)
eig_general(HAM, ham_re, ham_im)
ham_trace = 0.0
for i to HAM_N {
ham_trace = ham_trace + HAM[i, i]
}
show ham_trace
#| Split the Hamiltonian's twelve eigenvalues into the six stable ones the solver
#| keeps and the six unstable ones it discards; each unstable one is the negative
#| of a stable one.
stable_re = zeros(DEMO_N)
stable_im = zeros(DEMO_N)
unstable_re = zeros(DEMO_N)
unstable_im = zeros(DEMO_N)
ns = 0
nu = 0
for i to HAM_N {
if ham_re[i] < 0.0 {
stable_re[ns] = ham_re[i]
stable_im[ns] = ham_im[i]
ns = ns + 1
}
else {
unstable_re[nu] = ham_re[i]
unstable_im[nu] = ham_im[i]
nu = nu + 1
}
}
figure("The Hamiltonian's twelve eigenvalues", "real part", "imaginary part")
scatter(stable_re, stable_im, "the six stable ones, which P is built from")
scatter(unstable_re, unstable_im, "the six unstable ones, discarded")
# ---- the QR iteration, watched
SWEEPS = 30
#| Run the solver's own outer loop on `H`, one turn at a time, and record the
#| size of every subdiagonal entry after each one. `schur_advance` is the same
#| function `schur` calls; nothing here is a simplified stand-in for it.
def sweep_history(H, turns) {
n = len(H)
T = zeros(n, n)
Q = zeros(n, n)
junk = hessenberg(H, T, Q)
anorm = schur_norm(H)
hist = zeros(turns + 1, n - 1)
for k to n - 1 {
hist[0, k] = abs(T[k + 1, k])
}
hi = n - 1
its = 0.0
for s to turns {
if hi > 0 {
nexthi = schur_advance(T, Q, hi, its, anorm)
if nexthi == hi {
its = its + 1
}
else {
its = 0.0
}
hi = nexthi
}
for k to n - 1 {
hist[s + 1, k] = abs(T[k + 1, k])
}
}
return hist
}
HIST = sweep_history(HAM, SWEEPS)
#| The base-ten logarithm of a magnitude, with a floor so that an entry that has
#| reached exact zero still has somewhere to be drawn.
def log_floor(v) {
return log(max(v, 1.0e-17)) / 2.302585092994046
}
rows = zeros(NSUB)
for k to NSUB {
rows[k] = 1.0 * k
}
figure("Each subdiagonal entry shrinking to zero", "turns of the outer loop", "the entry's size")
yscale("log")
sweeps_axis = zeros(SWEEPS + 1)
for s to SWEEPS + 1 {
sweeps_axis[s] = 1.0 * s
}
last_row = zeros(SWEEPS + 1)
mid_row = zeros(SWEEPS + 1)
top_row = zeros(SWEEPS + 1)
for s to SWEEPS + 1 {
last_row[s] = HIST[s, 9]
mid_row[s] = HIST[s, 7]
top_row[s] = HIST[s, 3]
}
plot(sweeps_axis, last_row, "entry 9, the first to go")
plot(sweeps_axis, mid_row, "entry 7, which goes next")
plot(sweeps_axis, top_row, "entry 3, higher up")
stage("The subdiagonal being driven to zero", -0.6, 10.6, -18.0, 2.5)
axes("which subdiagonal entry", "log10 of its size")
first = zeros(NSUB)
for k to NSUB {
first[k] = log_floor(HIST[0, k])
}
curve("sub", rows, first)
hline("eps", -16.0)
label("epslabel", "rounding", 8.4, -15.0)
together()
fade_in("sub", 0.3)
fade_in("eps", 0.3)
fade_in("epslabel", 0.3)
apart()
say("Eleven subdiagonal entries, after the Householder reduction to Hessenberg form")
for s to SWEEPS {
shape = zeros(NSUB)
for k to NSUB {
shape[k] = log_floor(HIST[s + 1, k])
}
morph("sub", rows, shape, 0.28)
}
say("What is left standing is one entry per 2 by 2 block, and those are the complex pairs")
wait(1.2)
# ---- the Schur method against the sign function, on the same ten systems
NSYS = 10
sizes = zeros(NSYS)
res_schur = zeros(NSYS)
res_sign = zeros(NSYS)
abs_schur = zeros(NSYS)
pmax = zeros(NSYS)
for si to NSYS {
n = 2 + mod(si, 5.0)
A = are_test_system(n, 55.0 + 13.0 * si)
B = zeros(n, 1)
for i to n {
B[i, 0] = 0.4 + 0.3 * mod(i, 2.0)
}
R = [[0.7]]
W = zeros(n, n)
for i to n {
W[i, i] = 1.0
}
Ps = zeros(n, n)
Pn = zeros(n, n)
cut = are_solve(A, B, R, W, Ps)
junk = are_sign(A, B, R, W, Pn)
sizes[si] = 1.0 * si
res_schur[si] = log_floor(are_residual_rel(A, B, R, W, Ps))
abs_schur[si] = are_residual(A, B, R, W, Ps)
res_sign[si] = log_floor(are_residual_rel(A, B, R, W, Pn))
big = 0.0
for i to n {
for j to n {
big = max(big, abs(Ps[i, j]))
}
}
pmax[si] = big
}
show pmax
figure("Relative residual, ten systems, two methods", "system", "$\\log_{10}$ of the relative residual")
plot(sizes, res_schur, "the Schur method")
plot(sizes, res_sign, "the matrix sign function")
style("the Schur method", "s1 noline circle")
style("the matrix sign function", "s2 noline square")
test "the backward integration is wrong by four fifths on the slow system" {
assert_near(exact_slow, 414.2135623730951, 1.0e-9)
assert_lt(abs(schur_slow[0, 0] - exact_slow) / exact_slow, 1.0e-14)
assert_near(naive_slow[0, 0], 73.783, 1.0e-3)
assert_lt(0.8, abs(naive_slow[0, 0] - exact_slow) / exact_slow)
}
test "the demo system starts unstable and the gains put every mode in the left half plane" {
assert_lt(0.0, worst_open)
assert_lt(are_closed_slowest(DEMO_A, DEMO_B, DEMO_K), -1.0e-8)
}
test "the Hamiltonian has exactly six stable eigenvalues and zero trace" {
assert_near(dim_stable, 6.0, 1.0e-12)
assert_near(ham_trace, 0.0, 1.0e-12)
count = 0.0
for i to HAM_N {
if ham_re[i] < 0.0 {
count = count + 1.0
}
}
assert_near(count, 6.0, 1.0e-12)
}
test "every eigenvalue of the Hamiltonian is matched by minus itself" {
for i to HAM_N {
near = 1.0e30
for j to HAM_N {
near = min(near, abs(ham_re[i] + ham_re[j]) + abs(ham_im[i] - ham_im[j]))
}
assert_lt(near, 1.0e-9)
}
}
test "the largest P, five figures, still has a relative residual below 1e-14" {
# system 9: largest entry 22253.8, absolute residual 5.34e-7, measured
assert_lt(10000.0, pmax[9])
assert_lt(pmax[9], 100000.0)
assert_lt(res_schur[9], -14.0)
assert_lt(4.5e-7, abs_schur[9])
assert_lt(abs_schur[9], 5.5e-7)
}
test "P solves the Riccati equation with a relative residual below 1e-14" {
assert_lt(are_residual_rel(DEMO_A, DEMO_B, DEMO_R, DEMO_W, DEMO_P), 1.0e-14)
for i to DEMO_N {
for j to DEMO_N {
assert_near(DEMO_P[i, j], DEMO_P[j, i], 1.0e-12)
}
}
}
test "thirty turns leaves four subdiagonal entries, one per complex pair" {
standing = 0.0
for k to NSUB {
if HIST[SWEEPS, k] != 0.0 {
standing = standing + 1.0
}
}
assert_near(standing, 4.0, 1.0e-12)
# and they are the ones at the head of each 2 by 2 block
assert_lt(0.1, HIST[SWEEPS, 0])
assert_lt(0.1, HIST[SWEEPS, 2])
assert_lt(0.1, HIST[SWEEPS, 4])
assert_lt(0.1, HIST[SWEEPS, 6])
assert_near(HIST[SWEEPS, 1], 0.0, 1.0e-300)
assert_near(HIST[SWEEPS, 8], 0.0, 1.0e-300)
}
test "entry 9 converges quadratically: the exponent doubles each turn" {
a = HIST[3, 9]
b = HIST[4, 9]
c = HIST[5, 9]
d = HIST[6, 9]
assert_near(a, 3.4939e-3, 1.0e-6)
assert_near(b, 7.70091e-7, 1.0e-11)
assert_near(c, 1.85967e-13, 1.0e-17)
assert_near(d, 3.76417e-27, 1.0e-31)
assert_lt(d, 1.0e-25)
# each step roughly doubles how many digits are gone
assert_lt(1.7, log(b) / log(a))
assert_lt(1.7, log(c) / log(b))
}
test "the Schur method beats the sign function on every one of the ten systems" {
for si to NSYS {
assert_lt(res_schur[si], -14.0)
}
worse = 0.0
for si to NSYS {
worse = max(worse, res_sign[si] - res_schur[si])
}
# somewhere in there the sign function is two orders of magnitude behind
assert_lt(1.5, worse)
}
test "some of these systems are nearly uncontrollable, which is the hard case" {
biggest = 0.0
for si to NSYS {
biggest = max(biggest, pmax[si])
}
assert_lt(1000.0, biggest)
}
# SPDX-License-Identifier: MIT OR Apache-2.0
# SPDX-FileCopyrightText: 2026 The Viv authors