Placing poles needs you to know where they should go. The linear-quadratic regulator (LQR) asks instead what you care about: from the start x0, the closed loop u = -K x should make the total cost
J = sum over k = 0, 1, 2, ... of x[k]ᵀ Q x[k] + u[k]ᵀ R u[k]
as small as possible. Q (n by n) weights the errors in the state, R (m by m, for m inputs) weights the effort. For the lift, a big Q[0][0] says "be at the floor quickly", a big R says "be gentle with the motor". B is an n by m matrix here, one column per input, and K is m by n.
The cost of a stable loop is a quadratic form, J = x0ᵀ P x0, and the optimal P is the fixed point of the discrete Riccati equation. Iterate it:
P = Q
repeat (at most 10000 times):
K = solve(R + Bᵀ P B, Bᵀ P A) (numpy.linalg.solve: an m by n matrix)
P_new = Q + Aᵀ P (A - B K)
stop after this round if max |P_new - P| <= 1e-10 * max(1, max |P_new|); P = P_new
then the optimal gain is K = solve(R + Bᵀ P B, Bᵀ P A) from the final P, and its cost is x0ᵀ P x0.
To see what LQR gains over a design by hand, price a given gain K_hand the same way. Its closed loop is Acl = A - B K_hand. If the largest |eigenvalue| of Acl is 1 or more the cost is infinite: report None. Otherwise, with S = Q + K_handᵀ R K_hand (the cost of one step), iterate P = S, P_new = S + Aclᵀ P Acl with the same stopping rule; the cost is x0ᵀ P x0.
Write lqr(A, B, Q, R, x0, K_hand) that returns (K, cost, hand_cost): the optimal gain (a list of m rows), its cost, and the hand gain's cost (or None).
Examples
Input: A = [[1, 0.5], [0, 1]], B = [[0.125], [0.5]], Q = [[1, 0], [0, 0]], R = [[0.1]], x0 = [1, 0],
K_hand = [[0.8, 1.6]]
Output: ([[1.7031778364571166, 1.8456315105940682]], 2.1672798589669666, 2.7904761903997515)
Explanation: the lift 1 m below the floor, dt = 0.5 s. The hand gain puts the poles at 0.5 and 0.6 and
costs 2.79; the optimal gain is stiffer and costs 2.17, 22 % less.
Input: the same with R = [[1]]
Output: ([[0.7034648345510356, 1.1861406615753043]], 3.3722813230217143, 3.4761904760729676)
Explanation: effort is ten times dearer, so the best gain is softer, and close to the hand gain.
Input: A = [[0.9]], B = [[0.5]], Q = [[1]], R = [[1]], x0 = [2], K_hand = [[1.0]]
Output: ([[0.624220425433774]], 8.494387062834484, 9.523809523740898)
Constraints
1 <= n <= 4,1 <= m <= 2;Qis symmetric with no negative eigenvalues (zeros on its diagonal are allowed),Ris symmetric and positive definite- in every test the Riccati iteration settles in well under 10000 rounds, and no hand loop has a spectral radius between 0.97 and 1.03
- answers are compared with a tolerance of
1e-6
Goals
- State a controller's job as a quadratic cost on the state and the input
- Find the optimal state-feedback gain by iterating the discrete Riccati equation until it settles
- Price any given gain with the same kind of iteration, and compare it with the optimum