2 frequency harmonic cancellation added, but not working correctly

akvo/tressel/harmonic.py

         nK = number of harmonics to calculate 
 
     """
-    
     #A = np.exp(1j* np.tile( np.arange(1,nK+1),(len(t), 1)) * 2*np.pi* (f0/fs) * np.tile( np.arange(1, len(t)+1, 1),(nK,1)).T  )
     KK = np.arange(k1, kN+1, 1/ks )
     nK = len(KK)
     print(res)
     return harmonicEuler(sN, fs, t, res.x[0], k1, kN, ks)#[0]
 
-def harmonicEuler2 ( f0, f1, sN, fs, nK, t ): 
+#def harmonicEuler2 ( f0, f1, sN, fs, nK, t ): 
+def harmonicEuler2 ( sN, fs, t, f0, f0k1, f0kN, f0ks, f1, f1k1, f1kN, f1ks ): 
     """
     Performs inverse calculation of harmonics contaminating a signal. 
     Args:
-        f0 = base frequency of the sinusoidal noise 
         sN = signal containing noise 
         fs = sampling frequency
-        nK = number of harmonics to calculate 
         t = time samples 
+        f0 = first base frequency of the sinusoidal noise 
+        f0k1 = First harmonic to calulate for f0 
+        f0kN = Last base harmonic to calulate for f0
+        f0ks = subharmonics to calculate 
     """
-    A1 = np.exp(1j* np.tile( np.arange(1,nK+1),(len(t), 1)) * 2*np.pi* (f0/fs) * np.tile(np.arange(1, len(t)+1, 1),(nK,1)).T  )
-    A2 = np.exp(1j* np.tile( np.arange(1,nK+1),(len(t), 1)) * 2*np.pi* (f1/fs) * np.tile(np.arange(1, len(t)+1, 1),(nK,1)).T  )
-    A = np.concatenate( (A1, A2), axis=1 )
-
+    #A1 = np.exp(1j* np.tile( np.arange(1,nK+1),(len(t), 1)) * 2*np.pi* (f0/fs) * np.tile(np.arange(1, len(t)+1, 1),(nK,1)).T  )
+    #A2 = np.exp(1j* np.tile( np.arange(1,nK+1),(len(t), 1)) * 2*np.pi* (f1/fs) * np.tile(np.arange(1, len(t)+1, 1),(nK,1)).T  )
+    #A = np.concatenate( (A1, A2), axis=1 )
+    KK0 = np.arange(f0k1, f0kN+1, 1/f0ks )
+    nK0 = len(KK0)
+    A0 = np.exp(1j* np.tile(KK0,(len(t), 1)) * 2*np.pi* (f0/fs) * np.tile( np.arange(1, len(t)+1, 1),(nK0,1)).T  )
+    KK1 = np.arange(f1k1, f1kN+1, 1/f1ks )
+    nK1 = len(KK1)
+    A1 = np.exp(1j* np.tile(KK1,(len(t), 1)) * 2*np.pi* (f1/fs) * np.tile( np.arange(1, len(t)+1, 1),(nK1,1)).T  )
+    A = np.concatenate( (A0, A1), axis=1 )
 
     v = np.linalg.lstsq(A, sN, rcond=None) # rcond=None) #, rcond=1e-8)
-    amp = np.abs(v[0][0:nK])     
-    phase = np.angle(v[0][0:nK]) 
-    amp1 = np.abs(v[0][nK:2*nK])     
-    phase1 = np.angle(v[0][nK:2*nK]) 
+    amp = np.abs(v[0][0:nK0])     
+    phase = np.angle(v[0][0:nK0]) 
+    amp1 = np.abs(v[0][nK0::])     
+    phase1 = np.angle(v[0][nK0::]) 
 
     h = np.zeros(len(t))
-    for ik in range(nK):
-        h +=  2*amp[ik]  * np.cos( 2.*np.pi*(ik+1) * (f0/fs) * np.arange(1, len(t)+1, 1 )  + phase[ik] ) + \
-              2*amp1[ik] * np.cos( 2.*np.pi*(ik+1) * (f1/fs) * np.arange(1, len(t)+1, 1 )  + phase1[ik] )
+    for ik in range(nK0):
+        h +=  2*amp[ik]  * np.cos( 2.*np.pi*(ik+1) * (f0/fs) * np.arange(1, len(t)+1, 1 )  + phase[ik] ) 
+    for ik in range(nK1):
+        h +=  2*amp1[ik]  * np.cos( 2.*np.pi*(ik+1) * (f1/fs) * np.arange(1, len(t)+1, 1 )  + phase1[ik] ) # + \
+        #      2*amp1[ik] * np.cos( 2.*np.pi*(ik+1) * (f1/fs) * np.arange(1, len(t)+1, 1 )  + phase1[ik] )
 
     return sN-h
 
-def harmonic2Norm ( f0, sN, fs, nK, t ): 
-    return np.linalg.norm(harmonicEuler2(f0[0], f0[1], sN, fs, nK, t))
+def harmonic2Norm (f0, sN, fs, t, f0k1, f0kN, f0ks, f1k1, f1kN, f1ks): 
+    #def harmonic2Norm ( f0, sN, fs, nK, t ): 
+    #return np.linalg.norm(harmonicEuler2(f0[0], f0[1], sN, fs, nK, t))
+    ii =  sN < (3.* np.std(sN))
+    return np.linalg.norm( harmonicEuler2(sN, fs, t, f0[0], f0k1, f0kN, f0ks, f0[1], f1k1, f1kN, f1ks)[ii] ) 
 
 #def minHarmonic(f0, sN, fs, nK, t):
 #    f02 = guessf0(sN, fs)
 
 
 
-def minHarmonic2(f1, f2, sN, fs, nK, t):
+#def minHarmonic2OLD(f1, f2, sN, fs, nK, t):
     #f02 = guessf0(sN, fs)
     #print("minHarmonic2", f0, fs, nK, " guess=", f02)
     #methods with bounds, L-BFGS-B, TNC, SLSQP
-    res = minimize( harmonic2Norm, np.array((f1,f2)), args=(sN, fs, nK, t), jac='2-point', method='BFGS') #, bounds=((f1-1.,f1+1.0),(f2-1.0,f2+1.0)), method='TNC' )
+#    res = minimize( harmonic2Norm, np.array((f1,f2)), args=(sN, fs, nK, t), jac='2-point', method='BFGS') #, bounds=((f1-1.,f1+1.0),(f2-1.0,f2+1.0)), method='TNC' )
+#    print(res)
+#    return harmonicEuler2(res.x[0], res.x[1], sN, fs, nK, t) 
+
+def minHarmonic2(sN, fs, t, f0, f0k1, f0kN, f0ks, f1, f1k1, f1kN, f1ks):
+    # CG, BFGS, Newton-CG, L-BFGS-B, TNC, SLSQP, dogleg, trust-ncg, trust-krylov, trust-exact and trust-constr
+    res = minimize(harmonic2Norm, np.array((f0, f1)), args=(sN, fs, t, f0k1, f0kN, f0ks, f1k1,f1kN, f1ks), jac='2-point', method='BFGS') # hess=None, bounds=None )
     print(res)
-    return harmonicEuler2(res.x[0], res.x[1], sN, fs, nK, t) 
+    return harmonicEuler2(sN, fs, t, res.x[0], f0k1, f0kN, f0ks, res.x[1], f1kN, f1kN, f1ks)#[0]
 
 def guessf0( sN, fs ):
     S = np.fft.fft(sN)

akvo/tressel/mrsurvey.py

     
     def harmonicModel(self, nF, \
         f0, f0K1, f0KN, f0Ks,  \
-        f1, f1K1, f1KN, K1Ks,  \
+        f1, f1K1, f1KN, f1Ks,  \
         plot, canvas):
         """ nF = number of base frequencies, must be 1 or 2 
             f0 = first base frequency  
                             self.DATADICT[pulse][ichan][ipm][istack] = harmonic.minHarmonic( self.DATADICT[pulse][ichan][ipm][istack], self.samp,  self.DATADICT[pulse]["TIMES"], \
                                 f0, f0K1, f0KN, f0Ks ) 
                         elif nF == 2:
-                            self.DATADICT[pulse][ichan][ipm][istack] = harmonic.minHarmonic2( f0-1e-2, f1+1e-2, self.DATADICT[pulse][ichan][ipm][istack], self.samp, nK, self.DATADICT[pulse]["TIMES"] ) 
-
+                            #self.DATADICT[pulse][ichan][ipm][istack] = harmonic.minHarmonic2( f0-1e-2, f1+1e-2, self.DATADICT[pulse][ichan][ipm][istack], self.samp, nK, self.DATADICT[pulse]["TIMES"] ) 
+                            self.DATADICT[pulse][ichan][ipm][istack] = harmonic.minHarmonic2( self.DATADICT[pulse][ichan][ipm][istack], self.samp,  self.DATADICT[pulse]["TIMES"], \
+                                f0-1e-2, f0K1, f0KN, f0Ks,  \
+                                f1+1e-2, f1K1, f1KN, f1Ks ) 
                         # plot
                         if plot:
                             canvas.ax1.plot( self.DATADICT[pulse]["TIMES"], 1e9*self.DATADICT[pulse][ichan][ipm][istack], \
                             self.DATADICT[pulse][ichan][ipm][istack] = harmonic.minHarmonic( self.DATADICT[pulse][ichan][ipm][istack], self.samp,  self.DATADICT[pulse]["TIMES"], \
                                 f0, f0K1, f0KN, f0Ks ) 
                         elif nF == 2:
-                            self.DATADICT[pulse][ichan][ipm][istack] = harmonic.minHarmonic2( f0-1e-2, f1+1e-2, self.DATADICT[pulse][ichan][ipm][istack], self.samp, nK, self.DATADICT[pulse]["TIMES"] ) 
+                            #self.DATADICT[pulse][ichan][ipm][istack] = harmonic.minHarmonic2( f0-1e-2, f1+1e-2, self.DATADICT[pulse][ichan][ipm][istack], self.samp, nK, self.DATADICT[pulse]["TIMES"] ) 
                         #self.DATADICT[pulse][ichan][ipm][istack] = harmonic.harmonicEuler( f0, self.DATADICT[pulse][ichan][ipm][istack], self.samp, 20, self.DATADICT[pulse]["TIMES"] ) 
+                            self.DATADICT[pulse][ichan][ipm][istack] = harmonic.minHarmonic2( self.DATADICT[pulse][ichan][ipm][istack], self.samp,  self.DATADICT[pulse]["TIMES"], \
+                                f0-1e-2, f0K1, f0KN, f0Ks,  \
+                                f1+1e-2, f1K1, f1KN, f1Ks ) 
                
                         # plot
                         if plot: