Merge pull request #15 from domengorjup/main

BUG: Fix inverted phase bug

.gitignore

@@ -117,3 +117,5 @@ venv.bak/
 .mypy_cache/
 .dmypy.json
 dmypy.json
+
+_temp/

pyFRF.py

@@ -46,6 +46,7 @@ def __init__(self, sampling_freq,
                  archive_time_data=False,
                  frf_type='H1',
                  copy=True,
+                 analytical_inverse=False,
                  **kwargs):
         """
         Initiates the Data class:
@@ -95,6 +96,11 @@ def __init__(self, sampling_freq,
         :param copy: If true the excitation and response arrays are copied 
             (if data is not copied the applied window affects the source arrays).
         :type copy: bool
+        :param analytical_inverse: If true, use the analytical formula for inversion of small 
+            (2x2, 3x3) matrices. Otherwise, the generalized pseudoinverse (np.linalg.pinv) is used. 
+            The analytical inverse is faster when 3 or less excitation DOFs are used, but might be
+            less stable for ill-conditioned spectral matrices. Defaults to False.
+        :type analytical_inverse: bool
         """
         # previous pyFRF kwargs:
         if ("exc_window" in kwargs) or ("resp_window" in kwargs):
@@ -159,6 +165,7 @@ def __init__(self, sampling_freq,
 
         self.frf_type = frf_type
         self.copy = copy
+        self.analytical_inverse = analytical_inverse
 
         # ini
         self.exc = np.array([])
@@ -634,52 +641,52 @@ def get_H1(self):
         """
         Get H1 FRF averaged estimator (receptance), preferable call via get_FRF().
 
+        For clarification on the Multiple input formulation, see [1] pages 116-118.
+
+        Literature:
+            [1] Maia and Silva: Theoretical and Experimental Modal Analysis. Research Studies Press, 1997.
+
         :return: H1 FRF estimator matrix (ndarray) of shape (response DOF, excitation DOF, freqency points).
         :rtype: ndarray
         """
         with np.errstate(invalid='ignore'):
-            freq_len = np.fft.rfftfreq(self.fft_len, 1. / self.sampling_freq).shape[0]
-            H1 = np.zeros((self.resp.shape[1], self.exc.shape[1], freq_len), dtype="complex128")
-
+            S_FX = self.S_FX.transpose(1, 0, 2)
+            S_FF = self.S_FF
             if self.exc.shape[1] == 1:  # SISO, SIMO
-                #print("single input")
-                H1 = self.frf_norm * self.S_XF / self.S_FF
+                H1 = self.frf_norm * S_FX / S_FF
             else:  # MISO, MIMO
-                if (self.S_FF[:,:,0].shape == (2,2)) or (self.S_FF[:,:,0].shape == (3,3)):
-                    for i in range(freq_len):
-                        H1[:,:,i] = self.frf_norm * (self.S_XF[:,:,i] @ self._analytical_matrix_inverse(self.S_FF[:,:,i]))
-                else:
-                    for i in range(freq_len):
-                        H1[:,:,i] = self.frf_norm * (self.S_XF[:,:,i] @ np.linalg.inv(self.S_FF[:,:,i]))
+                freq_len = np.fft.rfftfreq(self.fft_len, 1. / self.sampling_freq).shape[0]
+                H1 = np.zeros((self.resp.shape[1], self.exc.shape[1], freq_len), dtype="complex128")
+                for i in range(freq_len):
+                    H1[:, :, i] = self.frf_norm * (self._matrix_inverse(self.S_FF[:, :, i]) @ self.S_FX[:, :, i]).T 
+
             return  (H1 * self.frf_conversion) / self._correct_time_delay()
 
     # OK:    
     def get_H2(self):
         """
         H2 FRF averaged estimator (receptance), preferable call via get_FRF().
+        
+        For clarification on the Multiple input formulation, see [1] pages 116-118.
+
+        Literature:
+            [1] Maia and Silva: Theoretical and Experimental Modal Analysis. Research Studies Press, 1997.
 
         :return: H2 FRF estimator matrix (ndarray) of shape (response DOF, excitation DOF, freqency points).
         :rtype: ndarray
         """
         with np.errstate(invalid='ignore'):
-            freq_len = np.fft.rfftfreq(self.fft_len, 1. / self.sampling_freq).shape[0]
-            H2 = np.zeros((self.resp.shape[1], self.exc.shape[1], freq_len), dtype="complex128")
-
+            S_XF = self.S_XF
+            S_XX = self.S_XX
             if self.exc.shape[1] == 1:  # SISO, SIMO
-                #print("single input")
+                S_XX = np.diagonal(S_XX).T[:, None, :] # diagonal elements of S_XX, reshaped to (resp_DOF, 1, freq)
                 for i in range(self.resp.shape[1]):
-                    H2[i,:,:] = self.frf_norm * (self.S_XX[i,i,:] / self.S_FX[:,i,:])
+                    H2 = self.frf_norm * S_XX / S_XF 
             else:
-                if (self.S_FX[:,:,0].shape == (2,2)) or (self.S_FX[:,:,0].shape == (3,3)):
-                    for i in range(freq_len):
-                        H2[:,:,i] = self.frf_norm * (self.S_XX[:,:,i] @ self._analytical_matrix_inverse(self.S_FX[:,:,i]))
-                else:
-                    if (self.S_FX[:,:,0].shape[0] == self.S_FX[:,:,0].shape[1]):
-                        for i in range(freq_len):
-                            H2[:,:,i] = self.frf_norm * (self.S_XX[:,:,i] @ np.linalg.inv(self.S_FX[:,:,i]))
-                    else:
-                        for i in range(freq_len):
-                            H2[:,:,i] = self.frf_norm * (self.S_XX[:,:,i] @ np.linalg.pinv(self.S_FX[:,:,i]))
+                freq_len = np.fft.rfftfreq(self.fft_len, 1. / self.sampling_freq).shape[0]
+                H2 = np.zeros((self.resp.shape[1], self.exc.shape[1], freq_len), dtype="complex128")
+                for i in range(freq_len):
+                    H2[:, :, i] = self.frf_norm * (self._matrix_inverse(self.S_XF[:, :, i]) @ self.S_XX[:, :, i]).T 
             return (H2 * self.frf_conversion) / self._correct_time_delay()
 
     def get_Hv(self):
@@ -766,7 +773,7 @@ def get_coherence(self):
                 for i in range(self.resp.shape[1]):
                     if (self.S_FF[:,:,0].shape == (2,2)) or (self.S_FF[:,:,0].shape == (3,3)):
                         for j in range(freq_len):
-                            coh[i,j] = ((self.S_XF[i,:,j] @ self._analytical_matrix_inverse(self.S_FF[:,:,j])) @ self.S_FX[:,i,j])
+                            coh[i,j] = ((self.S_XF[i,:,j] @ self._matrix_inverse(self.S_FF[:,:,j])) @ self.S_FX[:,i,j])
                     else:
                         for j in range(freq_len):
                             coh[i,j] = ((self.S_XF[i,:,j] @ np.linalg.inv(self.S_FF[:,:,j])) @ self.S_FX[:,i,j])
@@ -946,43 +953,63 @@ def _correct_time_delay(self):
         """
         return (np.exp(1j * self.get_w_axis() * self.resp_delay))
 
-    def _analytical_matrix_inverse(self, matrix):
+    def _matrix_inverse(self, matrix):
         """
-        Calculate faster analytical inverse of 2x2 or 3x3 matrix.
-
+        Matrix inverse computation.
+        If self.analytical_inverse is True, it attempts to calculate faster analytical inverse of 
+        2x2 or 3x3 matrices. For larger square matrices, it uses numpy.linalg.inv. For singular
+        or non-square matrices, or if self.analytical inverse is False, the generalized pseudoinverse
+        is computed, using numpy.linalg.pinv.
+    
         :param matrix: Array of shape (2x2) or (3x3).
         :return: Matrix inverse array.
         :rtype: ndarray
         """
-        if matrix.shape == (2,2):
-            a, b, c, d = matrix[0][0], matrix[0][1], matrix[1][0], matrix[1][1]
-            det = a * d - b * c
-
-            if det == 0:
-                raise ValueError("Singular matrix.")
-
-            inverse_matrix = [[d / det, -b / det],
-                            [-c / det, a / det]]
-
-            return np.array(inverse_matrix)
-
-        if matrix.shape == (3,3):
-            a, b, c = matrix[0]
-            d, e, f = matrix[1]
-            g, h, i = matrix[2]
-
-            det = a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g)
-
-            if det == 0:
-                raise ValueError("Singular matrix.")
-
-            inverse_matrix = [
-                [(e * i - f * h) / det, (c * h - b * i) / det, (b * f - c * e) / det],
-                [(f * g - d * i) / det, (a * i - c * g) / det, (c * d - a * f) / det],
-                [(d * h - e * g) / det, (g * b - a * h) / det, (a * e - b * d) / det]
-            ]
+        non_square = matrix.shape[0] != matrix.shape[1]
 
-            return np.array(inverse_matrix)
+        if not self.analytical_inverse or non_square:
+            return np.linalg.pinv(matrix)
+
+        # square matrix, self.analytical_inverse is True
+        else:
+            try:
+                if matrix.shape == (2,2):
+                    a, b, c, d = matrix[0][0], matrix[0][1], matrix[1][0], matrix[1][1]
+                    det = a * d - b * c
+
+                    if det == 0:
+                        raise np.linalg.LinAlgError("Singular matrix.")
+
+                    inverse_matrix = [[d / det, -b / det],
+                                    [-c / det, a / det]]
+
+                    return np.array(inverse_matrix)
+
+                elif matrix.shape == (3,3):
+                    a, b, c = matrix[0]
+                    d, e, f = matrix[1]
+                    g, h, i = matrix[2]
+
+                    det = a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g)
+
+                    if det == 0:
+                        raise np.linalg.LinAlgError("Singular matrix.")
+
+                    inverse_matrix = [
+                        [(e * i - f * h) / det, (c * h - b * i) / det, (b * f - c * e) / det],
+                        [(f * g - d * i) / det, (a * i - c * g) / det, (c * d - a * f) / det],
+                        [(d * h - e * g) / det, (g * b - a * h) / det, (a * e - b * d) / det]
+                    ]
+
+                    return np.array(inverse_matrix)
+
+                else:
+                    # For square matrices larger than 3x3, use numpy.linalg.inv
+                    return np.linalg.pinv(matrix)
+
+            except np.linalg.LinAlgError:
+                # For singular square matrices, try using numpy.linalg.pinv
+                return np.linalg.pinv(matrix)
 
 if __name__ == '__main__':
-    pass
+    pass

tests/model.py

@@ -0,0 +1,140 @@
+import numpy as np
+
+def mdof_K_C(p):
+    """
+    Construct the stiffness or damping matrix of a N-DOF lumped parameter
+    mass-spring-damper system from given parameter values `p`.
+    """
+
+    N = len(p) - 1
+    mat = np.zeros((N, N))
+    for i in range(N):
+        if i == 0:
+            mat[i, :2] = np.array([ p[0] + p[1], -p[1] ]) 
+        elif i == N-1:
+            mat[i, -2:] = np.array([ -p[-2], p[-2] + p[-1] ])
+        else:
+            mat[i, i-1:i+2] = np.array([ -p[i], p[i]+p[i+1], -p[i+1] ])
+    return mat.astype(np.float64)
+
+
+def modal_model(m, k, c, get_matrices=False):
+    """
+    Compute the modal model (natural frequencies, damping ratios, system 
+    roots and eigenvectors) of a N-DOF system with provided properties.
+    """
+
+    # System matrices
+    N = len(m)
+    M = np.diag(m).astype(np.float64)
+    C = mdof_K_C(c)
+    K = mdof_K_C(k)
+
+    # State-space model
+    A = np.zeros(2*np.array(M.shape))
+    A[:N, :N] = C
+    A[:N, -N:] = M
+    A[-N:, :N] = M 
+    B = np.zeros_like(A)
+    B[:N, :N] = K
+    B[-N:, -N:] = -M
+
+    # Save system matrices
+    matrices = (M, K, C, A, B)
+
+    # Modal analysis
+    AB_eig = np.linalg.inv(A) @ B
+    w, v = np.linalg.eig(AB_eig)
+
+    roots = -w[1::2][::-1]
+    roots_c = -w[::2][::-1]
+    _vectors = v[:N, ::-2]     # non-normalized
+    _vectors_c = v[:N, -2::-2] # non-normalized
+
+    # eigenvector matrix for normalization
+    PHI = np.zeros_like(v)
+    PHI[:N, :N] = _vectors
+    PHI[-N:, :N] = roots*_vectors
+    PHI[:N, -N:] = _vectors_c
+    PHI[-N:, -N:] = roots_c*_vectors_c
+    a_r = np.diagonal(PHI.T @ A @ PHI)
+    _a_r = a_r[:N]
+    _a_r_c = a_r[N:]
+
+    # A-normalization
+    vectors_a = _vectors / np.sqrt(_a_r) # A-normalized
+    vectors_a_c = _vectors_c / np.sqrt(_a_r_c) # A-normalized
+
+    # Order returned data by system roots amplitude
+    order = np.argsort(np.abs(roots))
+    eig = (roots[order], roots_c[order], vectors_a[:, order], vectors_a_c[:, order]) # A-normalization
+
+    # System natural frequencies and viscous damping ratios
+    w_r = np.abs(roots[order])
+    d = -np.real(roots[order]) / w_r
+    f = w_r / 2 / np.pi # [Hz]
+
+    if get_matrices:
+        return f, d, eig, matrices
+    else:
+        return f, d, eig
+
+
+def FRF_matrix(omega, roots, roots_c, vectors, vectors_c):
+    """
+    Compute the receptance matrix of a system with provided roots and 
+    eigenvectors at selected frequencies `omega`.
+    """
+    N = len(roots)
+    n = len(omega)
+    FRF = np.zeros((N, N, n), dtype=np.complex128)
+
+    for j in range(N): # response location
+        for k in range(N): # excitation location 
+            FRF_jk = (vectors[j]*vectors[k])[:, None] / (1j*omega[None, :] - roots[:, None])
+            FRF_jk += (vectors_c[j]*vectors_c[k])[:, None] / (1j*omega[None, :] - roots_c[:, None])
+            FRF[j, k] = np.sum(FRF_jk, axis=0)
+
+    return FRF
+
+
+def compute_response(FRF, excitation, excitation_locations=None):
+    """
+    Compute the time-reposnse of the N-DOF lumped-mass system with FRF matrix
+    `FRF` for the input `excitation` force vectors.
+    
+    :param FRF: array of shape (N, N, n), the receptance matrix of the
+        N-DOF lumped parameter system.
+    :param excitaion: array of shape (M, P, N_samples), excitation (force) time
+        signals for the `M` measurement repetitions `P` input locations.
+    :param excitation_locations: array of shape (P,) indices of the locations 
+        (DOFs) that will be excited. If None, it is assumed that all N DOFs 
+        are to be excited, and the number of excitation vectors `P` must match 
+        the number of DOFs `N`. Defaults to None.
+    :returns: array of shape (N, N_samples), the time-response of the system
+        at the `N` DOFs.
+    """
+    if excitation_locations is None and excitation.shape[1] == FRF.shape[1]:
+        FRF = FRF
+    elif len(excitation_locations) == excitation.shape[1] <= FRF.shape[1]:
+        FRF = FRF[:, excitation_locations, :]
+    else:
+        raise ValueError('FRF degrees-of-freedom and excitation_locations mismatch.')
+
+    N_samples = excitation.shape[2]
+    N = FRF.shape[0]
+    M = excitation.shape[0]
+
+    # Frequency-domain superposition
+    F_f = np.fft.rfft(excitation) * 2 / excitation.shape[-1] # excitation spectra
+
+    # Compute response spectra at each frequency for each meausrement
+    X_f = np.zeros((M, N, FRF.shape[-1]), dtype=np.complex128)
+    for m in range(M):
+        for i in range(FRF.shape[-1]):
+            X_f[m, :, i] = np.dot(FRF[:, :, i], F_f[m, :, i])
+
+    # Time-domain response with added random noise
+    X_t = (np.fft.irfft(X_f) * (X_f.shape[-1]-1))[:, :, :N_samples]
+
+    return X_t