Fitting a drone's flight log gave a model of how its pitch angle y answers the motor command u, as a difference equation in the one-sample delay z^-1 (coefficient lists with the constant term first, as in Level 4):
a[0]*y[k] + a[1]*y[k-1] + ... + a[n]*y[k-n] = b[1]*u[k-1] + ... + b[n]*u[k-n]
with b[0] = 0: the command takes at least one sample to show. Tools for state feedback want the same system as x[k+1] = A x[k] + B u[k], y[k] = C x[k]. One standard way is the companion form (also called the controllable canonical form). First divide every coefficient by a[0], so the equation starts with 1 * y[k]. Then, with n = max(len(a), len(b)) - 1 and missing coefficients counted as 0:
A = [[-a[1], -a[2], ..., -a[n]], B = [1, 0, ..., 0] C = [b[1], b[2], ..., b[n]]
[ 1, 0, ..., 0 ],
[ 0, 1, ..., 0 ],
...
[ 0, ..., 1, 0 ]]
The first row does the recursion; each lower row just copies one entry down (row i has a single 1 in column i - 1), so the state holds the last n values of a hidden signal. Simulating this model from x = 0 gives exactly the outputs of the difference equation from rest. (The way back: the characteristic polynomial of A, numpy.poly(A), is [1, a[1], ..., a[n]].)
Write companion(a, b) that returns (A, B, C), with every entry a float (write a negated coefficient as 0.0 - a[j], so that a zero stays 0.0 and does not print as -0.0).
Examples
Input: a = [1, -1.5, 0.7], b = [0, 0.4, 0.2]
Output: ([[1.5, -0.7], [1.0, 0.0]], [1.0, 0.0], [0.4, 0.2])
Explanation: y[k] = 1.5 y[k-1] - 0.7 y[k-2] + 0.4 u[k-1] + 0.2 u[k-2]. Both forms answer a one-sample
kick with 0, 0.4, 0.8, 0.92, 0.82, ...
Input: a = [2, -1], b = [0, 1]
Output: ([[0.5]], [1.0], [0.5])
Explanation: dividing by a[0] = 2 gives y[k] = 0.5 y[k-1] + 0.5 u[k-1].
Input: a = [1, -0.5], b = [0, 0, 0.3]
Output: ([[0.5, 0.0], [1.0, 0.0]], [1.0, 0.0], [0.0, 0.3])
Explanation: b is longer, so n = 2 and a is padded with a zero: a two-sample delay needs two states.
Constraints
n >= 1;a[0]is not zero;b[0] == 0- answers are compared with a tolerance of
1e-6; lists of lists are expected, and tuples or numpy arrays are accepted
Goals
- Rewrite a difference equation of order n as a state-space model with n states
- Normalise the equation so its leading coefficient is 1
- See that the bottom of the companion matrix only shifts the state along