add examples for create_{statefbk,estimator}_iosystem + related fixes

murrayrm · murrayrm · commit e82670e054db · 2025-02-02T21:19:17.000-08:00
diff --git a/control/mateqn.py b/control/mateqn.py
@@ -606,7 +606,9 @@ def _slycot_or_scipy(method):
 
 # Utility function to check matrix dimensions
 def _check_shape(M, n, m, square=False, symmetric=False, name="??"):
-    if square and M.shape[0] != M.shape[1]:
+    M = np.atleast_2d(M)
+
+    if (square or symmetric) and M.shape[0] != M.shape[1]:
         raise ControlDimension("%s must be a square matrix" % name)
 
     if symmetric and not _is_symmetric(M):
@@ -617,6 +619,8 @@ def _check_shape(M, n, m, square=False, symmetric=False, name="??"):
             f"Incompatible dimensions of {name} matrix; "
             f"expected ({n}, {m}) but found {M.shape}")
 
+    return M
+
 
 # Utility function to check if a matrix is symmetric
 def _is_symmetric(M):
diff --git a/control/nlsys.py b/control/nlsys.py
@@ -873,7 +873,7 @@ def cxn_string(signal, gain, first):
                 cxn, width=78, initial_indent=" * ",
                 subsequent_indent="     ")) + "\n"
 
-        out += "\nOutputs:\n"
+        out += "\nOutputs:"
         for i in range(len(self.output_labels)):
             first = True
             cxn = f"{self.output_labels[i]} <- "
@@ -883,9 +883,9 @@ def cxn_string(signal, gain, first):
                         cxn += cxn_string(
                             output_list[j], self.output_map[i, j], first)
                         first = False
-                out += "\n".join(textwrap.wrap(
+                out += "\n" + "\n".join(textwrap.wrap(
                     cxn, width=78, initial_indent=" * ",
-                    subsequent_indent="     ")) + "\n"
+                    subsequent_indent="     "))
 
         return out
 
diff --git a/control/statefbk.py b/control/statefbk.py
@@ -783,12 +783,12 @@ def create_statefbk_iosystem(
     if integral_action is not None:
         if not isinstance(integral_action, np.ndarray):
             raise ControlArgument("Integral action must pass an array")
-        elif integral_action.shape[1] != sys_nstates:
+
+        C = np.atleast_2d(integral_action)
+        if C.shape[1] != sys_nstates:
             raise ControlArgument(
                 "Integral gain size must match system state size")
-        else:
-            nintegrators = integral_action.shape[0]
-            C = integral_action
+        nintegrators = C.shape[0]
     else:
         # Create a C matrix with no outputs, just in case update gets called
         C = np.zeros((0, sys_nstates))
diff --git a/control/statesp.py b/control/statesp.py
@@ -1608,7 +1608,7 @@ def __call__(self, *args, **kwargs):
         return StateSpace.__call__(self, *args, **kwargs)
 
     def __str__(self):
-        string = InterconnectedSystem.__str__(self) + "\n"
+        string = InterconnectedSystem.__str__(self) + "\n\n"
         string += "\n\n".join([
             "{} = {}".format(Mvar,
                                "\n    ".join(str(M).splitlines()))
diff --git a/control/stochsys.py b/control/stochsys.py
@@ -321,15 +321,15 @@ def create_estimator_iosystem(
     .. math::
 
         d \hat{x}/dt &= A \hat{x} + B u - L (C \hat{x} - y) \\
-           dP/dt &= A P + P A^T + F Q_N F^T - P C^T R_N^{-1} C P \\
+           dP/dt &= A P + P A^T + G Q_N G^T - P C^T R_N^{-1} C P \\
                L &= P C^T R_N^{-1}
 
     or a discrete-time state estimator of the form
 
     .. math::
 
         \hat{x}[k+1] &= A \hat{x}[k] + B u[k] - L (C \hat{x}[k] - y[k]) \\
-               P[k+1] &= A P A^T + F Q_N F^T - A P C^T R_e^{-1} C P A \\
+               P[k+1] &= A P A^T + G Q_N G^T - A P C^T R_e^{-1} C P A \\
                     L &= A P C^T R_e^{-1}
 
     where :math:`R_e = R_N + C P C^T`.  It can be called in the form::
@@ -353,7 +353,7 @@ def create_estimator_iosystem(
         state covariance.
     G : ndarray, optional
         Disturbance matrix describing how the disturbances enters the
-        dynamics.  Defaults to sys.B.
+        dynamics.  Defaults to `sys.B`.
     C : ndarray, optional
         If the system has full state output, define the measured values to
         be used by the estimator.  Otherwise, use the system output as the
@@ -430,6 +430,11 @@ def create_estimator_iosystem(
         resp = ct.input_output_response(
            est, T, 0, [X0, P0], param={'correct': False)
 
+    References
+    ----------
+    .. [1] R. M. Murray, `Optimization-Based Control
+       <https://fbswiki.org/OBC`_, 2013.
+
     """
 
     # Make sure that we were passed an I/O system as an input
@@ -480,11 +485,13 @@ def create_estimator_iosystem(
     # Generate the disturbance matrix (G)
     if G is None:
         G = sys.B if len(dist_idx) == 0 else sys.B[:, dist_idx]
+    G = _check_shape(G, sys.nstates, len(dist_idx), name='G')
 
     # Initialize the covariance matrix
     if P0 is None:
         # Initalize P0 to the steady state value
         _, P0, _ = lqe(A, G, C, QN, RN)
+    P0 = _check_shape(P0, sys.nstates, sys.nstates, symmetric=True, name='P0')
 
     # Figure out the labels to use
     estimate_labels = _process_labels(
@@ -507,6 +514,10 @@ def create_estimator_iosystem(
     inputs = measurement_labels + control_labels if inputs is None \
         else inputs
 
+    # Process the disturbance covariances and check size
+    QN = _check_shape(QN, G.shape[1], G.shape[1], square=True, name='QN')
+    RN = _check_shape(RN, C.shape[0], C.shape[0], square=True, name='RN')
+
     if isinstance(covariance_labels, str):
         # Generate the list of labels using the argument as a format string
         covariance_labels = [
diff --git a/control/tests/response_test.py b/control/tests/response_test.py
@@ -61,7 +61,7 @@ def test_response_plot(respfcn, plotfcn, respargs):
     else:
         # Single argument is enough
         args = (ct.rss(4, 1, 1, strictly_proper=True), )
-    
+
     # Standard calling pattern - generate response, then plot
     plt.figure()
     resp = respfcn(*args, **respargs)
diff --git a/doc/iosys.rst b/doc/iosys.rst
diff --git a/doc/nlsys.rst b/doc/nlsys.rst
diff --git a/examples/repr_gallery.ipynb b/examples/repr_gallery.ipynb
