additional stochsys documentation, unit tests

murrayrm · murrayrm · commit d3e738710a19 · 2022-03-30T08:06:30.000-07:00
diff --git a/control/sisotool.py b/control/sisotool.py
@@ -215,14 +215,14 @@ def rootlocus_pid_designer(plant, gain='P', sign=+1, input_signal='r',
     derivative terms are given instead by Kp, Ki*dt/2*(z+1)/(z-1), and
     Kd/dt*(z-1)/z, respectively.
 
-        ------> C_ff ------    d
-        |                 |    |
-    r   |     e           V    V  u         y
-    ------->O---> C_f --->O--->O---> plant --->
-            ^-            ^-                |
-            |             |                 |
-            |             ----- C_b <-------|
-            ---------------------------------
+          ------> C_ff ------    d
+          |                 |    |
+      r   |     e           V    V  u         y
+      ------->O---> C_f --->O--->O---> plant --->
+              ^-            ^-                |
+              |             |                 |
+              |             ----- C_b <-------|
+              ---------------------------------
 
     It is also possible to move the derivative term into the feedback path
     `C_b` using `derivative_in_feedback_path=True`. This may be desired to
@@ -234,8 +234,8 @@ def rootlocus_pid_designer(plant, gain='P', sign=+1, input_signal='r',
 
     Remark: It may be helpful to zoom in using the magnifying glass on the
     plot. Just ake sure to deactivate magnification mode when you are done by
-    clicking the magnifying glass. Otherwise you will not be able to be able to choose
-    a gain on the root locus plot.
+    clicking the magnifying glass. Otherwise you will not be able to be able
+    to choose a gain on the root locus plot.
 
     Parameters
     ----------
@@ -269,6 +269,7 @@ def rootlocus_pid_designer(plant, gain='P', sign=+1, input_signal='r',
     ----------
     closedloop : class:`StateSpace` system
         The closed-loop system using initial gains.
+
     """
 
     plant = _convert_to_statespace(plant)
diff --git a/control/stochsys.py b/control/stochsys.py
@@ -31,7 +31,7 @@
 
 
 # contributed by Sawyer B. Fuller <minster@uw.edu>
-def lqe(*args, **keywords):
+def lqe(*args, **kwargs):
     """lqe(A, G, C, QN, RN, [, NN])
 
     Linear quadratic estimator design (Kalman filter) for continuous-time
@@ -126,18 +126,20 @@ def lqe(*args, **keywords):
     # Process the arguments and figure out what inputs we received
     #
 
+    # If we were passed a discrete time system as the first arg, use dlqe()
+    if isinstance(args[0], LTI) and isdtime(args[0], strict=True):
+        # Call dlqe
+        return dlqe(*args, **kwargs)
+
     # Get the method to use (if specified as a keyword)
-    method = keywords.get('method', None)
+    method = kwargs.pop('method', None)
+    if kwargs:
+        raise TypeError("unrecognized kwargs: ", str(kwargs))
 
     # Get the system description
     if (len(args) < 3):
         raise ControlArgument("not enough input arguments")
 
-    # If we were passed a discrete time system as the first arg, use dlqe()
-    if isinstance(args[0], LTI) and isdtime(args[0], strict=True):
-        # Call dlqe
-        return dlqe(*args, **keywords)
-
     # If we were passed a state space  system, use that to get system matrices
     if isinstance(args[0], StateSpace):
         A = np.array(args[0].A, ndmin=2, dtype=float)
@@ -179,7 +181,7 @@ def lqe(*args, **keywords):
 
 
 # contributed by Sawyer B. Fuller <minster@uw.edu>
-def dlqe(*args, **keywords):
+def dlqe(*args, **kwargs):
     """dlqe(A, G, C, QN, RN, [, N])
 
     Linear quadratic estimator design (Kalman filter) for discrete-time
@@ -251,7 +253,9 @@ def dlqe(*args, **keywords):
     #
 
     # Get the method to use (if specified as a keyword)
-    method = keywords.get('method', None)
+    method = kwargs.pop('method', None)
+    if kwargs:
+        raise TypeError("unrecognized kwargs: ", str(kwargs))
 
     # Get the system description
     if (len(args) < 3):
@@ -340,14 +344,14 @@ def create_estimator_iosystem(
         If the system has all full states output, define the measured values
         to be used by the estimator.  Otherwise, use the system output as the
         measured values.
-    {state, covariance, output}_labels : str or list of str, optional
+    {state, covariance, sensor, output}_labels : str or list of str, optional
         Set the name of the signals to use for the internal state, covariance,
-        and output (state estimate).  If a single string is specified, it
-        should be a format string using the variable ``i`` as an index (or
-        ``i`` and ``j`` for covariance).  Otherwise, a list of strings
-        matching the size of the respective signal should be used.  Default is
-        ``'xhat[{i}]'`` for state and output labels and ``'P[{i},{j}]'`` for
-        covariance labels.
+        sensors, and outputs (state estimate).  If a single string is
+        specified, it should be a format string using the variable ``i`` as an
+        index (or ``i`` and ``j`` for covariance).  Otherwise, a list of
+        strings matching the size of the respective signal should be used.
+        Default is ``'xhat[{i}]'`` for state and output labels, ``'y[{i}]'``
+        for output labels and ``'P[{i},{j}]'`` for covariance labels.
 
     Returns
     -------
@@ -478,6 +482,10 @@ def white_noise(T, Q, dt=0):
     sample time).
 
     """
+    # Convert input arguments to arrays
+    T = np.atleast_1d(T)
+    Q = np.atleast_2d(Q)
+
     # Check the shape of the input arguments
     if len(T.shape) != 1:
         raise ValueError("Time vector T must be 1D")
@@ -502,7 +510,37 @@ def white_noise(T, Q, dt=0):
     # Return a linear combination of the noise sources
     return sp.linalg.sqrtm(Q) @ W
 
-def correlation(T, X, Y=None, dt=0, squeeze=True):
+def correlation(T, X, Y=None, squeeze=True):
+    """Compute the correlation of time signals.
+
+    For a time series X(t) (and optionally Y(t)), the correlation() function
+    computes the correlation matrix E(X'(t+tau) X(t)) or the cross-correlation
+    matrix E(X'(t+tau) Y(t)]:
+
+      tau, Rtau = correlation(T, X[, Y])
+
+    The signal X (and Y, if present) represent a continuous time signal
+    sampled at times T.  The return value provides the correlation Rtau
+    between X(t+tau) and X(t) at a set of time offets tau.
+
+    Parameters
+    ----------
+    T : 1D array_like
+        Sample times for the signal(s).
+    X : 1D or 2D array_like
+        Values of the signal at each time in T.  The signal can either be
+        scalar or vector values.
+    Y : 1D or 2D array_like, optional
+        If present, the signal with which to compute the correlation.
+        Defaults to X.
+    squeeze : bool, optional
+        If True, squeeze Rtau to remove extra dimensions (useful if the
+        signals are scalars).
+
+    Returns
+    -------
+
+    """
     T = np.atleast_1d(T)
     X = np.atleast_2d(X)
     Y = np.atleast_2d(Y) if Y is not None else X
@@ -516,10 +554,7 @@ def correlation(T, X, Y=None, dt=0, squeeze=True):
         raise ValueError("Signals X and Y must have same length as T")
 
     # Figure out the time increment
-    if dt != 0:
-        raise NotImplementedError("Discrete time systems not yet supported")
-    else:
-        dt = T[1] - T[0]
+    dt = T[1] - T[0]
 
     # Make sure data points are equally spaced
     if not np.allclose(np.diff(T), T[1] - T[0]):
diff --git a/control/tests/stochsys_test.py b/control/tests/stochsys_test.py
@@ -13,18 +13,18 @@ def check_LQE(L, P, poles, G, QN, RN):
     P_expected = asmatarrayout(np.sqrt(G @ QN @ G @ RN))
     L_expected = asmatarrayout(P_expected / RN)
     poles_expected = -np.squeeze(np.asarray(L_expected))
-    np.testing.assert_array_almost_equal(P, P_expected)
-    np.testing.assert_array_almost_equal(L, L_expected)
-    np.testing.assert_array_almost_equal(poles, poles_expected)
+    np.testing.assert_almost_equal(P, P_expected)
+    np.testing.assert_almost_equal(L, L_expected)
+    np.testing.assert_almost_equal(poles, poles_expected)
 
 # Utility function to check discrete LQE solutions
 def check_DLQE(L, P, poles, G, QN, RN):
     P_expected = asmatarrayout(G.dot(QN).dot(G))
     L_expected = asmatarrayout(0)
     poles_expected = -np.squeeze(np.asarray(L_expected))
-    np.testing.assert_array_almost_equal(P, P_expected)
-    np.testing.assert_array_almost_equal(L, L_expected)
-    np.testing.assert_array_almost_equal(poles, poles_expected)
+    np.testing.assert_almost_equal(P, P_expected)
+    np.testing.assert_almost_equal(L, L_expected)
+    np.testing.assert_almost_equal(poles, poles_expected)
 
 @pytest.mark.parametrize("method", [None, 'slycot', 'scipy'])
 def test_LQE(matarrayin, method):
@@ -51,9 +51,9 @@ def test_lqe_call_format(cdlqe):
     
     # Call with system instead of matricees
     L, P, E = cdlqe(sys, Q, R)
-    np.testing.assert_array_almost_equal(Lref, L)
-    np.testing.assert_array_almost_equal(Pref, P)
-    np.testing.assert_array_almost_equal(Eref, E)
+    np.testing.assert_almost_equal(Lref, L)
+    np.testing.assert_almost_equal(Pref, P)
+    np.testing.assert_almost_equal(Eref, E)
 
     # Make sure we get an error if we specify N
     with pytest.raises(ct.ControlNotImplemented):
@@ -156,10 +156,10 @@ def test_estimator_iosys():
     # Check to make sure everything matches
     cls = clsys.linearize(0, 0)
     nstates = sys.nstates
-    np.testing.assert_array_almost_equal(cls.A[:2*nstates, :2*nstates], A_clchk)
-    np.testing.assert_array_almost_equal(cls.B[:2*nstates, :], B_clchk)
-    np.testing.assert_array_almost_equal(cls.C[:, :2*nstates], C_clchk)
-    np.testing.assert_array_almost_equal(cls.D, D_clchk)
+    np.testing.assert_almost_equal(cls.A[:2*nstates, :2*nstates], A_clchk)
+    np.testing.assert_almost_equal(cls.B[:2*nstates, :], B_clchk)
+    np.testing.assert_almost_equal(cls.C[:, :2*nstates], C_clchk)
+    np.testing.assert_almost_equal(cls.D, D_clchk)
 
 
 def test_estimator_errors():
@@ -185,3 +185,78 @@ def test_estimator_errors():
         sys_fs.C = np.eye(4)
         C = np.eye(1, 4)
         estim = ct.create_estimator_iosystem(sys_fs, QN, RN, C=C)
+
+
+def test_white_noise():
+    # Scalar white noise signal
+    T = np.linspace(0, 1000, 1000)
+    R = 0.5
+    V = ct.white_noise(T, R)
+    assert abs(np.mean(V)) < 0.1                # can occassionally fail
+    assert abs(np.cov(V) - 0.5) < 0.1           # can occassionally fail
+
+    # Vector white noise signal
+    R = [[0.5, 0], [0, 0.1]]
+    V = ct.white_noise(T, R)
+    assert abs(np.mean(V)) < 0.1                # can occassionally fail
+    assert np.all(abs(np.cov(V) - R) < 0.1)     # can occassionally fail
+
+    # Make sure time scaling works properly
+    T = T / 10
+    V = ct.white_noise(T, R)
+    assert abs(np.mean(V)) < np.sqrt(10)        # can occassionally fail
+    assert np.all(abs(np.cov(V) - R) < 10)      # can occassionally fail
+
+    # Make sure discrete time works properly
+    V = ct.white_noise(T, R, dt=T[1] - T[0])
+    assert abs(np.mean(V)) < 0.1                # can occassionally fail
+    assert np.all(abs(np.cov(V) - R) < 0.1)     # can occassionally fail
+
+    # Test error conditions
+    with pytest.raises(ValueError, match="T must be 1D"):
+        V = ct.white_noise(R, R)
+
+    with pytest.raises(ValueError, match="Q must be square"):
+        R = np.outer(np.eye(2, 3), np.ones_like(T))
+        V = ct.white_noise(T, R)
+
+    with pytest.raises(ValueError, match="Time values must be equally"):
+        T = np.logspace(0, 2, 100)
+        R = [[0.5, 0], [0, 0.1]]
+        V = ct.white_noise(T, R)
+
+
+def test_correlation():
+    # Create an uncorrelated random sigmal
+    T = np.linspace(0, 1000, 1000)
+    R = 0.5
+    V = ct.white_noise(T, R)
+
+    # Compute the correlation
+    tau, Rtau = ct.correlation(T, V)
+
+    # Make sure the correlation makes sense
+    zero_index = np.where(tau == 0)
+    np.testing.assert_almost_equal(Rtau[zero_index], np.cov(V), decimal=2)
+    for i, t in enumerate(tau):
+        if i == zero_index:
+            continue
+    assert abs(Rtau[i]) < 0.01
+
+    # Try passing a second argument
+    tau, Rneg = ct.correlation(T, V, -V)
+    np.testing.assert_equal(Rtau, -Rneg)
+    
+    # Test error conditions
+    with pytest.raises(ValueError, match="Time vector T must be 1D"):
+        tau, Rtau = ct.correlation(V, V)
+
+    with pytest.raises(ValueError, match="X and Y must be 2D"):
+        tau, Rtau = ct.correlation(T, np.zeros((3, T.size, 2)))
+
+    with pytest.raises(ValueError, match="X and Y must have same length as T"):
+        tau, Rtau = ct.correlation(T, V[:, 0:-1])
+
+    with pytest.raises(ValueError, match="Time values must be equally"):
+        T = np.logspace(0, 2, T.size)
+        tau, Rtau = ct.correlation(T, V)
diff --git a/doc/control.rst b/doc/control.rst
@@ -108,10 +108,11 @@ Control system synthesis
    :toctree: generated/
 
     acker
+    create_statefbk_iosystem
+    dlqr
     h2syn
     hinfsyn
     lqr
-    lqe
     mixsyn
     place
     rootlocus_pid_designer
@@ -143,6 +144,17 @@ Nonlinear system support
     tf2io
     flatsys.point_to_point
 
+Stochastic system support
+=========================
+.. autosummary::
+   :toctree: generated/
+
+    correlation
+    create_estimator_iosystem
+    dlqe
+    lqe
+    white_noise
+
 .. _utility-and-conversions:
 
 Utility functions and conversions
