Balancing a cart-pole with LQR

Linearize a cart-pole about the upright, compute the optimal state feedback from the continuous algebraic Riccati equation, and run the nonlinear system under it from 25 degrees off vertical. The Riccati solver is Viv's Schur method, an algorithm after SLICOT (lib/are.viv, lib/schur.viv); its residual is checked here and its solution compared with SciPy's.

The model

The state is (x, xdot, theta, thetadot): cart position and velocity, pole angle from vertical and its rate. vp.control.cartpole_model linearizes the nonlinear dynamics of lib/cartpole.viv (a 1 kg cart, a 0.1 kg pole of half-length 0.5 m) by central differences, so A and B describe the same system the simulation runs. The open loop has one positive eigenvalue: left alone, the pole falls.

import numpy as np
import vipyr as vp
from vipyr.figure import figure

A, B = vp.control.cartpole_model(h=1e-5)
print("A =\n", np.round(A, 4))
print("B =", np.round(B.ravel(), 4))
print(
    "open-loop eigenvalues:", np.round(np.sort(np.linalg.eigvals(A).real), 4)
)

A =
 [[ 0.      1.      0.      0.    ]
 [ 0.      0.     -0.7178  0.    ]
 [ 0.      0.      0.      1.    ]
 [ 0.      0.     15.7917  0.    ]]
B = [ 0.      0.9756  0.     -1.4634]
open-loop eigenvalues: [-3.9739  0.      0.      3.9739]

The gain

Q weights the state errors (the angle 20 times the others) and R the force. vp.control.lqr returns K = R^-1 B^T P, where P solves A^T P + P A - P B R^-1 B^T P + Q = 0, and the control is u = -K s.

Q = np.diag([1.0, 1.0, 20.0, 1.0])
R = np.array([[0.05]])
lqr = vp.control.lqr(A=A, B=B, Q=Q, R=R)
K, P = lqr.K, lqr.P
print("K =", np.round(K.ravel(), 4))
print(
    "closed-loop eigenvalues:",
    np.round(np.sort_complex(np.linalg.eigvals(A - B @ K)), 4),
)

K = [ -4.4721  -8.2239 -63.0331 -16.0163]
closed-loop eigenvalues: [-8.3944+0.j    -4.4524+0.j    -1.2842-0.262j -1.2842+0.262j]

Checking the Riccati solution

The relative residual is the equation's left side divided by the size of its terms; a backward-stable solver leaves a small multiple of the unit roundoff (1.1 × 10⁻¹⁶). SciPy's solve_continuous_are is an independent implementation of the same method, so its P should agree to the problem's conditioning.

from scipy.linalg import solve_continuous_are

Rinv = np.linalg.inv(R)
res = A.T @ P + P @ A - P @ B @ Rinv @ B.T @ P + Q
scale = (
    2 * np.linalg.norm(A, 1) * np.linalg.norm(P, 1)
    + np.linalg.norm(P, 1) ** 2 * np.linalg.norm(B @ Rinv @ B.T, 1)
    + np.linalg.norm(Q, 1)
)
P_ref = solve_continuous_are(A, B, Q, R)
print("relative CARE residual: %.1e" % (np.linalg.norm(res, 1) / scale))
print(
    "max |P - P_scipy| / max |P_scipy|: %.1e"
    % (np.abs(P - P_ref).max() / np.abs(P_ref).max())
)
print("check():", lqr.check().passed)

relative CARE residual: 3.6e-17
max |P - P_scipy| / max |P_scipy|: 1.3e-14
check(): True

The closed loop

vp.control.simulate runs the nonlinear cart-pole, not the linear model, under u = -K s for 6 seconds in steps of 10 ms. A second gain, with the force 20 times more expensive (R = 1), shows the trade the weights make: a gentler push and a slower recovery.

s0 = np.array([0.0, 0.0, np.radians(25.0), 0.0])
cheap = vp.control.simulate(K=K.ravel(), s0=s0, dt=0.01, steps=600).trajectory
K_dear = vp.control.lqr(A=A, B=B, Q=Q, R=np.array([[1.0]])).K
dear = vp.control.simulate(
    K=K_dear.ravel(), s0=s0, dt=0.01, steps=600
).trajectory

for name, tr in (("R = 0.05", cheap), ("R = 1", dear)):
    print(
        "%-8s  final angle %.1e rad   peak force %5.1f N   cart travels to x "
        "= %+.2f m"
        % (
            name,
            abs(tr[-1, 3]),
            np.abs(tr[:, 5]).max(),
            tr[np.argmax(np.abs(tr[:, 1])), 1],
        )
    )

fig = figure(
    "Pole angle under LQR, from 25 degrees", "t (s)", "$\\theta$ (degrees)"
)
fig.plot(cheap[::5, 0], np.degrees(cheap[::5, 3]), "$R = 0.05$")
fig.plot(dear[::5, 0], np.degrees(dear[::5, 3]), "$R = 1$")
fig.style("$R = 0.05$", "s1 nomarker")
fig.style("$R = 1$", "s2 dash nomarker")
fig.hline("upright", 0.0)

fig = figure("Force on the cart", "t (s)", "u (N)")
fig.plot(cheap[::5, 0], cheap[::5, 5], "$R = 0.05$")
fig.plot(dear[::5, 0], dear[::5, 5], "$R = 1$")
fig.style("$R = 0.05$", "s1 nomarker")
fig.style("$R = 1$", "s2 dash nomarker")

R = 0.05  final angle 9.6e-04 rad   peak force  27.5 N   cart travels to x = +0.84 m
R = 1     final angle 6.6e-03 rad   peak force  14.2 N   cart travels to x = +1.42 m

Credits

The Schur, Riccati and Lyapunov solvers use algorithms after SLICOT; no SLICOT code is copied.

Source

gallery/control/plot_cartpole_lqr.py, run as written when these pages were built. The output and the figures above are from that run.

The Viv program it calls is posts/2026-09-22-lqr-balances-the-cart-pole.viv: its atlas, the defs and the calls between them, or open it in the workbench.