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.
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, , because the equations of motion give accelerations from them and the torques.
So a robot's dynamics can always be written as : the rate of change of the state is a function of the state and the input . Near an operating point, a smooth is close to linear, and a linear system can be analysed completely with linear algebra. If every state can be measured, the natural controller is state feedback, : each input is a weighted sum of all the states, and the whole design problem is choosing the matrix .
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.
Definitions
Definition— state-space model
A continuous-time state-space model is , , with state , input and measured output . It is linear time-invariant (LTI) when , . A digital controller sees its discrete-time counterpart ; when is held constant over each period (a zero-order hold), and .
Definition— linearisation
An equilibrium is a pair with . The linearisation there uses the Jacobians and , evaluated at , and describes small deviations , by .
Definition— controllability
The discrete system is controllable if from any initial state some input sequence reaches any final state in finitely many steps. The controllability matrix is .
Definition— observability
The system is observable if the initial state can be recovered from the outputs (and the known inputs). The observability matrix stacks .
The inverted pendulum on a cart
A cart of mass kg runs on a straight rail, pushed by a force from a motor and belt. On top, a light rod of length m carries a bob of mass kg, pivoting freely. The state is : the cart's position and velocity, and the rod's angle from upright and its angular velocity.
The equations of motion follow from Lagrange's method, as in the dynamics lesson:
Upright and at rest, with , is an equilibrium. Near it, , and . Solving the two linear equations for the accelerations gives
so
Read the signs: pushing the cart to the right () makes the pendulum fall to the left (), 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.
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))The eigenvalue s⁻¹ is the falling pendulum: left alone, a small tilt grows like , doubling every 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 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.)
Controllability: can one force do all that?
Before designing a controller, ask whether any controller can work. In discrete time, unroll the dynamics from :
Theorem— Kalman's rank condition
The discrete-time system with states is controllable if and only if its controllability matrix has rank .
Proof
If , its columns span , so for any and target the vector is times some input sequence, and the equation above reaches in steps. Conversely, after steps the reachable states are plus the span of . By the Cayley–Hamilton theorem, is a linear combination of , so every column with lies in the column space of : for any , the span is contained in that column space. If its rank is below , some states can never be reached from , 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 and applies in continuous time.)
Pole placement
With state feedback , the closed loop is , and its behaviour is set by the eigenvalues of , 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 :
where is the desired characteristic polynomial, evaluated with in place of . The inverse of appears, so the formula fails precisely when the system is not controllable.
It is natural to choose poles in continuous time, where is a decay rate and a frequency, and map them to discrete time with . Below we pick and 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°).
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)))Both controllers balance the pendulum from 11° and bring the cart back. Note the sign of the cart-position gain: , 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 states and inputs, there are gains but only poles, so for multiple inputs the poles alone do not determine . We need a principle for choosing.
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 (penalising state errors) and a positive definite (penalising inputs), and minimise
Theorem— Discrete-time LQR
Let be controllable (stabilisable suffices), , , and let be observable (detectable suffices). Then the discrete algebraic Riccati equation
has a unique positive semidefinite solution ; the input sequence minimising from any is the state feedback with ; the closed loop is stable; and the minimum cost is .
Proof
A sketch by dynamic programming. Suppose the optimal cost from state is . Then the best first move minimises over . This is a convex quadratic in ; setting its gradient to zero gives . Substituting back, the minimum is , and for to be self-consistent this must equal for every : that is the Riccati equation. The conditions on and 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 : start with (the cost of a horizon of one step) and apply the right-hand side repeatedly. Each iteration adds one step to the horizon, and 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 fixed, a small makes force cheap: gains grow, the response is faster, and the peak force climbs to 30.6 N. A large 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 , , and ; then adjust by simulation.
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 and , 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 , , a Luenberger observer maintains an estimate by
predicting with the model and correcting by the innovation , the gap between what was measured and what the estimate predicted. The estimation error obeys , so it decays whenever the eigenvalues of lie inside the unit circle.
Choosing is the mirror image of choosing : the eigenvalues of are those of its transpose , which is a state-feedback problem for the pair . So we can place observer poles with Ackermann's formula, or compute by solving an LQR problem for the transposed system. This duality also gives the test: we can choose the observer poles freely exactly when is controllable, that is, when is observable.
The block below checks which sensors make the cart-pole observable, designs through the dual LQR, and balances the pendulum from a tilt plus a sideways tap using only the estimate.
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")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 , 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 from a Luenberger observer, the closed-loop poles are the eigenvalues of together with those of . So and can be designed separately.
Proof
With , the plant is , and the error is . In the coordinates the closed loop matrix is block upper triangular, , 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 to s⁻¹ against the controller's to , 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 , 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 and , 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.
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 as its terminal cost.
- Tools. You will rarely write
dlqrby 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 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 and 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 ( close to singular) even small corrections need huge inputs. Check the singular values of , 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
1. State space, LQR and observers
Warm-upHumanA 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?
2. State space, LQR and observers
CoreHumanA larger cart-pole has a cart of kg, a bob of kg and a rod of m ( 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
CoreHumanTwo pendulums of equal length (bob masses and ) are mounted side by side on one cart of mass , pushed by one force. The linearised state is , six states. What is the rank of the controllability matrix?
4. State space, LQR and observers
Warm-upHumanUsing Bryson's rule, what diagonal weight should you start with for the pendulum angle if the largest tilt you are willing to accept is rad?
5. State space, LQR and observers
CoreComputationalImplement dlqr(A, B, Q, R) returning (K, P) for the discrete-time LQR by iterating the Riccati equation: start from , repeat
until stops changing, and return the final and . (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 satisfies the Riccati equation, is symmetric, and that is stable.
6. State space, LQR and observers
StretchComputationalImplement Ackermann's formula acker(A, B, poles) for a single-input system: return the gain for which the eigenvalues of are the given poles (real, or complex in conjugate pairs):
where is the polynomial with the desired poles as roots. np.poly(poles) returns its coefficients , so .
7. State space, LQR and observers
CoreHumanYour 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?
8. State space, LQR and observers
Warm-upHumanThe matrix 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.