Problem 510552 · medium · Level 05 Advanced Algorithms & Graphs

Sampling a Motor with a Matrix Exponential

zero-order hold · state space · matrix exponential · power series · numpy · discretisation

A plant with several state variables is written in state-space form. For a DC motor the state is x = [angle, speed], and

rate of change of x = A x + B u,     A = [[0, 1], [0, -1/tau]],   B = [0, gain/tau]

(the angle changes at the speed; the speed relaxes towards gain * u with time constant tau). A computer that holds the voltage u constant for T seconds sees the exact sampled model

x[k+1] = Ad x[k] + Bd u[k]

with the matrix exponential and its integral, both given by power series you can sum term by term:

Ad = I + (AT) + (AT)^2 / 2! + (AT)^3 / 3! + ...
Bd = T * (I + (AT) / 2! + (AT)^2 / 3! + (AT)^3 / 4! + ...) B

Here I is the identity matrix and (AT)^k is the matrix A*T multiplied by itself k times. (For a single state these are exp(aT) and (exp(aT) - 1) / a * b, the first-order formulas.) Each term is the previous one times AT / k, so no powers or factorials need to be computed separately.

Write zoh(A, B, T) that returns (Ad, Bd): Ad as an n by n list of rows and Bd as a list of n numbers, where n = len(A). Keep adding terms until the largest entry (in absolute value) of a new term of the Ad series is below 1e-14, or 200 terms. numpy makes this short: numpy.array(A, dtype=float), M @ N for a matrix product, numpy.eye(n) for I, numpy.abs(M).max(), and .tolist() (arrays are accepted too).

Examples

Input:  A = [[0, 1], [0, -5]], B = [0, 50], T = 0.1
Output: ([[1.0, 0.07869386805747333], [0.0, 0.6065306597126333]], [0.2130613194252669, 3.9346934028736644])
Explanation: the motor with tau = 0.2 s and gain 10 rad/s per volt, sampled every 0.1 s. The speed
keeps exp(-0.5) = 0.607 of itself; one volt held for 0.1 s adds 3.93 rad/s and 0.213 rad.

Input:  A = [[0, 1], [0, 0]], B = [0, 1], T = 0.5
Output: ([[1.0, 0.5], [0.0, 1.0]], [0.125, 0.5])
Explanation: a pure double integrator (a mass pushed by a force): the series stops after two terms,
and Bd = [T^2 / 2, T].

Constraints

  • 1 <= n <= 6; every entry of A * T has size at most 5 / n, so the series converges quickly
  • answers are compared with a tolerance of 1e-6

Goals

  • Write a continuous plant as a state-space model x' = A x + B u
  • Compute the exact zero-order-hold model x[k+1] = Ad x[k] + Bd u[k] by summing a power series
  • Use numpy arrays and the @ operator for matrix products
Starting Python…