Problem 622202 · medium · Level 06 Heuristics & Optimization

Ackermann's Formula for the Drone and the Pendulum

pole placement · Ackermann's formula · controllability matrix · characteristic polynomial · state feedback · numpy

Matching coefficients by hand works for two states. A drone's height loop with a lagging motor has three, a cart with a pendulum four, and Ackermann's formula places every pole of a single-input system at once. To get A - B K with the poles p1, ..., pn:

  1. Build the controllability matrix W = [B, A B, ..., A^(n-1) B] (the vectors as columns).
  2. Let [1, c1, ..., cn] be the polynomial with those roots, numpy.poly(poles) (take numpy.real: a complex pair gives real coefficients). Evaluate it at the matrix A: phi(A) = A^n + c1 A^(n-1) + ... + cn I. Horner's rule does this without powers: P = 0, then for each coefficient c in order, P = P @ A + c * I.
  3. Then
K = [0, 0, ..., 0, 1] W^-1 phi(A)

that is, K = q @ phi(A) where q is the last row of W^-1. Get q by solving W.T q = [0, ..., 0, 1] with numpy.linalg.solve rather than inverting W.

If W has rank below n (numpy.linalg.matrix_rank(W) < n) some mode is out of reach and no K exists: return None.

Write ackermann(A, B, poles) that returns K as a list of n floats (an array is accepted), for u = -K x.

Examples

Input:  A = [[1, 0.05, 0.00125], [0, 1, 0.05], [0, 0, 0.6]], B = [0, 0, 0.4], poles = [0.8, 0.8, 0.7]
Output: [12.00000000000012, 7.70000000000002, 0.7500000000000008]
Explanation: a drone's height, vertical speed and motor thrust (the thrust keeps 60 % of its old
value each 0.05 s step and takes 40 % of the command).

Input:  A = [[1, 0.5], [0, 1]], B = [0.125, 0.5], poles = [0.5, 0.6]
Output: [0.7999999999999996, 1.5999999999999999]
Explanation: a lift with dt = 0.5 s; the same gains as matching the coefficients by hand.

Input:  A = [[0.9, 0], [0, 0.8]], B = [1, 0], poles = [0.5, 0.5]
Output: None

Constraints

  • 1 <= n <= 4; one input; len(poles) == n, real numbers and complex-conjugate pairs
  • in the tests W is either clearly invertible or clearly of lower rank
  • answers are compared with a tolerance of 1e-6

Goals

  • Place every pole of a single-input system of any order with one formula
  • Evaluate a polynomial at a matrix with Horner's rule
  • Solve with the controllability matrix instead of inverting it
Starting Python…