A drone's height controller measures the whole state, here x = [height error (m), vertical speed (m/s)], and sets the extra thrust as a weighted sum of it: u = -K x = -(K[0]*x[0] + K[1]*x[1] + ...). This is state feedback, with one gain per state. Put it into the model x[k+1] = A x[k] + B u[k]:
x[k+1] = A x[k] - B (K x[k]) = (A - B K) x[k]
The loop is again a state-space system, with no input left and the closed-loop matrix A - B K. Entry (i, j) of B K is B[i] * K[j] (an outer product: numpy.outer(B, K)). The closed loop's poles are the eigenvalues of A - B K (numpy.linalg.eigvals): the loop is stable when every one has radius below 1, and the largest radius (the spectral radius) says how fast the slowest part of the error dies away.
Write closed_loop(A, B, K) that returns (Acl, radius, stable): the matrix A - B K as a list of rows (a numpy array is accepted), the largest |eigenvalue| as a float, and whether the loop is stable, radius < 1 - 1e-9 (a pole on the unit circle, up to rounding, never dies away).
Examples
Input: A = [[1, 0.1], [0, 1]], B = [0.005, 0.1], K = [4, 3]
Output: ([[0.98, 0.085], [-0.4, 0.7]], 0.8485281374238571, True)
Explanation: dt = 0.1 s. Both poles have radius 0.849: an error shrinks by about 15 % a step.
Input: A = [[1, 0.1], [0, 1]], B = [0.005, 0.1], K = [0, 0]
Output: ([[1.0, 0.1], [0.0, 1.0]], 1.0, False)
Explanation: no feedback: a drift in speed is never corrected.
Input: A = [[1, 0.1], [0, 1]], B = [0.005, 0.1], K = [-4, 3]
Output: ([[1.02, 0.085], [0.4, 0.7]], 1.1041311123146742, False)
Explanation: the height gain has the wrong sign and pushes the drone further away.
Constraints
1 <= n <= 8; one input, soBandKeach havenentries- answers are compared with a tolerance of
1e-6
Goals
- Close a loop with state feedback u = -K x and write the result as x[k+1] = (A - B K) x[k]
- Build A - B K with numpy, using an outer product for B K
- Judge the closed loop by the largest eigenvalue radius of A - B K