added/fixed seq_multi_fourparam_sineFit and seq_multi_fourparam_Dmatrix

SineTools.py

@@ -703,7 +703,7 @@ def PR_MultiSine(
     )  # frequency series, sample rate, sample timestamps, waveform
 
 
-def seq_multi_threeparam_Dmatrix(f,t,periods=1, progressive=True):
+def seq_multi_threeparam_Dmatrix_old(f,t,periods=1, progressive=True):
     """
     Fit a multi-sinus-signal in slices in one go.
 
@@ -765,8 +765,86 @@ def seq_multi_threeparam_Dmatrix(f,t,periods=1, progressive=True):
 
     # 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_threeparam_Dmatrix(f,t,periods=1, progressive=True):
+    """
+    Fit a multi-sinus-signal in slices in one go.
+
+    Parameters
+    ----------
+    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
+    -------
+    
+    fr,  : 1-d numpy.array of floats
+          frequencies related to slices
+    D    : 2-d numpy.array of floats 
+          Design matrix for the fit 
+
+    """
+    Nt = len(t)
+    Nf = len(f)
+
+    ci = []  # initialise column indices
+    ri = [a for a in range(len(t))]*(2*len(f)+1) # row indices
+    data = [0.0]*len(ri)  # initialise column indices
+    fr = []
+    
+    # Designmatrix for cos/sin 
+    c_count = 0 # counter for columns
+    f_count = 0 # counter for frequencies * 2
+    for fi, omi in zip(f,2*np.pi*f):
+        om_t = omi*t        # omega*t
+        data[f_count*Nt:(f_count+1)*Nt] = np.cos(om_t).tolist() # sparse matrix entries
+        data[(f_count+1)*Nt:(f_count+2)*Nt] = np.sin(om_t).tolist() # sparse matrix entries
+        
+        # now split the vectors over the sparse matrix
+        if progressive:
+            tau = 1/fi*(fi//f[0])*periods # approximately same abs. slice length for all fi  
+        else:
+            tau = 1/fi*periods  # slice length in seconds for periods of fi
+        N_samp = int(tau//np.mean(np.diff(t)))  # samples/rows per slice
+        N_slices = len(t)//N_samp               # slices for 
+        
+        slic_inds = np.array_split(np.arange(len(t)), N_slices) # r-indices in the slices
+        if len(slic_inds[-1] < N_samp):  # if last slice too short
+            slic_inds[-2] = np.concatenate([slic_inds[-2],slic_inds[-1]])  # merge last two slices
+            slic_inds.pop() # remove old last slice
+        
+        fr = fr+[fi]*len(slic_inds)        
+        # for cosine
+        for i,s in enumerate(slic_inds):
+            ci = ci + [c_count+2*i] *len(s)
+        # for sine
+        for i,s in enumerate(slic_inds):
+            ci = ci + [c_count+(2*i+1)] *len(s)
+        
+        c_count += 2*len(slic_inds)                        
+        f_count += 2
+            
+    # Bias part
+    data[2*Nf*Nt:(2*Nf+1)*Nt] = [1.0]*Nt  
+    ci[2*Nf*Nt:(2*Nf+1)*Nt] = [c_count]*Nt
+    c_count += 1
+
+    # build sparse matrix, init as coo, map to csr
+    D = coo_matrix((data,(ri,ci)),shape=(Nt,c_count)).tocsr()
+    # if False:
+    #     print("seq_multi_threeparam_Dmatrix/ D.shape= %s" % str(D.shape))
+    #     mp.spy(D, markersize=5, marker=".",aspect="auto")
+    #     mp.show()
     return np.array(fr), D
 
+
+
 def seq_multi_threeparsinefit(f,y,t,periods=1, D_fr=None, abc0=None, progressive=True):
     """
     performs a simultanius, sliced three-parameter fit on a multisine signal y
@@ -814,7 +892,106 @@ def seq_multi_threeparsinefit(f,y,t,periods=1, D_fr=None, abc0=None, progressive
     return f_ab_c, y0, y-y0  # coefficients, fit signal, residuals
 
 
-def seq_multi_fourparam_sineFit(f,y,t,periods=1, tol=1.0e-6, n_max=100):
+def seq_multi_fourparam_Dmatrix(f,t, abc=None, periods=1, progressive=True):
+    """
+    Fit a multi-sinus-signal in slices in one go.
+
+    Parameters
+    ----------
+    f : numpy.array of floats
+        list of (adapted) frequencies in the signal 
+    t : numpy.array of floats
+        timestamps of y (typically seconds) 
+    abc :numpy.array of floats
+         parameters from last fit-iteration 
+    periods : float, optional
+        the number of periods of each frequncy used for each fit. The default is 1.
+
+    Returns
+    -------
+    
+    fr,  : 1-d numpy.array of floats
+          frequencies related to slices
+    D    : 2-d numpy.array of floats 
+          Design matrix for the fit 
+
+    """
+    Nt = len(t)
+    Nf = len(f)
+
+    ci = []  # initialise column indices
+    ri = [a for a in range(Nt)]*(2*Nf+2) # row indices
+    data = [0.0]*len(ri)  # initialise column indices
+    fr = []
+    col_a =[]  # extra part -A*t*sin(om t)
+    col_b=[]   # extra part  B*t*cos(om t)
+    last_col = np.zeros((Nt), dtype=np.float64)
+    
+    
+    # Designmatrix for cos/sin 
+    c_count = 0 # counter for columns
+    f_count = 0 # counter for frequencies * 2
+    for fi, omi in zip(f,2*np.pi*f):
+        col_a =[]  # extra part -A*t*sin(om t)
+        col_b=[]   # extra part  B*t*cos(om t)
+        om_t = omi*t        # omega*t
+        co = np.cos(om_t)
+        si = np.sin(om_t)
+        data[f_count*Nt:(f_count+1)*Nt] = co.tolist() # sparse matrix entries
+        data[(f_count+1)*Nt:(f_count+2)*Nt] = si.tolist() # sparse matrix entries
+        
+        # now split the vectors over the sparse matrix
+        if progressive:
+            tau = 1/fi*(fi//f[0])*periods # approximately same abs. slice length for all fi  
+        else:
+            tau = 1/fi*periods  # slice length in seconds for periods of fi
+        N_samp = int(tau//np.mean(np.diff(t)))  # samples/rows per slice
+        N_slices = len(t)//N_samp               # slices for 
+        
+        slic_inds = np.array_split(np.arange(len(t)), N_slices) # r-indices in the slices
+        if len(slic_inds[-1] < N_samp):  # if last slice too short
+            slic_inds[-2] = np.concatenate([slic_inds[-2],slic_inds[-1]])  # merge last two slices
+            slic_inds.pop() # remove old last slice
+        
+        
+        fr = fr+[fi]*len(slic_inds)
+        # for cosine
+        for i,s in enumerate(slic_inds):
+            ci = ci + [c_count+2*i] *len(s)
+        # for sine
+        for i,s in enumerate(slic_inds):
+            ci = ci + [c_count+(2*i+1)] *len(s)
+            
+        if abc is not None:
+            for i,s in enumerate(slic_inds):
+                col_a = col_a + [-abc[2*i]]*len(s)  # list of A coeffs of cos
+                col_b = col_b + [abc[2*i+1]]*len(s) # list of B coeffs of sine
+            last_col = last_col + t*omi*(np.array(col_b)*co + np.array(col_a)*si) # add up 4 param column
+        # else last column is zero as initialised
+            
+        c_count += 2*len(slic_inds)                        
+        f_count += 2
+            
+    # Bias part
+    data[2*Nf*Nt:(2*Nf+1)*Nt] = [1.0]*Nt  
+    ci[2*Nf*Nt:(2*Nf+1)*Nt] = [c_count]*Nt
+    c_count += 1
+    
+    # 4th parameter part
+    data[(2*Nf+1)*Nt:(2*Nf+2)*Nt] = last_col.tolist()
+    ci[(2*Nf+1)*Nt:(2*Nf+2)*Nt] = [c_count]*Nt
+    c_count += 1
+
+    # build sparse matrix, init as coo, map to csr
+    D = coo_matrix((data,(ri,ci)),shape=(Nt,c_count)).tocsr()
+    if False:
+        print("seq_multi_threeparam_Dmatrix/ D.shape= %s" % str(D.shape))
+        mp.spy(D, markersize=5, marker=".",aspect="auto")
+        mp.show()
+    return np.array(fr), D
+
+
+def seq_multi_fourparam_sineFit(f,y,t,periods=1, progressive=True, tol=1.0e-9, n_max=100):
     """
     Fit a multi-sinus-signal in slices in one go. With a frequency correction 
     universal for all frequncies (time-stretch)
@@ -841,42 +1018,44 @@ def seq_multi_fourparam_sineFit(f,y,t,periods=1, tol=1.0e-6, n_max=100):
           Design matrix for the fit 
 
     """
-    T = t[-1]-t[0]
-    dw = 1.0
-    lam=1.0
-    itr=0
-    while (np.abs(dw) > tol) and (itr<n_max):
-        itr += 1
-        # Designmatrix for three paramsin/cos
-        fr,D = seq_multi_threeparam_Dmatrix(f, t, periods=periods, progressive=False)
-        # initial ab 
-        f_ab_c = seq_multi_threeparsinefit(f, y, t, D_fr=(D,fr))[0]
-        last_col = np.zeros(len(t))
-        for fi, omi in zip(f,2*np.pi*f):
-            tau = 1/fi*periods  # slice length in seconds
-            t_sl = np.array_split(t,np.ceil(T/tau)) # array of slices of sample times
-            sin_cos=np.array([])
-            for i,ti in enumerate(t_sl): # loop over slices for one frequncy
-                si = -f_ab_c[i,1]*omi*ti*np.sin(omi * ti)
-                co = f_ab_c[i,2]*omi*ti*np.cos(omi * ti)
-                
-                sin_cos = np.hstack((sin_cos,si+co))  # data vector
-            last_col = last_col+sin_cos
-        last_col = coo_matrix((last_col,([i for i in range(len(last_col))],[0 for i in range(len(last_col))])),shape=(len(last_col),1)).tocsr()
-        D = hstack((D, last_col))
-        abc = sp_lsqr(D,y)[0]
-        dw = abc[-1]
-        f = (1-0.9*dw)*f
-        lam=lam*(1-dw)
-        if False:
-            print("abc:")
-            print(abc)
-            print("f=")
-            print(f)
-        print("cor. = %f" %(lam))
-        print("took %d iterations" % (itr))
-    f_ab_c, y0, res  = seq_multi_threeparsinefit(f, y, t, D_fr=None)
-
+    
+    fr,D = seq_multi_threeparam_Dmatrix(f,t,periods=periods, progressive=progressive)
+    f_abc, y1, res1 = seq_multi_threeparsinefit(f, y, t,D_fr=[D,fr], periods=periods, progressive=progressive  )
+    
+    abc = (f_abc[0:-1,1:3]).flatten()  # a,b,a,b,a,b,
+    abc = np.hstack([abc, f_abc[-2,-2],0.0])  # ababab, c, dw
+    
+    i = 0
+    
+    while True: # do-while emulation 
+        fr, D = seq_multi_fourparam_Dmatrix(f,t, abc=abc, periods=periods, progressive=progressive)  # calculate the design matrix (as sparse matrix)
+        param = sp_lsqr(D,y,x0=abc, atol=1.0e-10, btol=1.0e-10)
+        #print("abc: %s" % str(param))
+        f = f*(1+param[0][-1]) # korrekt the frequencies by the factor \Delta\omega
+        abc = param[0] 
+        print("\delta\Omega: %g" % param[0][-1])
+        print("f= %s" % str(f))
+        
+        if (param[0][-1]/f[0]<tol) :
+            print("fit arrived at tolerance")
+            break 
+        
+        if (i>n_max):
+            print("too many iterations")
+            break
+        i += 1
+    
+    y0 = D.dot(param[0])
+    fr = fr*(1+param[0][-1])
+    
+    f_ab_c =[]
+    k=0
+    for fi in fr: # compile a list of frequencies and coefficients
+        f_ab_c = f_ab_c+ [fi, abc[k],abc[k+1]] # [f, a, b]...
+        k=k+2
+    f_ab_c = f_ab_c + [0.0,abc[-2],0.0] # add the bias to the list [0,c,0]
+    f_ab_c = np.array(f_ab_c).reshape((len(f_ab_c)//3,3))
+    
     return f_ab_c, y0, y-y0  # coefficients, fit signal, residuals
 
 def seq_multi_amplitude(f_ab_c):