diff --git a/control/modelsimp.py b/control/modelsimp.py index 06c3d350d..685529d0f 100644 --- a/control/modelsimp.py +++ b/control/modelsimp.py @@ -43,11 +43,11 @@ # External packages and modules import numpy as np import warnings -from .exception import ControlSlycot, ControlMIMONotImplemented, \ - ControlDimension +from .exception import ControlSlycot, ControlArgument, ControlDimension from .iosys import isdtime, isctime from .statesp import StateSpace from .statefbk import gram +from .timeresp import TimeResponseData __all__ = ['hsvd', 'balred', 'modred', 'era', 'markov', 'minreal'] @@ -402,41 +402,66 @@ def era(YY, m, n, nin, nout, r): raise NotImplementedError('This function is not implemented yet.') -def markov(Y, U, m=None, transpose=False): - """Calculate the first `m` Markov parameters [D CB CAB ...] - from input `U`, output `Y`. +def markov(*args, m=None, transpose=False, dt=None, truncate=False): + """markov(Y, U, [, m]) + + Calculate the first `m` Markov parameters [D CB CAB ...] from data. - This function computes the Markov parameters for a discrete time system + This function computes the Markov parameters for a discrete time + system .. math:: x[k+1] &= A x[k] + B u[k] \\\\ y[k] &= C x[k] + D u[k] - given data for u and y. The algorithm assumes that that C A^k B = 0 for - k > m-2 (see [1]_). Note that the problem is ill-posed if the length of - the input data is less than the desired number of Markov parameters (a - warning message is generated in this case). + given data for u and y. The algorithm assumes that that C A^k B = 0 + for k > m-2 (see [1]_). Note that the problem is ill-posed if the + length of the input data is less than the desired number of Markov + parameters (a warning message is generated in this case). + + The function can be called with either 1, 2 or 3 arguments: + + * ``H = markov(data)`` + * ``H = markov(data, m)`` + * ``H = markov(Y, U)`` + * ``H = markov(Y, U, m)`` + + where `data` is a `TimeResponseData` object, `YY` is a 1D or 3D + array, and r is an integer. Parameters ---------- Y : array_like - Output data. If the array is 1D, the system is assumed to be single - input. If the array is 2D and transpose=False, the columns of `Y` - are taken as time points, otherwise the rows of `Y` are taken as - time points. + Output data. If the array is 1D, the system is assumed to be + single input. If the array is 2D and transpose=False, the columns + of `Y` are taken as time points, otherwise the rows of `Y` are + taken as time points. U : array_like Input data, arranged in the same way as `Y`. + data : TimeResponseData + Response data from which the Markov parameters where estimated. + Input and output data must be 1D or 2D array. m : int, optional - Number of Markov parameters to output. Defaults to len(U). + Number of Markov parameters to output. Defaults to len(U). + dt : True of float, optional + True indicates discrete time with unspecified sampling time and a + positive float is discrete time with the specified sampling time. + It can be used to scale the Markov parameters in order to match + the unit-area impulse response of python-control. Default is True + for array_like and dt=data.time[1]-data.time[0] for + TimeResponseData as input. + truncate : bool, optional + Do not use first m equation for least squares. Default is False. transpose : bool, optional Assume that input data is transposed relative to the standard - :ref:`time-series-convention`. Default value is False. + :ref:`time-series-convention`. For TimeResponseData this parameter + is ignored. Default is False. Returns ------- H : ndarray - First m Markov parameters, [D CB CAB ...] + First m Markov parameters, [D CB CAB ...]. References ---------- @@ -445,15 +470,6 @@ def markov(Y, U, m=None, transpose=False): and experiments. Journal of Guidance Control and Dynamics, 16(2), 320-329, 2012. http://doi.org/10.2514/3.21006 - Notes - ----- - Currently only works for SISO systems. - - This function does not currently comply with the Python Control Library - :ref:`time-series-convention` for representation of time series data. - Use `transpose=False` to make use of the standard convention (this - will be updated in a future release). - Examples -------- >>> T = np.linspace(0, 10, 100) @@ -462,78 +478,89 @@ def markov(Y, U, m=None, transpose=False): >>> H = ct.markov(Y, U, 3, transpose=False) """ - # Convert input parameters to 2D arrays (if they aren't already) - Umat = np.array(U, ndmin=2) - Ymat = np.array(Y, ndmin=2) - - # If data is in transposed format, switch it around - if transpose: - Umat, Ymat = np.transpose(Umat), np.transpose(Ymat) - # Make sure the system is a SISO system - if Umat.shape[0] != 1 or Ymat.shape[0] != 1: - raise ControlMIMONotImplemented + # Convert input parameters to 2D arrays (if they aren't already) + # Get the system description + if len(args) < 1: + raise ControlArgument("not enough input arguments") + + if isinstance(args[0], TimeResponseData): + data = args[0] + Umat = np.array(data.inputs, ndmin=2) + Ymat = np.array(data.outputs, ndmin=2) + if dt is None: + dt = data.time[1] - data.time[0] + if not np.allclose(np.diff(data.time), dt): + raise ValueError("response time values must be equally " + "spaced.") + transpose = data.transpose + if data.transpose and not data.issiso: + Umat, Ymat = np.transpose(Umat), np.transpose(Ymat) + if len(args) == 2: + m = args[1] + elif len(args) > 2: + raise ControlArgument("too many positional arguments") + else: + if len(args) < 2: + raise ControlArgument("not enough input arguments") + Umat = np.array(args[1], ndmin=2) + Ymat = np.array(args[0], ndmin=2) + if dt is None: + dt = True + if transpose: + Umat, Ymat = np.transpose(Umat), np.transpose(Ymat) + if len(args) == 3: + m = args[2] + elif len(args) > 3: + raise ControlArgument("too many positional arguments") # Make sure the number of time points match if Umat.shape[1] != Ymat.shape[1]: raise ControlDimension( "Input and output data are of differnent lengths") - n = Umat.shape[1] + l = Umat.shape[1] # If number of desired parameters was not given, set to size of input data if m is None: - m = Umat.shape[1] + m = l + + t = 0 + if truncate: + t = m + + q = Ymat.shape[0] # number of outputs + p = Umat.shape[0] # number of inputs # Make sure there is enough data to compute parameters - if m > n: + if m*p > (l-t): warnings.warn("Not enough data for requested number of parameters") + # the algorithm - Construct a matrix of control inputs to invert # - # Original algorithm (with mapping to standard order) - # - # RMM note, 24 Dec 2020: This algorithm sets the problem up correctly - # until the final column of the UU matrix is created, at which point it - # makes some modifications that I don't understand. This version of the - # algorithm does not seem to return the actual Markov parameters for a - # system. - # - # # Create the matrix of (shifted) inputs - # UU = np.transpose(Umat) - # for i in range(1, m-1): - # # Shift previous column down and add a zero at the top - # newCol = np.vstack((0, np.reshape(UU[0:n-1, i-1], (-1, 1)))) - # UU = np.hstack((UU, newCol)) - # - # # Shift previous column down and add a zero at the top - # Ulast = np.vstack((0, np.reshape(UU[0:n-1, m-2], (-1, 1)))) - # - # # Replace the elements of the last column new values (?) - # # Each row gets the sum of the rows above it (?) - # for i in range(n-1, 0, -1): - # Ulast[i] = np.sum(Ulast[0:i-1]) - # UU = np.hstack((UU, Ulast)) - # - # # Solve for the Markov parameters from Y = H @ UU - # # H = [[D], [CB], [CAB], ..., [C A^{m-3} B], [???]] - # H = np.linalg.lstsq(UU, np.transpose(Ymat))[0] - # - # # Markov parameters are in rows => transpose if needed - # return H if transpose else np.transpose(H) - - # - # New algorithm - Construct a matrix of control inputs to invert + # (q,l) = (q,p*m) @ (p*m,l) + # YY.T = H @ UU.T # # This algorithm sets up the following problem and solves it for # the Markov parameters # + # (l,q) = (l,p*m) @ (p*m,q) + # YY = UU @ H.T + # # [ y(0) ] [ u(0) 0 0 ] [ D ] # [ y(1) ] [ u(1) u(0) 0 ] [ C B ] # [ y(2) ] = [ u(2) u(1) u(0) ] [ C A B ] # [ : ] [ : : : : ] [ : ] - # [ y(n-1) ] [ u(n-1) u(n-2) u(n-3) ... u(n-m) ] [ C A^{m-2} B ] + # [ y(l-1) ] [ u(l-1) u(l-2) u(l-3) ... u(l-m) ] [ C A^{m-2} B ] + # + # truncated version t=m, do not use first m equation + # + # [ y(t) ] [ u(t) u(t-1) u(t-2) u(t-m) ] [ D ] + # [ y(t+1) ] [ u(t+1) u(t) u(t-1) u(t-m+1)] [ C B ] + # [ y(t+2) ] = [ u(t+2) u(t+1) u(t) u(t-m+2)] [ C B ] + # [ : ] [ : : : : ] [ : ] + # [ y(l-1) ] [ u(l-1) u(l-2) u(l-3) ... u(l-m) ] [ C A^{m-2} B ] # - # Note: if the number of Markov parameters (m) is less than the size of - # the input/output data (n), then this algorithm assumes C A^{j} B = 0 + # Note: This algorithm assumes C A^{j} B = 0 # for j > m-2. See equation (3) in # # J.-N. Juang, M. Phan, L. G. Horta, and R. W. Longman, Identification @@ -542,17 +569,27 @@ def markov(Y, U, m=None, transpose=False): # 320-329, 2012. http://doi.org/10.2514/3.21006 # + # Set up the full problem # Create matrix of (shifted) inputs - UU = Umat - for i in range(1, m): - # Shift previous column down and add a zero at the top - new_row = np.hstack((0, UU[i-1, 0:-1])) - UU = np.vstack((UU, new_row)) - UU = np.transpose(UU) - - # Invert and solve for Markov parameters - YY = np.transpose(Ymat) - H, _, _, _ = np.linalg.lstsq(UU, YY, rcond=None) + UUT = np.zeros((p*m,(l))) + for i in range(m): + # Shift previous column down and keep zeros at the top + UUT[i*p:(i+1)*p,i:] = Umat[:,:l-i] + + # Truncate first t=0 or t=m time steps, transpose the problem for lsq + YY = Ymat[:,t:].T + UU = UUT[:,t:].T + + # Solve for the Markov parameters from YY = UU @ H.T + HT, _, _, _ = np.linalg.lstsq(UU, YY, rcond=None) + H = HT.T/dt # scaling + + H = H.reshape(q,m,p) # output, time*input -> output, time, input + H = H.transpose(0,2,1) # output, input, time + + # for siso return a 1D array instead of a 3D array + if q == 1 and p == 1: + H = np.squeeze(H) # Return the first m Markov parameters - return H if transpose else np.transpose(H) + return H if not transpose else np.transpose(H) diff --git a/control/tests/modelsimp_test.py b/control/tests/modelsimp_test.py index 49c2afd58..88cc93a75 100644 --- a/control/tests/modelsimp_test.py +++ b/control/tests/modelsimp_test.py @@ -7,8 +7,8 @@ import pytest -from control import StateSpace, forced_response, tf, rss, c2d -from control.exception import ControlMIMONotImplemented +from control import StateSpace, forced_response, impulse_response, tf, rss, c2d, TimeResponseData +from control.exception import ControlArgument, ControlDimension from control.tests.conftest import slycotonly from control.modelsimp import balred, hsvd, markov, modred @@ -33,36 +33,149 @@ def testHSVD(self): assert not isinstance(hsv, np.matrix) def testMarkovSignature(self): - U = np.array([[1., 1., 1., 1., 1.]]) + U = np.array([[1., 1., 1., 1., 1., 1., 1.]]) Y = U + response = TimeResponseData(time=np.arange(U.shape[-1]), + outputs=Y, + output_labels='y', + inputs=U, + input_labels='u', + ) + + # setup m = 3 - H = markov(Y, U, m, transpose=False) - Htrue = np.array([[1., 0., 0.]]) - np.testing.assert_array_almost_equal(H, Htrue) + Htrue = np.array([1., 0., 0.]) + Htrue_l = np.array([1., 0., 0., 0., 0., 0., 0.]) + + # test not enough input arguments + with pytest.raises(ControlArgument): + H = markov(Y) + with pytest.raises(ControlArgument): + H = markov() + + # to many positional arguments + with pytest.raises(ControlArgument): + H = markov(Y,U,m,1) + with pytest.raises(ControlArgument): + H = markov(response,m,1) + + # to many positional arguments + with pytest.raises(ControlDimension): + U2 = np.hstack([U,U]) + H = markov(Y,U2,m) + + # not enough data + with pytest.warns(Warning): + H = markov(Y,U,8) - # Make sure that transposed data also works - H = markov(np.transpose(Y), np.transpose(U), m, transpose=True) - np.testing.assert_array_almost_equal(H, np.transpose(Htrue)) + # Basic Usage, m=l + H = markov(Y, U) + np.testing.assert_array_almost_equal(H, Htrue_l) - # Generate Markov parameters without any arguments + H = markov(response) + np.testing.assert_array_almost_equal(H, Htrue_l) + + # Basic Usage, m H = markov(Y, U, m) np.testing.assert_array_almost_equal(H, Htrue) + H = markov(response, m) + np.testing.assert_array_almost_equal(H, Htrue) + + H = markov(Y, U, m=m) + np.testing.assert_array_almost_equal(H, Htrue) + + H = markov(response, m=m) + np.testing.assert_array_almost_equal(H, Htrue) + + response.transpose=False + H = markov(response, m=m) + np.testing.assert_array_almost_equal(H, Htrue) + + # Make sure that transposed data also works, siso + HT = markov(Y.T, U.T, m, transpose=True) + np.testing.assert_array_almost_equal(HT, np.transpose(Htrue)) + + response.transpose = True + HT = markov(response, m) + np.testing.assert_array_almost_equal(HT, np.transpose(Htrue)) + response.transpose=False + + # Test example from docstring + # TODO: There is a problem here, last markov parameter does not fit + # the approximation error could be to big + Htrue = np.array([0, 1., -0.5]) T = np.linspace(0, 10, 100) U = np.ones((1, 100)) T, Y = forced_response(tf([1], [1, 0.5], True), T, U) - H = markov(Y, U, 3, transpose=False) + H = markov(Y, U, 4, dt=True) + np.testing.assert_array_almost_equal(H[:3], Htrue[:3]) + + response = forced_response(tf([1], [1, 0.5], True), T, U) + H = markov(response, 4, dt=True) + np.testing.assert_array_almost_equal(H[:3], Htrue[:3]) # Test example from issue #395 inp = np.array([1, 2]) outp = np.array([2, 4]) mrk = markov(outp, inp, 1, transpose=False) - # Make sure MIMO generates an error - U = np.ones((2, 100)) # 2 inputs (Y unchanged, with 1 output) - with pytest.raises(ControlMIMONotImplemented): - markov(Y, U, m) + # Test mimo example + # Mechanical Vibrations: Theory and Application, SI Edition, 1st ed. + # Figure 6.5 / Example 6.7 + m1, k1, c1 = 1., 4., 1. + m2, k2, c2 = 2., 2., 1. + k3, c3 = 6., 2. + + A = np.array([ + [0., 0., 1., 0.], + [0., 0., 0., 1.], + [-(k1+k2)/m1, (k2)/m1, -(c1+c2)/m1, c2/m1], + [(k2)/m2, -(k2+k3)/m2, c2/m2, -(c2+c3)/m2] + ]) + B = np.array([[0.,0.],[0.,0.],[1/m1,0.],[0.,1/m2]]) + C = np.array([[1.0, 0.0, 0.0, 0.0],[0.0, 1.0, 0.0, 0.0]]) + D = np.zeros((2,2)) + + sys = StateSpace(A, B, C, D) + dt = 0.25 + sysd = sys.sample(dt, method='zoh') + + T = np.arange(0,100,dt) + U = np.random.randn(sysd.B.shape[-1], len(T)) + response = forced_response(sysd, U=U) + Y = response.outputs + + m = 100 + _, Htrue = impulse_response(sysd, T=dt*(m-1)) + + + # test array_like + H = markov(Y, U, m, dt=dt) + np.testing.assert_array_almost_equal(H, Htrue) + + # test array_like, truncate + H = markov(Y, U, m, dt=dt, truncate=True) + np.testing.assert_array_almost_equal(H, Htrue) + + # test array_like, transpose + HT = markov(Y.T, U.T, m, dt=dt, transpose=True) + np.testing.assert_array_almost_equal(HT, np.transpose(Htrue)) + + # test response data + H = markov(response, m, dt=dt) + np.testing.assert_array_almost_equal(H, Htrue) + + # test response data + H = markov(response, m, dt=dt, truncate=True) + np.testing.assert_array_almost_equal(H, Htrue) + + # test response data, transpose + response.transpose = True + HT = markov(response, m, dt=dt) + np.testing.assert_array_almost_equal(HT, np.transpose(Htrue)) + # Make sure markov() returns the right answer @pytest.mark.parametrize("k, m, n", @@ -98,18 +211,34 @@ def testMarkovResults(self, k, m, n): Mtrue = np.hstack([Hd.D] + [ Hd.C @ np.linalg.matrix_power(Hd.A, i) @ Hd.B for i in range(m-1)]) + + Mtrue = np.squeeze(Mtrue) # Generate input/output data T = np.array(range(n)) * Ts U = np.cos(T) + np.sin(T/np.pi) - _, Y = forced_response(Hd, T, U, squeeze=True) - Mcomp = markov(Y, U, m) + + ir_true = impulse_response(Hd,T) + Mtrue_scaled = ir_true[1][:m] # Compare to results from markov() # experimentally determined probability to get non matching results # with rtot=1e-6 and atol=1e-8 due to numerical errors # for k=5, m=n=10: 0.015 % + T, Y = forced_response(Hd, T, U, squeeze=True) + Mcomp = markov(Y, U, m, dt=True) + Mcomp_scaled = markov(Y, U, m, dt=Ts) + np.testing.assert_allclose(Mtrue, Mcomp, rtol=1e-6, atol=1e-8) + np.testing.assert_allclose(Mtrue_scaled, Mcomp_scaled, rtol=1e-6, atol=1e-8) + + response = forced_response(Hd, T, U, squeeze=True) + Mcomp = markov(response, m, dt=True) + Mcomp_scaled = markov(response, m, dt=Ts) + + np.testing.assert_allclose(Mtrue, Mcomp, rtol=1e-6, atol=1e-8) + np.testing.assert_allclose(Mtrue_scaled, Mcomp_scaled, rtol=1e-6, atol=1e-8) + def testModredMatchDC(self): #balanced realization computed in matlab for the transfer function: diff --git a/doc/examples.rst b/doc/examples.rst index 21364157e..db6cbaad6 100644 --- a/doc/examples.rst +++ b/doc/examples.rst @@ -35,6 +35,7 @@ other sources. kincar-flatsys mrac_siso_mit mrac_siso_lyapunov + markov Jupyter notebooks ================= diff --git a/doc/markov.py b/doc/markov.py new file mode 120000 index 000000000..471188252 --- /dev/null +++ b/doc/markov.py @@ -0,0 +1 @@ +../examples/markov.py \ No newline at end of file diff --git a/doc/markov.rst b/doc/markov.rst new file mode 100644 index 000000000..36e0fd8e5 --- /dev/null +++ b/doc/markov.rst @@ -0,0 +1,15 @@ +Estimation of Makrov parameters +------------------------------- + +Code +.... +.. literalinclude:: markov.py + :language: python + :linenos: + + +Notes +..... + +1. The environment variable `PYCONTROL_TEST_EXAMPLES` is used for +testing to turn off plotting of the outputs.0 \ No newline at end of file diff --git a/examples/markov.py b/examples/markov.py new file mode 100644 index 000000000..6c02499bd --- /dev/null +++ b/examples/markov.py @@ -0,0 +1,108 @@ +# markov.py +# Johannes Kaisinger, 4 July 2024 +# +# Demonstrate estimation of markov parameters. +# SISO, SIMO, MISO, MIMO case + +import numpy as np +import matplotlib.pyplot as plt +import os + +import control as ct + +def create_impulse_response(H, time, transpose, dt): + """Helper function to use TimeResponseData type for plotting""" + + H = np.array(H, ndmin=3) + + if transpose: + H = np.transpose(H) + + q, p, m = H.shape + inputs = np.zeros((p,p,m)) + + issiso = True if (q == 1 and p == 1) else False + + input_labels = [] + trace_labels, trace_types = [], [] + for i in range(p): + inputs[i,i,0] = 1/dt # unit area impulse + input_labels.append(f"u{[i]}") + trace_labels.append(f"From u{[i]}") + trace_types.append('impulse') + + output_labels = [] + for i in range(q): + output_labels.append(f"y{[i]}") + + return ct.TimeResponseData(time=time[:m], + outputs=H, + output_labels=output_labels, + inputs=inputs, + input_labels=input_labels, + trace_labels=trace_labels, + trace_types=trace_types, + sysname="H_est", + transpose=transpose, + plot_inputs=False, + issiso=issiso) + +# set up a mass spring damper system (2dof, MIMO case) +# Mechanical Vibrations: Theory and Application, SI Edition, 1st ed. +# Figure 6.5 / Example 6.7 +# m q_dd + c q_d + k q = f +m1, k1, c1 = 1., 4., 1. +m2, k2, c2 = 2., 2., 1. +k3, c3 = 6., 2. + +A = np.array([ + [0., 0., 1., 0.], + [0., 0., 0., 1.], + [-(k1+k2)/m1, (k2)/m1, -(c1+c2)/m1, c2/m1], + [(k2)/m2, -(k2+k3)/m2, c2/m2, -(c2+c3)/m2] +]) +B = np.array([[0.,0.],[0.,0.],[1/m1,0.],[0.,1/m2]]) +C = np.array([[1.0, 0.0, 0.0, 0.0],[0.0, 1.0, 0.0, 0.0]]) +D = np.zeros((2,2)) + + +xixo_list = ["SISO","SIMO","MISO","MIMO"] +xixo = xixo_list[3] # choose a system for estimation +match xixo: + case "SISO": + sys = ct.StateSpace(A, B[:,0], C[0,:], D[0,0]) + case "SIMO": + sys = ct.StateSpace(A, B[:,:1], C, D[:,:1]) + case "MISO": + sys = ct.StateSpace(A, B, C[:1,:], D[:1,:]) + case "MIMO": + sys = ct.StateSpace(A, B, C, D) + +dt = 0.25 +sysd = sys.sample(dt, method='zoh') +sysd.name = "H_true" + + # random forcing input +t = np.arange(0,100,dt) +u = np.random.randn(sysd.B.shape[-1], len(t)) + +response = ct.forced_response(sysd, U=u) +response.plot() +plt.show() + +m = 50 +ir_true = ct.impulse_response(sysd, T=dt*m) + +H_est = ct.markov(response, m, dt=dt) +# Helper function for plotting only +ir_est = create_impulse_response(H_est, + ir_true.time, + ir_true.transpose, + dt) + +ir_true.plot(title=xixo) +ir_est.plot(color='orange',linestyle='dashed') +plt.show() + +if 'PYCONTROL_TEST_EXAMPLES' not in os.environ: + plt.show() \ No newline at end of file