diff --git a/control/iosys.py b/control/iosys.py index 7ed4c8b05..e28de59f2 100644 --- a/control/iosys.py +++ b/control/iosys.py @@ -358,26 +358,88 @@ def _update_params(self, params, warning=False): if (warning): warn("Parameters passed to InputOutputSystem ignored.") - def _rhs(self, t, x, u): + def _rhs(self, t, x, u, params={}): """Evaluate right hand side of a differential or difference equation. Private function used to compute the right hand side of an - input/output system model. + input/output system model. Intended for fast + evaluation; for a more user-friendly interface + you may want to use :meth:`dynamics`. """ NotImplemented("Evaluation not implemented for system of type ", type(self)) + def dynamics(self, t, x, u): + """Compute the dynamics of a differential or difference equation. + + Given time `t`, input `u` and state `x`, returns the value of the + right hand side of the dynamical system. If the system is continuous, + returns the time derivative + + dx/dt = f(t, x, u) + + where `f` is the system's (possibly nonlinear) dynamics function. + If the system is discrete-time, returns the next value of `x`: + + x[t+dt] = f(t, x[t], u[t]) + + Where `t` is a scalar. + + The inputs `x` and `u` must be of the correct length. + + Parameters + ---------- + t : float + the time at which to evaluate + x : array_like + current state + u : array_like + input + + Returns + ------- + dx/dt or x[t+dt] : ndarray + """ + return self._rhs(t, x, u) + def _out(self, t, x, u, params={}): """Evaluate the output of a system at a given state, input, and time Private function used to compute the output of of an input/output - system model given the state, input, parameters, and time. + system model given the state, input, parameters. Intended for fast + evaluation; for a more user-friendly interface you may want to use + :meth:`output`. """ # If no output function was defined in subclass, return state return x + def output(self, t, x, u): + """Compute the output of the system + + Given time `t`, input `u` and state `x`, returns the output of the + system: + + y = g(t, x, u) + + The inputs `x` and `u` must be of the correct length. + + Parameters + ---------- + t : float + the time at which to evaluate + x : array_like + current state + u : array_like + input + + Returns + ------- + y : ndarray + """ + return self._out(t, x, u) + def set_inputs(self, inputs, prefix='u'): """Set the number/names of the system inputs. diff --git a/control/statesp.py b/control/statesp.py index d2b613024..758b91ed9 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -1227,12 +1227,101 @@ def dcgain(self, warn_infinite=False): return self(0, warn_infinite=warn_infinite) if self.isctime() \ else self(1, warn_infinite=warn_infinite) + def dynamics(self, t, x, u=0): + """Compute the dynamics of the system + + Given input `u` and state `x`, returns the dynamics of the state-space + system. If the system is continuous, returns the time derivative dx/dt + + dx/dt = A x + B u + + where A and B are the state-space matrices of the system. If the + system is discrete-time, returns the next value of `x`: + + x[t+dt] = A x[t] + B u[t] + + The inputs `x` and `u` must be of the correct length for the system. + + The first argument `t` is ignored because :class:`StateSpace` systems + are time-invariant. It is included so that the dynamics can be passed + to most numerical integrators, such as :func:`scipy.integrate.solve_ivp` + and for consistency with :class:`IOSystem` systems. + + Parameters + ---------- + t : float (ignored) + time + x : array_like + current state + u : array_like (optional) + input, zero if omitted + + Returns + ------- + dx/dt or x[t+dt] : ndarray + """ + x = np.reshape(x, (-1, 1)) # force to a column in case matrix + if np.size(x) != self.nstates: + raise ValueError("len(x) must be equal to number of states") + if u is 0: + return self.A.dot(x).reshape((-1,)) # return as row vector + else: # received t, x, and u, ignore t + u = np.reshape(u, (-1, 1)) # force to a column in case matrix + if np.size(u) != self.ninputs: + raise ValueError("len(u) must be equal to number of inputs") + return self.A.dot(x).reshape((-1,)) \ + + self.B.dot(u).reshape((-1,)) # return as row vector + + def output(self, t, x, u=0): + """Compute the output of the system + + Given input `u` and state `x`, returns the output `y` of the + state-space system: + + y = C x + D u + + where A and B are the state-space matrices of the system. + + The first argument `t` is ignored because :class:`StateSpace` systems + are time-invariant. It is included so that the dynamics can be passed + to most numerical integrators, such as scipy's `integrate.solve_ivp` and + for consistency with :class:`IOSystem` systems. + + The inputs `x` and `u` must be of the correct length for the system. + + Parameters + ---------- + t : float (ignored) + time + x : array_like + current state + u : array_like (optional) + input (zero if omitted) + + Returns + ------- + y : ndarray + """ + x = np.reshape(x, (-1, 1)) # force to a column in case matrix + if np.size(x) != self.nstates: + raise ValueError("len(x) must be equal to number of states") + + if u is 0: + return self.C.dot(x).reshape((-1,)) # return as row vector + else: # received t, x, and u, ignore t + u = np.reshape(u, (-1, 1)) # force to a column in case matrix + if np.size(u) != self.ninputs: + raise ValueError("len(u) must be equal to number of inputs") + return self.C.dot(x).reshape((-1,)) \ + + self.D.dot(u).reshape((-1,)) # return as row vector + def _isstatic(self): """True if and only if the system has no dynamics, that is, if A and B are zero. """ return not np.any(self.A) and not np.any(self.B) + # TODO: add discrete time check def _convert_to_statespace(sys, **kw): """Convert a system to state space form (if needed). diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py index 983b9d7a6..1eec5eadb 100644 --- a/control/tests/statesp_test.py +++ b/control/tests/statesp_test.py @@ -8,6 +8,7 @@ """ import numpy as np +from numpy.testing import assert_array_almost_equal import pytest import operator from numpy.linalg import solve @@ -47,6 +48,17 @@ def sys322(self, sys322ABCD): """3-states square system (2 inputs x 2 outputs)""" return StateSpace(*sys322ABCD) + @pytest.fixture + def sys121(self): + """2 state, 1 input, 1 output (siso) system""" + A121 = [[4., 1.], + [2., -3]] + B121 = [[5.], + [-3.]] + C121 = [[2., -4]] + D121 = [[3.]] + return StateSpace(A121, B121, C121, D121) + @pytest.fixture def sys222(self): """2-states square system (2 inputs x 2 outputs)""" @@ -751,6 +763,70 @@ def test_horner(self, sys322): np.squeeze(sys322.horner(1.j)), mag[:, :, 0] * np.exp(1.j * phase[:, :, 0])) + @pytest.mark.parametrize('x', + [[1, 1], [[1], [1]], np.atleast_2d([1,1]).T]) + @pytest.mark.parametrize('u', [0, 1, np.atleast_1d(2)]) + def test_dynamics_and_output_siso(self, x, u, sys121): + assert_array_almost_equal( + sys121.dynamics(0, x, u), + sys121.A.dot(x).reshape((-1,)) + sys121.B.dot(u).reshape((-1,))) + assert_array_almost_equal( + sys121.output(0, x, u), + sys121.C.dot(x).reshape((-1,)) + sys121.D.dot(u).reshape((-1,))) + assert_array_almost_equal( + sys121.dynamics(0, x), + sys121.A.dot(x).reshape((-1,))) + assert_array_almost_equal( + sys121.output(0, x), + sys121.C.dot(x).reshape((-1,))) + + # too few and too many states and inputs + @pytest.mark.parametrize('x', [0, 1, [], [1, 2, 3], np.atleast_1d(2)]) + def test_error_x_dynamics_and_output_siso(self, x, sys121): + with pytest.raises(ValueError): + sys121.dynamics(0, x) + with pytest.raises(ValueError): + sys121.output(0, x) + @pytest.mark.parametrize('u', [[1, 1], np.atleast_1d((2, 2))]) + def test_error_u_dynamics_output_siso(self, u, sys121): + with pytest.raises(ValueError): + sys121.dynamics(0, 1, u) + with pytest.raises(ValueError): + sys121.output(0, 1, u) + + @pytest.mark.parametrize('x', + [[1, 1], [[1], [1]], np.atleast_2d([1,1]).T]) + @pytest.mark.parametrize('u', + [[1, 1], [[1], [1]], np.atleast_2d([1,1]).T]) + def test_dynamics_and_output_mimo(self, x, u, sys222): + assert_array_almost_equal( + sys222.dynamics(0, x, u), + sys222.A.dot(x).reshape((-1,)) + sys222.B.dot(u).reshape((-1,))) + assert_array_almost_equal( + sys222.output(0, x, u), + sys222.C.dot(x).reshape((-1,)) + sys222.D.dot(u).reshape((-1,))) + assert_array_almost_equal( + sys222.dynamics(0, x), + sys222.A.dot(x).reshape((-1,))) + assert_array_almost_equal( + sys222.output(0, x), + sys222.C.dot(x).reshape((-1,))) + + # too few and too many states and inputs + @pytest.mark.parametrize('x', [0, 1, [1, 1, 1]]) + def test_error_x_dynamics_mimo(self, x, sys222): + with pytest.raises(ValueError): + sys222.dynamics(0, x) + with pytest.raises(ValueError): + sys222.output(0, x) + @pytest.mark.parametrize('u', [1, [1, 1, 1]]) + def test_error_u_dynamics_mimo(self, u, sys222): + with pytest.raises(ValueError): + sys222.dynamics(0, (1, 1), u) + with pytest.raises(ValueError): + sys222.output(0, (1, 1), u) + + class TestRss: """These are tests for the proper functionality of statesp.rss."""