added seq_multi_threeparsinefit() and helpers

SineTools.py

@@ -9,6 +9,8 @@ auxiliary functions related to sine-approximation
 
 import numpy as np
 from scipy import linalg as la
+from scipy.sparse import coo_matrix
+from scipy.sparse.linalg import lsqr as sp_lsqr
 import matplotlib.pyplot as mp
 import warnings
 
@@ -19,7 +21,7 @@ def sampletimes(Fs, T):  #
     sample rate Fs \n
     from 0 to T
     """
-    num = np.ceil(T * Fs)
+    num = int(np.ceil(T * Fs))
     return np.linspace(0, T, num, dtype=np.float64)
 
 
@@ -236,7 +238,6 @@ def fourparsinefit(y, t, f0, tol=1.0e-7, nmax=1000):
                 (-1.0) * abcd[0] * t * np.sin(w * t) + abcd[1] * t * np.cos(w * t),
             ]
         )
-        print(dw)
         abcd = (la.lstsq(D.transpose(), y))[0]
         dw = abcd[3]
         w = w + 0.9 * dw
@@ -705,51 +706,167 @@ def PR_MultiSine(
     )  # frequency series, sample rate, sample timestamps, waveform
 
 
-###################################
-# MultiSine for "quick calibration" based on displacement and velocity without
-# explicit time-information (used for Linearmotor-control)
-def XV_MultiSine(freq, phase, x0, tau, N=1000, deg=True):
-    '''
-    Calculation of a multisine trajectory in terms of position and velocity based 
-    on given frequency, amplitude and phase
+def seq_multi_threeparam_Dmatrix(f,t,periods=1):
+    """
+    Fit a multi-sinus-signal in slices in one go.
 
     Parameters
     ----------
-    freq : numpy.array float
-        Vector of frequencies of the sine components
-    phase : numpy array float
-        Vector of initial phases of the sine components
-    x0 : TYPE
-        Vector of amplitudes of the sine components
-    tau : float
-        duration for the whole motion
-    N : int, optional
-        number of samples. The default is 1000.
-    deg : boolean, optional
-        If True phases are in degree, if False in rad. The default is True/Deg.
+    f : numpy.array of floats
+        list of frequencies in the signal
+    t : numpy.array of floats
+        timestamps of y (typically seconds) 
+    periods : float, optional
+        the number of periods of each frequncy used for each fit. The default is 1.
 
     Returns
     -------
-    multi_x : numpy.array float
-        Calculated position vector
-    multi_v : numpy.array
-        Calculated velocity vector
+    
+    fr,  : 1-d numpy.array of floats
+          frequencies related to slices
+    D    : 2-d numpy.array of floats 
+          Design matrix for the fit 
+
+    """
+    T = t[-1]-t[0]
+    col = 0
+    data = np.array([])
+    ci = []
+    ri = []
+    fr = []
+    # Designmatrix for sin/cos
+    for fi, omi in zip(f,2*np.pi*f):
+        Nri = 0 # counter for the current row index
+        tau = 1/fi*periods  # slice length in seconds
+        t_sl = np.array_split(t,np.ceil(T/tau)) # array of slices of sample times
+        fr = fr + [fi]*len(t_sl) # len(t_sl) times frequency fi in Design matrix
+        for ti in t_sl: # loop over slices fo one frequncy
+            sin = np.sin(omi * ti)
+            cos = np.cos(omi * ti)
+            
+            data = np.hstack((data,sin))  # data vector
+            ci = ci+[col]*len(ti)  # column index vector
+            col = col+1
+            ri = ri+ [Nri+i for i in range(len(ti))] # row index vector
+            
+            data = np.hstack((data,cos))  # data vector
+            ci = ci+[col]*len(ti)  # column index vector
+            col = col+1
+            ri = ri+ [Nri+i for i in range(len(ti))] # row index vector
+
+            Nri = Nri+len(ti)
+
+    # Designmatrix for Bias
+    data = np.hstack((data,np.ones((len(t)))))
+    ci = ci = ci+[col]*len(t)
+    ri = ri + [i for i in range(len(t))]
+    col = col+1
+
+    # build sparse matrix, init as coo, map to csr
+    D = coo_matrix((data,(ri,ci)),shape=(len(t),col)).tocsr()
+    return np.array(fr), D
+
+def seq_multi_threeparsinefit(f,y,t,periods=1):
+    """
+    performs a simultanius, sliced three-parameter fit on a multisine signal y
 
-    '''
-    assert (
-        len(freq) == len(x0) == len(phase)
-    ), "XV_MultiSine: Unequal length of frequency and amplitude arrays!"
+    Parameters
+    ----------
+    f : 1-d numpy.array of floats
+        frequencies in the signal
+    y : 1-d numpy.array
+        samples of the signal
+    t : 1-d numpy.array
+        vector of sample times
+    periods : float
+        (fractional) number of periods in one slice. The default is 1.
 
-    if deg:  # rad -> deg
-        phase = phase * np.pi / 180.0
+    Returns
+    -------
+    f_ab_c : 2-d numpy.array of floats
+        frequencies, a/b-coefficients and bias related to given frequencies [f1,f1,f1,f2,f2, ...fn, fn, f=0=bias]
+    y0 : 1-d numpy.array of floats
+        samples of the optimal fit
+    resid : 1-d numpy.array of floats
+        Residuals = Difference =(y-y0)
+
+    """
+    fr, D = seq_multi_threeparam_Dmatrix(f,t,periods)  # calculate the design matrix (as sparse matrix)
+    
+    abc = sp_lsqr(D,y)
+    y0 = D.dot(abc[0])
+    
+    #print(abc)
+    f_ab_c =[]
+    k=0
+    for fi in fr:
+        f_ab_c = f_ab_c+ [fi, abc[0][k],abc[0][k+1]]
+        k=k+2
+    f_ab_c = f_ab_c + [0.0,abc[0][-1],0.0]
+    f_ab_c = np.array(f_ab_c).reshape((len(f_ab_c)//3,3))
+    
+    print(f_ab_c)
+    return f_ab_c, y0, y-y0  # coefficients, fit signal, residuals
+
+
+def seq_multi_amplitude(f_ab_c):
+    """
+    return the amplitude(s) of a sequentially fit multi-sine signal, 
+    i.e. amplitudes encoded in the return value of seq_multi_threeparsinefit.
+
+    Parameters
+    ----------
+    f_ab_c : 2-d numpy array of floats (Nx3) 
+        f,a,b in a row for several rows, as returned by seq_multi_threeparsinefit.
+        
+    Returns
+    -------
+    2d-numpy-array of floats (Nx2)
+        frequency and associated amplitude. 
 
-    ti = np.arange(0, tau, tau / N)
+    """
+    amp = [np.abs(f_ab_c[i,1]+1j*f_ab_c[i][2]) for i in range(f_ab_c.shape[0]-1)]
+    return np.vstack((f_ab_c[:-1,0],np.array(amp))).T
+
+def seq_multi_phase(f_ab_c, deg=True):
+    """
+    Calculates the initial phase of a sequentially fit multi-sine signal, 
+    i.e. initial phases encoded in the return value of seq_multi_threeparsinefit.
+    result is either in degrees (deg=True) or rad (deg=False).
+
+    Parameters
+    ----------
+    f_ab_c : 2-d numpy array of floats (Nx3) 
+        f,a,b in a row for several rows, as returned by seq_multi_threeparsinefit.
+    deg : Boolean, optional
+        Flag whether result is in degrees or rad. The default is True (Degrees).
+
+    Returns
+    -------
+    2d-numpy-array of floats (Nx2)
+        frequency and associated initial phase. 
+
+    """
+    amp = [np.angle(1j*f_ab_c[i,1]+f_ab_c[i][2],deg=deg) for i in range(f_ab_c.shape[0]-1)]
+    return np.vstack((f_ab_c[:-1,0],np.array(amp))).T
 
-    multi_x = np.zeros(len(ti), dtype=np.float64)
-    multi_v = np.zeros(len(ti), dtype=np.float64)
-    for f, x, p in zip(freq, x0, phase):
-        om = 2 * np.pi * f
-        multi_x = multi_x + x * np.sin(om * ti + p)
-        multi_v = multi_v + x * om * np.cos(om * ti + p)
+def seq_multi_bias(f_ab_c):
+    """
+    Returns the single bias of a sequentially fit multi-sine signal, 
+    i.e. bias encoded in the return value of seq_multi_threeparsinefit.
+
+    Parameters
+    ----------
+    f_ab_c : 2-d numpy array of floats (Nx3) 
+        f,a,b in a row for several rows, as returned by seq_multi_threeparsinefit.
 
-    return ti, multi_x, multi_v
+    Returns
+    -------
+    float
+        Bias of the signal.
+
+    """
+    return f_ab_c[-1][1] 
+    
+
+