implement delayLTI and PartionedStateSpace classes and first algorith…

…m to solve delay ODE in timeresp. Add litlle tests about timereps
MythasNauveili · Mar 26, 2025 · Mar 26, 2025 · Mar 26, 2025 · Mar 28, 2025 · Mar 29, 2025
commit de5a61fbd7e9b49d0c994a00f0ead356cb02c475
diff --git a/control/delaylti.py b/control/delaylti.py
@@ -0,0 +1,193 @@
+import numpy as np
+from .lti import LTI
+from .partitionedssp import PartitionedStateSpace
+from .statesp import ss, StateSpace, tf2ss
+from .xferfcn import TransferFunction
+from .iosys import _process_iosys_keywords
+
+
+
+class DelayLTISystem(LTI):
+
+    def __init__(self, P: PartitionedStateSpace, tau: np.ndarray = [], **kwargs):
+        self.P = P
+        self.tau = tau
+
+        self.A = P.A
+        self.B = P.B
+        self.C = P.C
+        self.D = P.D
+
+        self.B1 = P.B1
+        self.B2 = P.B2
+        self.C1 = P.C1
+        self.C2 = P.C2
+        self.D11 = P.D11
+        self.D12 = P.D12
+        self.D21 = P.D21
+        self.D22 = P.D22
+
+        static = (self.A.size == 0)
+
+        self.ninputs = self.P.sys.ninputs - len(self.tau)
+        self.noutputs = self.P.sys.noutputs - len(self.tau)
+        self.nstates = self.P.sys.nstates
+
+        defaults = {'inputs': self.B1.shape[1], 'outputs': self.C1.shape[0],
+             'states': self.A.shape[0]}
+        name, inputs, outputs, states, dt = _process_iosys_keywords(
+            kwargs, defaults, static=static)
+
+        super().__init__(inputs, outputs, states, name)
+
+    @classmethod
+    def from_ss(cls, sys: StateSpace, tau: np.ndarray = []):
+        nu = sys.D.shape[1] - len(tau)
+        ny = sys.D.shape[0] - len(tau)
+
+        if nu < 0 or ny < 0:
+            raise ValueError("tau is too long")
+
+        psys = PartitionedStateSpace(sys, nu, ny)
+        return cls(psys, tau)
+
+    def size(self):
+        return (self.noutputs, self.ninputs)
+
+    def __mul__(self, other):
+        if isinstance(other, (int, float, complex)):
+            new_C = np.block([[self.P.C1 * other], [self.P.C2]])
+            new_D = np.block([[self.P.D11 * other, self.P.D12 * other], [self.P.D21, self.P.D22]])
+            new_P = PartitionedStateSpace(ss(self.P.A, self.P.B, new_C, new_D), self.P.nu1, self.P.ny1)
+            return DelayLTISystem(new_P, self.tau)
+
+        elif isinstance(other, DelayLTISystem):
+            psys_new = self.P * other.P
+            tau_new = np.concatenate([self.tau, other.tau])
+            return DelayLTISystem(psys_new, tau_new)
+
+        elif isinstance(other, TransferFunction):
+            dlti = tf2dlti(other)
+            return self * dlti
+        else:
+            raise TypeError("Unsupported operand type(s) for *: '{}' and '{}'".format(type(self), type(other)))
+
+    def __rmul__(self, other):  # Handle  System * Number
+        if isinstance(other, (int, float, complex)):
+            new_B = np.block([self.P.B1 * other, self.P.B2])
+            new_D = np.block([[self.P.D11 * other, self.P.D12], [self.P.D21 * other, self.P.D22]])
+            new_P = PartitionedStateSpace(ss(self.P.A, new_B, self.P.C, new_D), self.P.nu1, self.P.ny1)
+            return DelayLTISystem(new_P, self.tau)
+
+        elif isinstance(other, TransferFunction):
+            dlti = tf2dlti(other)
+            return dlti * self
+        else:
+            raise TypeError("Unsupported operand type(s) for *: '{}' and '{}'".format(type(other), type(self)))
+
+    def __add__(self, other):
+        if isinstance(other, (int, float, complex)):
+            # Add to direct term from input to output
+            new_D = self.P.sys.D.copy()
+            new_D[:self.noutputs(), :self.ninputs()] += other
+            pnew = PartitionedStateSpace(ss(self.P.sys.A, self.P.sys.B, self.P.sys.C, new_D), self.P.nu1, self.P.ny1)
+            return DelayLTISystem(pnew, self.tau)
+
+        elif isinstance(other, DelayLTISystem):
+            psys_new = self.P + other.P
+            tau_new = np.concatenate([self.tau, other.tau])
+            return DelayLTISystem(psys_new.P, tau_new)
+        else:
+            raise TypeError("unsupported operand type(s) for +: '{}' and '{}'".format(type(self), type(other)))
+
+    def __sub__(self, other):
+        return self + (-other)
+
+    def __neg__(self):
+        return self * -1
+
+    def __rsub__(self, other):
+        return -self + other
+
+    def __eq__(self, other):
+        if not isinstance(other, DelayLTISystem):
+            return False
+        return (np.array_equal(self.P.A, other.P.A) and
+                np.array_equal(self.P.B, other.P.B) and
+                np.array_equal(self.P.C, other.P.C) and
+                np.array_equal(self.P.D, other.P.D) and
+                np.array_equal(self.tau, other.tau))
+
+    def feedback(self, other):
+            psys_new = self.P.feedback(other.P)
+            tau_new  = np.concatenate([self.tau, other.tau])
+            return DelayLTISystem(psys_new.P, tau_new)
+
+    def __str__(self):
+        s = "DelayLTISystem\n"
+        s += "P:\n" + str(self.P) + "\n"
+        s += "A =\n" + str(self.P.A) + "\n"
+        s += "B =\n" + str(self.P.B) + "\n"
+        s += "C =\n" + str(self.P.C) + "\n"
+        s += "D =\n" + str(self.P.D) + "\n" + "\n"
+        s += "delays: " + str(self.tau)
+        return s
+
+
+def delay(tau):
+    """
+    Pure delay
+    """
+
+    if isinstance(tau, (int, float)):
+        tau_arr = [float(tau)]
+        num_delays = 1
+    elif isinstance(tau, (list, np.ndarray)):
+        tau_arr = [float(t) for t in tau] 
+        num_delays = len(tau_arr)
+    else:
+        raise TypeError("tau must be a number, list, or NumPy array")
+
+    D = np.array([
+        [0, 1],
+        [1, 0]
+    ])
+
+    ny, nu = D.shape[0], D.shape[1]
+
+    A = np.zeros((0,0))
+    B = np.zeros((0, nu))
+    C = np.zeros((ny, 0))
+
+    P = PartitionedStateSpace(ss(A, B, C, D), 1, 1)
+    return DelayLTISystem(P, tau_arr)
+
+
+def exp(G):
+    """
+    create delay in the form of exp(-τ*s) where s=tf("s")
+    """
+    num = G.num[0][0]
+    den = G.den[0][0]
+
+    if not (len(den) == 1 and len(num) == 2 and num[0] < 0 and num[1] == 0):
+        raise ValueError("Input must be of the form -τ*s, τ>0.")
+
+    return delay(-num[0] / den[0])
+
+
+def tf2dlti(tf: TransferFunction):
+    """
+    Convert a TransferFunction to a DelayLTISystem
+    """
+    if not isinstance(tf, TransferFunction):
+        raise TypeError("Input must be a TransferFunction")
+
+    ss_tf = tf2ss(tf)
+
+    if hasattr(tf, 'delays'):
+        tau = tf.delays
+        delay_tau= delay(tau)
+        return DelayLTISystem.from_ss(ss_tf) * delay_tau
+    else:
+        return DelayLTISystem.from_ss(ss_tf)
diff --git a/control/partitionedssp.py b/control/partitionedssp.py
@@ -0,0 +1,199 @@
+import numpy as np
+from scipy.linalg import block_diag, inv
+
+from .statesp import ss, StateSpace
+
+class PartitionedStateSpace:
+    def __init__(self, sys: StateSpace, nu1: int, ny1: int):
+        self.sys = sys
+        self.nu1 = nu1
+        self.ny1 = ny1
+
+        self.A = self.sys.A
+        self.B = self.sys.B
+        self.C = self.sys.C
+        self.D = self.sys.D
+
+    @property
+    def B1(self):
+        return self.B[:, :self.nu1]
+
+    @property
+    def B2(self):
+        return self.B[:, self.nu1:]
+
+    @property
+    def C1(self):
+        return self.C[:self.ny1, :]
+
+    @property
+    def C2(self):
+        return self.C[self.ny1:, :]
+
+    @property
+    def D11(self):
+        return self.D[:self.ny1, :self.nu1]
+
+    @property
+    def D12(self):
+        return self.D[:self.ny1, self.nu1:]
+
+    @property
+    def D21(self):
+        return self.D[self.ny1:, :self.nu1]
+
+    @property
+    def D22(self):
+        return self.D[self.ny1:, self.nu1:]
+
+
+    @classmethod
+    def from_matrices(cls, A, B1, B2, C1, C2, D11, D12, D21, D22):
+        nx = A.shape[0]   
+        nw = B1.shape[1] 
+        nu = B2.shape[1]  
+        nz = C1.shape[0] 
+        ny = C2.shape[0]
+
+        # Shape validations
+        if A.shape[1] != nx and nx != 0:
+            raise ValueError("A must be square")
+        if B1.shape[0] != nx:
+            raise ValueError("B1 must have the same row size as A")
+        if B2.shape[0] != nx:
+            raise ValueError("B2 must have the same row size as A")
+        if C1.shape[1] != nx:
+            raise ValueError("C1 must have the same column size as A")
+        if C2.shape[1] != nx:
+            raise ValueError("C2 must have the same column size as A")
+        if D11.shape[1] != nw:
+            raise ValueError("D11 must have the same column size as B1")
+        if D21.shape[1] != nw:
+            raise ValueError("D21 must have the same column size as B1")
+        if D12.shape[1] != nu:
+            raise ValueError("D12 must have the same column size as B2")
+        if D22.shape[1] != nu:
+            raise ValueError("D22 must have the same column size as B2")
+        if D11.shape[0] != nz:
+            raise ValueError("D11 must have the same row size as C1")
+        if D12.shape[0] != nz:
+            raise ValueError("D12 must have the same row size as C1")
+        if D21.shape[0] != ny:
+            raise ValueError("D21 must have the same row size as C2")
+        if D22.shape[0] != ny:
+            raise ValueError("D22 must have the same row size as C2")
+
+        B = np.hstack((B1, B2))
+        C = np.vstack((C1, C2))
+        D = np.block([[D11, D12], [D21, D22]])
+
+        sys = ss(A, B, C, D)
+
+        return cls(sys, nw, nz)
+
+
+    def __add__(self, other):
+        if not isinstance(other, PartitionedStateSpace):
+            raise TypeError("Can only add PartitionedStateSpace objects")
+
+        A = block_diag(self.A, other.A)
+        B1 = np.vstack((self.B1, other.B1))
+        B2 = block_diag(self.B2, other.B2)
+        B = np.hstack((B1, B2))
+
+        C1 = np.hstack((self.C1, other.C1))
+        C2 = block_diag(self.C2, other.C2)
+        C = np.vstack((C1, C2))
+
+        D11 = self.D11 + other.D11
+        D12 = np.hstack((self.D12, other.D12))
+        D21 = np.vstack((self.D21, other.D21))
+        D22 = block_diag(self.D22, other.D22)
+        D = np.block([[D11, D12], [D21, D22]])
+
+        P = ss(A, B, C, D)
+        return PartitionedStateSpace(P, self.nu1 + other.nu1, self.ny1 + other.ny1)
+
+    def __mul__(self, other):
+        if not isinstance(other, PartitionedStateSpace):
+            raise TypeError("Can only multiply PartitionedStateSpace objects")
+
+        A = np.block([
+            [self.A, self.B1 @ other.C1],
+            [np.zeros((other.A.shape[0], self.A.shape[1])), other.A]
+        ])
+
+        B = np.block([
+            [self.B1 @ other.D11, self.B2, self.B1 @ other.D12],
+            [other.B1, np.zeros((other.B2.shape[0], self.B2.shape[1])), other.B2]
+        ])
+
+        C = np.block([
+            [self.C1, self.D11 @ other.C1],
+            [self.C2, self.D21 @ other.C1],
+            [np.zeros((other.C2.shape[0], self.C2.shape[1])), other.C2]
+        ])
+
+        D = np.block([
+            [self.D11 @ other.D11, self.D12, self.D11 @ other.D12],
+            [self.D21 @ other.D11, self.D22, self.D21 @ other.D12],
+            [other.D21, np.zeros((other.D22.shape[0], self.D22.shape[1])), other.D22]
+        ])
+
+        P = ss(A, B, C, D)
+        return PartitionedStateSpace(P, other.nu1, self.ny1)
+
+    def feedback(self, other):
+        """
+        Feedback connection of two PartitionedStateSpace systems.
+        """
+        if not isinstance(other, PartitionedStateSpace):
+            raise TypeError("Feedback connection only defined for PartitionedStateSpace objects.")
+
+        # Pre-calculate repeated inverses
+        I1 = np.eye(other.D11.shape[0])
+        I2 = np.eye(self.D11.shape[0])
+
+        try:
+            inv_I_plus_D211_D111 = inv(I1 + other.D11 @ self.D11)
+            inv_I_plus_D111_D211 = inv(I2 + self.D11 @ other.D11)
+        except np.linalg.LinAlgError:
+            raise ValueError("Singular matrix encountered. Feedback connection may not be well-posed.")
+
+
+        X_11 = inv_I_plus_D211_D111 @ (-other.D11 @ self.C1 - other.C1)
+        X_21 = inv_I_plus_D111_D211 @ (self.C1 - self.D11 @ other.C1)
+        X_12 = inv_I_plus_D211_D111 @ (I1 - other.D11 @ self.D12 - other.D12)
+        X_22 = inv_I_plus_D111_D211 @ (self.D11 + self.D12 - self.D11 @ other.D12)
+
+
+        A = np.block([
+            [self.A + self.B1 @ X_11, self.B1 @ X_11[ : , self.C1.shape[1]:]],
+            [other.B1 @ X_21[:other.C1.shape[0], :], other.A + other.B1 @ X_21]
+        ])
+
+
+        B = np.block([
+            [self.B1 @ X_12, self.B1 @ X_12[:, I1.shape[1]: ] + self.B2],
+            [other.B1 @ X_22[:, :self.D11.shape[0]], other.B1 @ X_22[:, self.D11.shape[0]:] + other.B2]
+        ])
+
+        C = np.block([
+            [self.C1 + self.D11 @ X_11, self.D11 @ X_11[:, self.C1.shape[1]:]],
+            [self.C2 + self.D21 @ X_11, self.D21 @ X_11[:, self.C1.shape[1]:]],
+            [other.D21 @ X_21, other.C2 + other.D21 @ X_21[:, other.C1.shape[1]:]]
+        ])
+
+        D = np.block([
+            [self.D11 @ X_12, self.D11 @ X_12[:, I1.shape[1]:] + self.D12],
+            [self.D21 @ X_12, self.D21 @ X_12[:, I1.shape[1]:] + self.D22],
+            [other.D21 @ X_22[:, :self.D11.shape[0]], other.D21 @ X_22[:, self.D11.shape[0]:] + other.D22]
+        ])
+
+
+        P = ss(A, B, C, D)
+        return PartitionedStateSpace(P, other.nu1, self.ny1)
+
+
+
+