added methods dynamics and output to StateSpace

sawyerbfuller · sawyerbfuller · commit f701b758151a · 2021-02-16T12:24:33.000-08:00
diff --git a/control/statesp.py b/control/statesp.py
@@ -1215,9 +1215,77 @@ def dcgain(self):
 
     def _isstatic(self):
         """True if and only if the system has no dynamics, that is,
-        if A and B are zero. """
+        if `self.A` and `self.B` are zero.
+        """
         return not np.any(self.A) and not np.any(self.B)
 
+    def dynamics(self, x, u=None):
+        """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[k+1] = A x[k] + B u[k]
+
+        The inputs `x` and `u` must be of the correct length.
+
+        Parameters
+        ----------
+        x : array_like
+            current state
+        u : array_like
+            input (optional)
+
+        Returns
+        -------
+        dx/dt or x[k+1] : ndarray
+        """
+
+        if len(np.atleast_1d(x)) != self.nstates:
+            raise ValueError("len(x) must be equal to number of states")
+        if u is not None:
+            if len(np.atleast_1d(u)) != self.ninputs:
+                raise ValueError("len(u) must be equal to number of inputs")
+
+        return self.A.dot(x) if u is None else self.A.dot(x) + self.B.dot(u)
+
+    def output(self, x, u=None):
+        """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 inputs
+        `x` and `u` must be of the correct length.
+
+        Parameters
+        ----------
+        x : array_like
+            current state
+        u : array_like
+            input (optional)
+
+        Returns
+        -------
+        y : ndarray
+        """
+        if len(np.atleast_1d(x)) != self.nstates:
+            raise ValueError("len(x) must be equal to number of states")
+        if u is not None:
+            if len(np.atleast_1d(u)) != self.ninputs:
+                raise ValueError("len(u) must be equal to number of inputs")
+
+        return self.C.dot(x) if u is None else self.C.dot(x) + self.D.dot(u)
+
+
+
 
 # TODO: add discrete time check
 def _convert_to_statespace(sys, **kw):
diff --git 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_allclose, assert_array_almost_equal
 import pytest
 import operator
 from numpy.linalg import solve
@@ -42,14 +43,26 @@ def sys322ABCD(self):
                 [0., 1.]]
         return (A322, B322, C322, D322)
 
+
+    @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 sys322(self, sys322ABCD):
-        """3-states square system (2 inputs x 2 outputs)"""
+        """3-state square system (2 inputs x 2 outputs)"""
         return StateSpace(*sys322ABCD)
 
     @pytest.fixture
     def sys222(self):
-        """2-states square system (2 inputs x 2 outputs)"""
+        """2-state square system (2 inputs x 2 outputs)"""
         A222 = [[4., 1.],
                 [2., -3]]
         B222 = [[5., 2.],
@@ -744,6 +757,45 @@ 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', [None, 0, 1, np.atleast_1d(2)])
+    def test_dynamics_output_siso(self, x, u, sys121):
+        assert_array_almost_equal(
+            sys121.dynamics(x, u),
+            sys121.A.dot(x) + (0 if u is None else sys121.B.dot(u)))
+        assert_array_almost_equal(
+            sys121.output(x, u),
+            sys121.C.dot(x) + (0 if u is None else sys121.D.dot(u)))
+
+    # too few and too many states and inputs
+    @pytest.mark.parametrize('x', [0, 1, [], [1, 2, 3], np.atleast_1d(2)])
+    @pytest.mark.parametrize('u', [None, [1, 1], np.atleast_1d((2, 2))])
+    def test_dynamics_output_siso_fails(self, x, u, sys121):
+        with pytest.raises(ValueError):
+            sys121.dynamics(x, u)
+        with pytest.raises(ValueError):
+            sys121.output(x, u)
+
+    @pytest.mark.parametrize('x',[[1, 1], [[1], [1]], np.atleast_2d([1,1]).T])
+    @pytest.mark.parametrize('u',
+        [None, [1, 1], [[1], [1]], np.atleast_2d([1,1]).T])
+    def test_dynamics_output_mimo(self, x, u, sys222):
+        assert_array_almost_equal(
+            sys222.dynamics(x, u),
+            sys222.A.dot(x) + (0 if u is None else sys222.B.dot(u)))
+        assert_array_almost_equal(
+            sys222.output(x, u),
+            sys222.C.dot(x) + (0 if u is None else sys222.D.dot(u)))
+
+    # too few and too many states and inputs
+    @pytest.mark.parametrize('x', [0, 1, [1, 1, 1]])
+    @pytest.mark.parametrize('u', [None, 0, 1, [1, 1, 1]])
+    def test_dynamics_mimo_fails(self, x, u, sys222):
+        with pytest.raises(ValueError):
+            sys222.dynamics(x, u)
+        with pytest.raises(ValueError):
+            sys222.output(x, u)
+
 class TestRss:
     """These are tests for the proper functionality of statesp.rss."""
 
