Skip to content
LumenaryAcademy
Control 80 min

State space, LQR and observers

Describe a robot by its state, balance an inverted pendulum with one gain matrix, choose that matrix optimally, and estimate what you cannot measure.

Code tested · 5AI-draftedAwaiting review by a roboticist

Your guides: Chloe · The Builder, Margaret · The Experimenter, Gideon · The Safety Engineer

MotivationH

Balancing a broom

Balance a broom upright on the palm of your hand. You do not watch only the angle: you feel which way it is falling and how fast, and you notice where your hand is, because you cannot walk across the room to keep it up. Four quantities matter at once (hand position, hand speed, broom angle, broom angular speed), and every correction you make affects all of them.

A PID loop from the previous lesson takes one error and produces one command. That is ideal for one motor holding one joint, but awkward here: one motor, the cart's, must hold the pendulum upright and bring the cart back to the middle of its track, and the two goals pull against each other. Two-wheeled delivery robots, legged robots standing on one foot, a rocket landing on its exhaust and a quadrotor hovering in wind all have the same structure: several quantities, coupled, controlled through few inputs.

The tool for such systems is the state-space model: describe the system by a vector of numbers, its state, and the control law by a matrix. This lesson builds that model for the inverted pendulum on a cart, the classic test bench of control, and balances it in simulation three ways: by placing the closed-loop poles where we choose, by the linear-quadratic regulator (LQR), which chooses the gains optimally, and by feedback from an observer that estimates the velocities nobody measures.

IntuitionH

What a state is

The state of a system is the smallest set of numbers that, together with the future inputs, determines its future. For a pendulum it is the angle and the angular velocity: knowing only that the pendulum hangs at 10° does not tell you whether it is about to swing left or right. For a rigid robot arm it is the joint angles and joint velocities, x=(q,q˙)x = (q, \dot q), because the equations of motion give accelerations from them and the torques.

So a robot's dynamics can always be written as x˙=f(x,u)\dot x = f(x, u): the rate of change of the state is a function of the state and the input uu. Near an operating point, a smooth ff is close to linear, and a linear system x˙=Ax+Bu\dot x = A x + B u can be analysed completely with linear algebra. If every state can be measured, the natural controller is state feedback, u=−Kxu = -Kx: each input is a weighted sum of all the states, and the whole design problem is choosing the matrix KK.

Analogy, not a formal construction

A pilot's instrument panel is a state display: altitude, airspeed, heading, attitude, climb rate. No single dial tells the pilot what to do, but together they determine what the aircraft will do next, and the pilot's corrections on the stick and throttle are a learned function of the whole panel.

DefinitionsH

Definitions

Definition— state-space model

A continuous-time state-space model is x˙=f(x,u)\dot x = f(x, u), y=h(x)y = h(x), with state x∈Rnx \in \mathbb{R}^n, input u∈Rmu \in \mathbb{R}^m and measured output y∈Rpy \in \mathbb{R}^p. It is linear time-invariant (LTI) when x˙=Ax+Bu\dot x = Ax + Bu, y=Cxy = Cx. A digital controller sees its discrete-time counterpart xk+1=Adxk+Bdukx_{k+1} = A_d x_k + B_d u_k; when uu is held constant over each period TT (a zero-order hold), Ad=eATA_d = e^{AT} and Bd=∫0TeAs ds BB_d = \int_0^T e^{As}\,ds\,B.

Definition— linearisation

An equilibrium is a pair (x∗,u∗)(x^*, u^*) with f(x∗,u∗)=0f(x^*, u^*) = 0. The linearisation there uses the Jacobians A=∂f/∂xA = \partial f/\partial x and B=∂f/∂uB = \partial f/\partial u, evaluated at (x∗,u∗)(x^*, u^*), and describes small deviations δx=x−x∗\delta x = x - x^*, δu=u−u∗\delta u = u - u^* by δx˙≈A δx+B δu\dot{\delta x} \approx A\,\delta x + B\,\delta u.

Definition— controllability

The discrete system (Ad,Bd)(A_d, B_d) is controllable if from any initial state some input sequence reaches any final state in finitely many steps. The controllability matrix is C=[B, AB, A2B, …, An−1B]\mathcal{C} = [B,\ AB,\ A^2B,\ \ldots,\ A^{n-1}B].

Definition— observability

The system (A,C)(A, C) is observable if the initial state can be recovered from the outputs y0,y1,…y_0, y_1, \ldots (and the known inputs). The observability matrix stacks C,CA,…,CAn−1C, CA, \ldots, CA^{n-1}.

Worked examplesH

The inverted pendulum on a cart

A cart of mass M=1M = 1 kg runs on a straight rail, pushed by a force uu from a motor and belt. On top, a light rod of length l=0.5l = 0.5 m carries a bob of mass m=0.2m = 0.2 kg, pivoting freely. The state is x=(p,p˙,θ,θ˙)x = (p, \dot p, \theta, \dot\theta): the cart's position and velocity, and the rod's angle from upright and its angular velocity.

uθmlMp0
The cart-pole. The cart (mass M) moves along the rail at position p, pushed by force u; the pendulum (bob of mass m on a rod of length l) is at angle θ from upright. Upright, θ = 0, is an equilibrium, but an unstable one.

The equations of motion follow from Lagrange's method, as in the dynamics lesson:

(M+m) p¨+mlcos⁡θ θ¨−mlsin⁡θ θ˙2=u,cos⁡θ p¨+l θ¨=gsin⁡θ.(M + m)\,\ddot p + m l \cos\theta\,\ddot\theta - m l \sin\theta\,\dot\theta^2 = u, \qquad \cos\theta\,\ddot p + l\,\ddot\theta = g \sin\theta .

Upright and at rest, x∗=0x^* = 0 with u∗=0u^* = 0, is an equilibrium. Near it, sin⁡θ≈θ\sin\theta \approx \theta, cos⁡θ≈1\cos\theta \approx 1 and θ˙2≈0\dot\theta^2 \approx 0. Solving the two linear equations for the accelerations gives

p¨=u−mg θM,θ¨=(M+m) g θ−uMl,\ddot p = \frac{u - m g\,\theta}{M}, \qquad \ddot\theta = \frac{(M + m)\,g\,\theta - u}{M l},

so

A=[010000−mgM0000100(M+m)gMl0],B=[01M0−1Ml].A = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & -\frac{mg}{M} & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \frac{(M+m)g}{Ml} & 0 \end{bmatrix}, \qquad B = \begin{bmatrix} 0 \\ \frac{1}{M} \\ 0 \\ -\frac{1}{Ml} \end{bmatrix}.

Read the signs: pushing the cart to the right (u>0u > 0) makes the pendulum fall to the left (θ¨<0\ddot\theta < 0), which is why a cart can catch a falling pendulum by driving underneath it. The code checks the hand derivation against numerical differentiation of the full nonlinear model, a habit worth keeping, since sign errors in linearisations are the most common bug in this field.

python
import numpy as np

M, m, l, g = 1.0, 0.2, 0.5, 9.81      # cart kg, pendulum bob kg, rod m, gravity

def f(x, u):
    """Nonlinear cart-pole: x = [p, p_dot, theta, theta_dot], theta = 0 upright, u = force (N)."""
    p, v, th, w = x
    s, c = np.sin(th), np.cos(th)
    lhs = np.array([[M + m, m * l * c], [c, l]])
    rhs = np.array([u + m * l * s * w**2, g * s])
    acc, alpha = np.linalg.solve(lhs, rhs)
    return np.array([v, acc, w, alpha])

# Linearise at the upright equilibrium by central differences ...
x0, h = np.zeros(4), 1e-6
A_num = np.column_stack([(f(x0 + h * e, 0) - f(x0 - h * e, 0)) / (2 * h) for e in np.eye(4)])
B_num = ((f(x0, h) - f(x0, -h)) / (2 * h)).reshape(4, 1)
# ... and compare with the hand derivation.
A = np.array([[0, 1, 0, 0],
              [0, 0, -m * g / M, 0],
              [0, 0, 0, 1],
              [0, 0, (M + m) * g / (M * l), 0]])
B = np.array([[0], [1 / M], [0], [-1 / (M * l)]])
print("A =\n", np.round(A, 3))
print("B^T =", B.ravel())
print("hand derivation matches finite differences:",
      np.allclose(A, A_num, atol=1e-6), np.allclose(B, B_num, atol=1e-6))
print("eigenvalues of A:", np.round(np.sort(np.linalg.eigvals(A).real), 3))

# Controllability: rank of [B, AB, A^2 B, A^3 B]
C_mat = np.hstack([np.linalg.matrix_power(A, i) @ B for i in range(4)])
print("rank of the controllability matrix:", np.linalg.matrix_rank(C_mat))
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

The eigenvalue +4.85+4.85 s⁻¹ is the falling pendulum: left alone, a small tilt grows like e4.85te^{4.85t}, doubling every ln⁡2/4.85=0.14\ln 2/4.85 = 0.14 s. The double eigenvalue at zero is the cart, which, unpushed, simply keeps rolling. Any controller must move the unstable eigenvalue into the left half-plane, and fast. (The eigenvalues of AA are the poles of the transfer function from the force to any output, which is how Lumenary EE's Laplace and stability lesson would describe the same instability.)

ExplanationH

Controllability: can one force do all that?

Before designing a controller, ask whether any controller can work. In discrete time, unroll the dynamics from x0x_0:

xn=Anx0+[BAB⋯An−1B][un−1⋮u0].x_n = A^n x_0 + \begin{bmatrix} B & AB & \cdots & A^{n-1}B \end{bmatrix} \begin{bmatrix} u_{n-1} \\ \vdots \\ u_0 \end{bmatrix}.

Theorem— Kalman's rank condition

The discrete-time system xk+1=Axk+Bukx_{k+1} = Ax_k + Bu_k with nn states is controllable if and only if its controllability matrix C=[B, AB, …, An−1B]\mathcal{C} = [B,\ AB,\ \ldots,\ A^{n-1}B] has rank nn.

Proof

If rank⁡C=n\operatorname{rank}\mathcal{C} = n, its columns span Rn\mathbb{R}^n, so for any x0x_0 and target x†x^\dagger the vector x†−Anx0x^\dagger - A^n x_0 is C\mathcal{C} times some input sequence, and the equation above reaches x†x^\dagger in nn steps. Conversely, after NN steps the reachable states are ANx0A^N x_0 plus the span of B,AB,…,AN−1BB, AB, \ldots, A^{N-1}B. By the Cayley–Hamilton theorem, AnA^n is a linear combination of I,A,…,An−1I, A, \ldots, A^{n-1}, so every column AjBA^jB with j≥nj \ge n lies in the column space of C\mathcal{C}: for any NN, the span is contained in that column space. If its rank is below nn, some states can never be reached from x0=0x_0 = 0, however long we wait.

∎

For the cart-pole the rank is 4: one force can, in principle, steer cart and pendulum anywhere near upright. (The same test with AA and BB applies in continuous time.)

ExplanationH

Pole placement

With state feedback u=−Kxu = -Kx, the closed loop is xk+1=(A−BK)xkx_{k+1} = (A - BK)x_k, and its behaviour is set by the eigenvalues of A−BKA - BK, the closed-loop poles. For a single-input controllable system, any set of poles (in complex-conjugate pairs) can be achieved, and Ackermann's formula gives the unique KK:

K=[0⋯01]C−1 ϕ(A),K = \begin{bmatrix} 0 & \cdots & 0 & 1 \end{bmatrix} \mathcal{C}^{-1}\, \phi(A),

where ϕ(z)=∏i(z−zi)\phi(z) = \prod_i (z - z_i) is the desired characteristic polynomial, evaluated with AA in place of zz. The inverse of C\mathcal{C} appears, so the formula fails precisely when the system is not controllable.

It is natural to choose poles sis_i in continuous time, where Re⁡si\operatorname{Re} s_i is a decay rate and Im⁡si\operatorname{Im} s_i a frequency, and map them to discrete time with zi=esiTz_i = e^{s_i T}. Below we pick s=−3±js = -3 \pm j and −6±2j-6 \pm 2j s⁻¹ for the cart-pole at 100 Hz. The block then designs a second controller by LQR, which the next section explains, and runs both on the nonlinear cart-pole, starting tilted by 0.2 rad (11°).

python
import numpy as np

M, m, l, g = 1.0, 0.2, 0.5, 9.81
T = 0.01                                   # controller period: 100 Hz

def f(x, u):
    """Nonlinear cart-pole dynamics, x = [p, p_dot, theta, theta_dot]."""
    p, v, th, w = x
    s, c = np.sin(th), np.cos(th)
    acc, alpha = np.linalg.solve([[M + m, m * l * c], [c, l]], [u + m * l * s * w**2, g * s])
    return np.array([v, acc, w, alpha])

def rk4(x, u, h=T):
    k1 = f(x, u); k2 = f(x + h / 2 * k1, u); k3 = f(x + h / 2 * k2, u); k4 = f(x + h * k3, u)
    return x + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4)

A = np.array([[0, 1, 0, 0], [0, 0, -m * g / M, 0], [0, 0, 0, 1], [0, 0, (M + m) * g / (M * l), 0]])
B = np.array([[0], [1 / M], [0], [-1 / (M * l)]])

def expm(X, terms=20):
    """Matrix exponential by scaling and squaring a truncated Taylor series."""
    s = max(0, int(np.ceil(np.log2(max(np.abs(X).sum(axis=0).max(), 1e-16)))) + 1)
    Y, term, E = X / 2**s, np.eye(len(X)), np.eye(len(X))
    for i in range(1, terms):
        term = term @ Y / i
        E = E + term
    for _ in range(s):
        E = E @ E
    return E

# Exact zero-order-hold discretisation: x[k+1] = Ad x[k] + Bd u[k]
Z = expm(np.block([[A, B], [np.zeros((1, 5))]]) * T)
Ad, Bd = Z[:4, :4], Z[:4, 4:]

def acker(Ad, Bd, poles):
    """Ackermann's formula: the K with eig(Ad - Bd K) = poles, for a single input."""
    n = len(Ad)
    Co = np.hstack([np.linalg.matrix_power(Ad, i) @ Bd for i in range(n)])
    coeffs = np.real(np.poly(poles))                   # desired characteristic polynomial
    phi = sum(c * np.linalg.matrix_power(Ad, n - i) for i, c in enumerate(coeffs))
    return (np.linalg.solve(Co.T, np.eye(n)[:, -1]) @ phi).reshape(1, n)

def dlqr(Ad, Bd, Q, R, iters=10000, tol=1e-10):
    """Discrete LQR: iterate the Riccati equation from P = Q until it stops changing."""
    P = Q.copy()
    for i in range(iters):
        K = np.linalg.solve(R + Bd.T @ P @ Bd, Bd.T @ P @ Ad)
        P_new = Q + Ad.T @ P @ (Ad - Bd @ K)
        if np.abs(P_new - P).max() < tol * max(1.0, np.abs(P).max()):
            return K, P_new, i
        P = P_new
    raise RuntimeError("Riccati iteration did not converge")

def balance(K, x0, seconds=10.0):
    x, xs, us = np.array(x0, float), [], []
    for _ in range(int(seconds / T)):
        u = -(K @ x).item()                # full state feedback u = -K x
        xs.append(x); us.append(u)
        x = rk4(x, u)                      # the nonlinear plant, not the linear model
    xs, us = np.array(xs), np.array(us)
    busy = np.nonzero((np.abs(xs[:, 0]) > 0.01) | (np.abs(xs[:, 2]) > 0.01))[0]
    return np.abs(us).max(), np.ptp(xs[:, 0]), (busy[-1] + 1) * T, np.sum(us**2) * T

s_poles = np.array([-3 + 1j, -3 - 1j, -6 + 2j, -6 - 2j])   # chosen in continuous time ...
K_place = acker(Ad, Bd, np.exp(s_poles * T))               # ... and mapped by z = e^(sT)
Q, R = np.diag([10.0, 1.0, 100.0, 1.0]), np.array([[0.1]])
K_lqr, P, iters = dlqr(Ad, Bd, Q, R)
print("pole placement K =", np.round(K_place.ravel(), 2))
print(f"LQR K            = {np.round(K_lqr.ravel(), 2)}   ({iters} Riccati iterations)")
print("LQR closed-loop poles, s = ln(z)/T:", np.round(np.log(np.linalg.eigvals(Ad - Bd @ K_lqr)) / T, 2))

print(f"\nstart tilted 0.2 rad   {'peak |F|':>8} {'cart travel':>11} {'settled':>8} {'effort':>7}")
for name, K in [("pole placement", K_place), ("LQR, R = 0.1", K_lqr)]:
    print("{:22} {:6.1f} N {:9.2f} m {:6.2f} s {:7.1f}".format(name, *balance(K, [0, 0, 0.2, 0])))

def cost(K, x0, steps=3000):
    """LQR's own score J, summed on the linear model (for which LQR is exactly optimal)."""
    x, J = np.array(x0, float), 0.0
    for _ in range(steps):
        u = -(K @ x).item()
        J += x @ Q @ x + R[0, 0] * u * u
        x = Ad @ x + Bd.ravel() * u
    return J
x0 = np.array([0, 0, 0.2, 0])
print(f"cost J: pole placement {cost(K_place, x0):.1f}, LQR {cost(K_lqr, x0):.1f}, x0' P x0 = {x0 @ P @ x0:.1f}")

print("\nThe same Q, and R from cheap to expensive force:")
for r in [0.01, 1.0, 10.0]:
    K, _, _ = dlqr(Ad, Bd, Q, np.array([[r]]))
    print("{:22} {:6.1f} N {:9.2f} m {:6.2f} s {:7.1f}   K = {}".format(
        f"LQR, R = {r}", *balance(K, [0, 0, 0.2, 0]), np.round(K.ravel(), 1)))
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

Both controllers balance the pendulum from 11° and bring the cart back. Note the sign of the cart-position gain: Kp<0K_p < 0, so if the cart is to the right of centre the controller first pushes it further right. That tips the pendulum to the left, and then the large angle gain drives the cart left, under the falling pendulum, back towards the centre. You do the same with a broom: to move your hand left you first jerk it right.

Pole placement answers "where should the poles go?" with "wherever you like", which is both its power and its weakness. Our poles were picked by taste. Push them further left and the response gets faster but the force, which the table shows, grows quickly; and with nn states and mm inputs, there are nmnm gains but only nn poles, so for multiple inputs the poles alone do not determine KK. We need a principle for choosing.

ExplanationH

The linear-quadratic regulator

The LQR replaces "where do the poles go?" with a question an engineer can answer: how much do errors cost, and how much does effort cost? Choose a symmetric positive semidefinite QQ (penalising state errors) and a positive definite RR (penalising inputs), and minimise

J=∑k=0∞(xk⊤Q xk+uk⊤R uk).J = \sum_{k=0}^{\infty} \left( x_k^\top Q\, x_k + u_k^\top R\, u_k \right).

Theorem— Discrete-time LQR

Let (A,B)(A, B) be controllable (stabilisable suffices), R≻0R \succ 0, Q⪰0Q \succeq 0, and let (A,Q1/2)(A, Q^{1/2}) be observable (detectable suffices). Then the discrete algebraic Riccati equation

P=Q+A⊤PA−A⊤PB (R+B⊤PB)−1B⊤PAP = Q + A^\top P A - A^\top P B\,(R + B^\top P B)^{-1} B^\top P A

has a unique positive semidefinite solution PP; the input sequence minimising JJ from any x0x_0 is the state feedback uk=−Kxku_k = -K x_k with K=(R+B⊤PB)−1B⊤PAK = (R + B^\top P B)^{-1} B^\top P A; the closed loop A−BKA - BK is stable; and the minimum cost is x0⊤Px0x_0^\top P x_0.

Proof

A sketch by dynamic programming. Suppose the optimal cost from state xx is V(x)=x⊤PxV(x) = x^\top P x. Then the best first move minimises x⊤Qx+u⊤Ru+(Ax+Bu)⊤P(Ax+Bu)x^\top Q x + u^\top R u + (Ax + Bu)^\top P (Ax + Bu) over uu. This is a convex quadratic in uu; setting its gradient 2Ru+2B⊤P(Ax+Bu)2Ru + 2B^\top P(Ax + Bu) to zero gives u=−(R+B⊤PB)−1B⊤PA x=−Kxu = -(R + B^\top P B)^{-1} B^\top P A\,x = -Kx. Substituting back, the minimum is x⊤(Q+A⊤PA−A⊤PB(R+B⊤PB)−1B⊤PA)xx^\top \big(Q + A^\top P A - A^\top P B (R + B^\top P B)^{-1} B^\top P A\big) x, and for VV to be self-consistent this must equal x⊤Pxx^\top P x for every xx: that is the Riccati equation. The conditions on (A,B)(A, B) and (A,Q1/2)(A, Q^{1/2}) ensure that a stabilising solution exists, is unique, and is the limit of the iteration below; a complete proof is in the references.

∎

The proof also tells you how to compute PP: start with P=QP = Q (the cost of a horizon of one step) and apply the right-hand side repeatedly. Each iteration adds one step to the horizon, and PP converges to the infinite-horizon solution. Our dlqr above took 773 iterations, a fraction of a second of work; libraries use faster direct methods, but the iteration is honest and short.

Q and R: trading error against effort

The table above shows the trade. With QQ fixed, a small RR makes force cheap: gains grow, the response is faster, and the peak force climbs to 30.6 N. A large RR makes force expensive: gains shrink, the peak force falls to 6.1 N, but the cart wanders 0.55 m and takes 8 s to settle. LQR does not tell you which row is best; it tells you, for each weighting, the best possible behaviour, and gives you knobs that mean something physical. Whichever row you pick, the poles are placed for you, the closed loop is guaranteed stable for the linear model, and the gains of all states are chosen together.

A standard starting point is Bryson's rule: make each weight the inverse square of the largest acceptable value. If 0.1 rad of tilt, 0.3 m of cart travel and 10 N of force are each "as much as we will tolerate", then Qθθ=1/0.12=100Q_{\theta\theta} = 1/0.1^2 = 100, Qpp=1/0.32≈11Q_{pp} = 1/0.3^2 \approx 11, and R=1/102=0.01R = 1/10^2 = 0.01; then adjust by simulation.

ExplanationH

Observers: estimating what you cannot measure

State feedback needs the whole state, and robots rarely measure it. Our cart-pole has encoders on the cart's motor and on the pivot, so it measures pp and θ\theta, but not the velocities. Differencing positions works at a pinch but amplifies noise, as the PID lesson showed. The better answer is to run a copy of the model alongside the robot and correct it with the measurements.

Definition— Luenberger observer

For xk+1=Axk+Bukx_{k+1} = Ax_k + Bu_k, yk=Cxky_k = Cx_k, a Luenberger observer maintains an estimate x^\hat x by

x^k+1=Ax^k+Buk+L (yk−Cx^k),\hat x_{k+1} = A\hat x_k + B u_k + L\,(y_k - C\hat x_k),

predicting with the model and correcting by the innovation yk−Cx^ky_k - C\hat x_k, the gap between what was measured and what the estimate predicted. The estimation error e=x−x^e = x - \hat x obeys ek+1=(A−LC) eke_{k+1} = (A - LC)\,e_k, so it decays whenever the eigenvalues of A−LCA - LC lie inside the unit circle.

Choosing LL is the mirror image of choosing KK: the eigenvalues of A−LCA - LC are those of its transpose A⊤−C⊤L⊤A^\top - C^\top L^\top, which is a state-feedback problem for the pair (A⊤,C⊤)(A^\top, C^\top). So we can place observer poles with Ackermann's formula, or compute LL by solving an LQR problem for the transposed system. This duality also gives the test: we can choose the observer poles freely exactly when (A⊤,C⊤)(A^\top, C^\top) is controllable, that is, when (A,C)(A, C) is observable.

plantx, unknownobservermodel + L−Kuyx̂
Output feedback with an observer. The controller never sees the true state x; it applies u = −K x̂, where the observer builds x̂ from the commands u it sends and the measurements y it receives.

The block below checks which sensors make the cart-pole observable, designs LL through the dual LQR, and balances the pendulum from a tilt plus a sideways tap using only the estimate.

python
import numpy as np

M, m, l, g = 1.0, 0.2, 0.5, 9.81
T = 0.01                                   # controller period: 100 Hz

def f(x, u):
    """Nonlinear cart-pole dynamics, x = [p, p_dot, theta, theta_dot]."""
    p, v, th, w = x
    s, c = np.sin(th), np.cos(th)
    acc, alpha = np.linalg.solve([[M + m, m * l * c], [c, l]], [u + m * l * s * w**2, g * s])
    return np.array([v, acc, w, alpha])

def rk4(x, u, h=T):
    k1 = f(x, u); k2 = f(x + h / 2 * k1, u); k3 = f(x + h / 2 * k2, u); k4 = f(x + h * k3, u)
    return x + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4)

A = np.array([[0, 1, 0, 0], [0, 0, -m * g / M, 0], [0, 0, 0, 1], [0, 0, (M + m) * g / (M * l), 0]])
B = np.array([[0], [1 / M], [0], [-1 / (M * l)]])

def expm(X, terms=20):
    """Matrix exponential by scaling and squaring a truncated Taylor series."""
    s = max(0, int(np.ceil(np.log2(max(np.abs(X).sum(axis=0).max(), 1e-16)))) + 1)
    Y, term, E = X / 2**s, np.eye(len(X)), np.eye(len(X))
    for i in range(1, terms):
        term = term @ Y / i
        E = E + term
    for _ in range(s):
        E = E @ E
    return E

# Exact zero-order-hold discretisation: x[k+1] = Ad x[k] + Bd u[k]
Z = expm(np.block([[A, B], [np.zeros((1, 5))]]) * T)
Ad, Bd = Z[:4, :4], Z[:4, 4:]

def dlqr(Ad, Bd, Q, R, iters=10000, tol=1e-10):
    """Discrete LQR: iterate the Riccati equation from P = Q until it stops changing."""
    P = Q.copy()
    for i in range(iters):
        K = np.linalg.solve(R + Bd.T @ P @ Bd, Bd.T @ P @ Ad)
        P_new = Q + Ad.T @ P @ (Ad - Bd @ K)
        if np.abs(P_new - P).max() < tol * max(1.0, np.abs(P).max()):
            return K, P_new, i
        P = P_new
    raise RuntimeError("Riccati iteration did not converge")

# Which sensors make the state observable?
for name, C in [("angle only", [[0, 0, 1, 0]]), ("cart position only", [[1, 0, 0, 0]]),
                ("position and angle", [[1, 0, 0, 0], [0, 0, 1, 0]])]:
    C = np.array(C, float)
    O = np.vstack([C @ np.linalg.matrix_power(Ad, i) for i in range(4)])
    print(f"{name:20} rank of the observability matrix: {np.linalg.matrix_rank(O)}")

C = np.array([[1.0, 0, 0, 0], [0, 0, 1.0, 0]])     # encoders on the cart and on the pivot
K, _, _ = dlqr(Ad, Bd, np.diag([10.0, 1.0, 100.0, 1.0]), np.array([[0.1]]))
Lt, _, _ = dlqr(Ad.T, C.T, np.diag([1e-6, 1e-3, 1e-6, 1e-2]), np.diag([2.5e-7, 1e-6]))  # dual problem
L = Lt.T
print("observer poles, s = ln(z)/T:", np.round(np.sort_complex(np.log(np.linalg.eigvals(Ad - L @ C)) / T), 1))

def run(use_observer, seconds=4.0, show=()):
    rng = np.random.default_rng(3)
    x = np.array([0.0, 0.0, 0.05, 0.5])           # truth: tilted, and just tapped at 0.5 rad/s
    xh = None
    us = []
    for k in range(int(seconds / T)):
        y = C @ x + rng.normal(0, [0.0005, 0.001])    # 0.5 mm and 1 mrad of sensor noise
        if xh is None:
            xh = np.array([y[0], 0.0, y[1], 0.0])     # first guess: as measured, zero velocities
        u = -(K @ (xh if use_observer else x)).item()
        if k in show:
            print(f"{k*T:5.2f} {x[3]:7.3f} {xh[3]:7.3f} {x[1]:7.3f} {xh[1]:7.3f} {u:7.2f}")
        xh = Ad @ xh + Bd.ravel() * u + L @ (y - C @ xh)  # predict with the model, correct with y
        us.append(u)
        x = rk4(x, u)
    return np.abs(us).max()

print(f"\n{'t (s)':>5} {'omega':>7} {'est':>7} {'cart v':>7} {'est':>7} {'force':>7}")
peak_obs = run(True, show=(0, 1, 2, 3, 5, 10, 20, 50, 100))
peak_full = run(False)
print(f"\npeak force: {peak_full:.2f} N with the true state, {peak_obs:.2f} N with the estimate")
Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

With the angle alone the rank is 2: the cart's position and velocity never affect the linearised pendulum, so watching the pendulum reveals nothing about where the cart is, and the controller would slowly drive the cart off the end of the rail. The cart's position alone is enough, because the pendulum's swinging pushes the cart and so shows up in its motion. With both encoders, the observer starts with zero velocity estimates, is wrong by 0.5 rad/s in θ˙\dot\theta, and has caught up within about a tenth of a second; from then on the controller using the estimate behaves almost exactly like the one using the truth.

Theorem— The separation principle

For a linear system with state feedback u=−Kx^u = -K\hat x from a Luenberger observer, the closed-loop poles are the eigenvalues of A−BKA - BK together with those of A−LCA - LC. So KK and LL can be designed separately.

Proof

With e=x−x^e = x - \hat x, the plant is xk+1=Axk−BK(xk−ek)=(A−BK)xk+BKekx_{k+1} = Ax_k - BK(x_k - e_k) = (A - BK)x_k + BKe_k, and the error is ek+1=(A−LC)eke_{k+1} = (A - LC)e_k. In the coordinates (x,e)(x, e) the closed loop matrix is block upper triangular, [A−BKBK0A−LC]\begin{bmatrix} A - BK & BK \\ 0 & A - LC \end{bmatrix}, and the eigenvalues of a block triangular matrix are those of its diagonal blocks.

∎

The usual guideline is to make the observer a few times faster than the controller, so the estimate converges before the controller relies on it, but not so fast that it passes sensor noise straight through. Ours, with poles at −32-32 to −174-174 s⁻¹ against the controller's −1.3-1.3 to −7.7-7.7, is on the fast side; that is affordable because the encoders are quiet.

What the Kalman filter adds

Our observer's weights were half principled and half guessed: those on the measurements were the encoders' noise variances, those on the model a judgement of how much to trust it. If the model's disturbances and the sensors' noise are both random with known covariances, there is an optimal LL, the one that minimises the variance of the estimation error. It comes from the same Riccati equation, with the two covariances in the places of QQ and RR, and the resulting estimator is the Kalman filter (Rudolf Kálmán, 1960). It also handles time-varying and, in its extended forms, nonlinear models, and it is the estimator inside almost every robot's localisation and sensor fusion. The Perception level takes it up in Bayes filters and Kalman filters. The combination of an LQR with a Kalman filter is called LQG (linear-quadratic-Gaussian) control.

ExplanationH

State feedback in practice

LQR's role in robotics today is less as a final controller than as a building block.

  • Balancing and hovering. Two-wheeled robots, reaction-wheel balancers, quadrotor hover and attitude loops, and standing balance on legged robots are commonly designed by LQR or pole placement about an equilibrium, and the gains are often scheduled, recomputed for several operating points and interpolated.
  • Along trajectories. Linearising about a moving trajectory rather than a fixed point gives a time-varying linear system and a time-varying LQR (TVLQR), solved by running the Riccati recursion backwards along the trajectory. It stabilises a planned motion such as a jump or a swing-up. Iterative LQR (iLQR) and differential dynamic programming use the same backward pass to optimise trajectories, as the Planning level's trajectory optimisation lesson shows.
  • Constraints. LQR knows nothing of force limits or rail ends. When they matter, model predictive control takes over, and typically uses the LQR's PP as its terminal cost.
  • Tools. You will rarely write dlqr by hand at work: python-control, SciPy, MATLAB and Drake solve the Riccati equation directly. Writing it once, as here, makes those calls transparent, and comparing your gains with a library's is a good test of your model.

On a real cart-pole the model is never exact: belt friction, motor dynamics, a pendulum that is not a point mass, sensor delay. Engineers identify the parameters from experiments, add an integrator or a disturbance estimate to remove the steady offset that friction causes, saturate the force, and test robustness to the model errors they could not remove. And they start in simulation, then on a small rig with soft end stops, never with a heavy machine or anyone within reach of the rail.

Common misconceptionsH

Common misconceptions

Misconception

"The linearised model is the system." It is an approximation valid near the equilibrium. The LQR above, designed for small angles, balances the nonlinear cart-pole from 0.2 rad, but from 1 rad it will not, and nothing in the linear analysis warns you. Always test a linear design on the nonlinear model, and over the range of states it will meet.

Misconception

"Optimal means best." LQR is optimal for the cost you wrote, on the model you wrote, with no constraints. Change QQ and RR and you get a different optimum. The value of LQR is that its knobs are meaningful and that it always yields a stabilising controller for the linear model, not that its answer is uniquely right.

Misconception

"Controllable means controllable with the motor I have." Controllability is about the linear model with unlimited input. A system can be controllable but need forces far beyond the motor's limit to recover from a given tilt; near the boundary of controllability (C\mathcal{C} close to singular) even small corrections need huge inputs. Check the singular values of C\mathcal{C}, not just its rank.

Misconception

"An observer is a filter for noisy measurements." Its first job is to estimate states that are not measured at all, using the model. It does smooth noise as a side effect, and the Kalman filter makes that trade-off optimal, but an observer on a wrong model will confidently estimate the wrong state.

Exercises

Exercises

1. State space, LQR and observers

Warm-upHuman

A cart-pole kit has room for only one encoder, and the controller will use an observer to estimate the rest of the state. Using the linearised model of this lesson, where should the encoder go?

Choose one

2. State space, LQR and observers

CoreHuman

A larger cart-pole has a cart of M=2M = 2 kg, a bob of m=0.5m = 0.5 kg and a rod of l=1.0l = 1.0 m (g=9.81g = 9.81 m/s²). Without control, how many milliseconds does it take a small tilt to double? Round to the nearest millisecond.

3. State space, LQR and observers

CoreHuman

Two pendulums of equal length ll (bob masses m1m_1 and m2m_2) are mounted side by side on one cart of mass MM, pushed by one force. The linearised state is (p,p˙,θ1,θ˙1,θ2,θ˙2)(p, \dot p, \theta_1, \dot\theta_1, \theta_2, \dot\theta_2), six states. What is the rank of the controllability matrix?

4. State space, LQR and observers

Warm-upHuman

Using Bryson's rule, what diagonal weight QθθQ_{\theta\theta} should you start with for the pendulum angle if the largest tilt you are willing to accept is 0.050.05 rad?

5. State space, LQR and observers

CoreComputational

Implement dlqr(A, B, Q, R) returning (K, P) for the discrete-time LQR by iterating the Riccati equation: start from P=QP = Q, repeat

K=(R+B⊤PB)−1B⊤PA,P←Q+A⊤P(A−BK)K = (R + B^\top P B)^{-1} B^\top P A, \qquad P \leftarrow Q + A^\top P (A - BK)

until PP stops changing, and return the final KK and PP. (The update is the Riccati equation of the lesson rearranged.) Use np.linalg.solve rather than forming the inverse.

The tests check a scalar case whose answer is the golden ratio, and a double integrator: that PP satisfies the Riccati equation, is symmetric, and that A−BKA - BK is stable.

Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

6. State space, LQR and observers

StretchComputational

Implement Ackermann's formula acker(A, B, poles) for a single-input system: return the 1×n1 \times n gain KK for which the eigenvalues of A−BKA - BK are the given poles (real, or complex in conjugate pairs):

K=[0⋯01]C−1 ϕ(A),C=[B, AB, …, An−1B],K = \begin{bmatrix} 0 & \cdots & 0 & 1 \end{bmatrix} \mathcal{C}^{-1}\, \phi(A), \qquad \mathcal{C} = [B,\ AB,\ \ldots,\ A^{n-1}B],

where ϕ\phi is the polynomial with the desired poles as roots. np.poly(poles) returns its coefficients [1,a1,…,an][1, a_1, \ldots, a_n], so ϕ(A)=An+a1An−1+⋯+anI\phi(A) = A^n + a_1 A^{n-1} + \cdots + a_n I.

Runs entirely in your browser, in an isolated worker. Nothing is sent to a server.

7. State space, LQR and observers

CoreHuman

Your LQR-balanced two-wheeled robot works, but its drive motors run hot and the current often touches its limit. Which change to the LQR weights is the natural first step?

Choose one

8. State space, LQR and observers

Warm-upHuman

The matrix PP that gives the LQR gain solves a quadratic matrix equation named after an eighteenth-century Venetian mathematician. What is the equation called? (Give his name followed by equation.)

Further reading and resources

  • Underactuated Robotics · Russ TedrakeA free online textbook; its chapters on the cart-pole, LQR and time-varying LQR are the natural next step after this lesson.
  • Feedback Systems: An Introduction for Scientists and Engineers · Karl Johan Åström and Richard M. Murray · 2021Free textbook chapters on state feedback, reachability, observability and output feedback.
  • Contributions to the theory of optimal control · Rudolf E. Kálmán · 1960The paper that posed and solved the linear-quadratic regulator and introduced controllability. Boletín de la Sociedad Matemática Mexicana 5.
  • A New Approach to Linear Filtering and Prediction Problems · Rudolf E. Kálmán · 1960The Kalman filter, the optimal observer for linear systems with Gaussian noise.
  • Observing the state of a linear system · David G. Luenberger · 1964The observer that reconstructs a full state from a few measurements. IEEE Transactions on Military Electronics 8.
  • Guaranteed margins for LQG regulators · John C. Doyle · 1978A one-page paper whose abstract reads, in full, "There are none." Why an observer can destroy LQR's robustness. IEEE Transactions on Automatic Control 23.
  • python-controlAn open-source Python library with lqr, dlqr, place and observer design, for checking your own code.
  • SciPy: scipy.linalg.solve_discrete_areA direct (non-iterative) solver for the discrete Riccati equation.
  • DrakeA robotics toolbox from MIT and Toyota Research Institute with LQR, time-varying LQR and trajectory optimisation built in.