From 8f332b1ffaa79849290764d12b23af0ac9601062 Mon Sep 17 00:00:00 2001
From: naotoY <naotino@gmail.com>
Date: Tue, 19 Dec 2023 12:45:19 +0900
Subject: [PATCH] [add] IIR filter for PI-FB

---
 mbtrack2/tracking/rf.py | 98 +++++++++++++++++++++++++++++++++++++++--
 1 file changed, 95 insertions(+), 3 deletions(-)

diff --git a/mbtrack2/tracking/rf.py b/mbtrack2/tracking/rf.py
index 2dba4de..46d452f 100644
--- a/mbtrack2/tracking/rf.py
+++ b/mbtrack2/tracking/rf.py
@@ -938,6 +938,59 @@ class CavityResonator():
 
         return fig
 
+    def is_CBI_stable(self,I0,tune=0,mode=[0],max_freq=5,bool_return=False):
+        """
+        Check Coupled-Bunch-Instability stability 
+        Effect of Direct RF feedback is not included.
+
+        This method caluclates the CBI growth rate from own impedance.
+
+        Parameters
+        ----------
+        I0 : float
+            Beam current in [A].
+        tune : float
+            fractional number of longitudinal tune
+        mode : float or list of float
+            Coupled Bunch Instability mode number 
+        bool_return : bool
+            if True, return bool
+            
+        Returns
+        -------
+        bool
+
+        """
+        if tune == 0:
+            tune = self.ring.synchrotron_tune(self.Vc)
+        if tune > 1:
+            tune = tune - int(tune)
+        cof = self.ring.ac*I0/2.0/tune/self.ring.E0
+        if isinstance(mode,list):
+            CBImode = mode
+        else:
+            CBImode = [mode]
+
+        gr = np.zeros(len(CBImode))
+        gr_bool = np.zeros(len(CBImode),dtype=bool)
+        count = 0
+        for i in CBImode:
+            #fp = np.array([0,1,2,3,4])*self.ring.f1+i*self.ring.f0*(i+tune)
+            fp = self.ring.f0*(np.arange(max_freq)*self.ring.h+(i+tune))
+            #fm = np.array([1,2,3,4,5])*self.ring.f1-i*self.ring.f0*(i+tune)
+            fm = self.ring.f0*(np.arange(1,max_freq+1)*self.ring.h-(i+tune))
+            sumZ = np.sum(fp*np.real(self.Z(fp))) - np.sum(fm*np.real(self.Z(fm)))
+            
+            gr[count] = cof*sumZ
+            gr_bool[count] = ( gr[count] > 1/self.ring.tau[2] )
+            count +=1
+
+        if bool_return:
+            return gr_bool
+        else:
+            return gr 
+
+
     def is_DC_Robinson_stable(self, I0):
         """
         Check DC Robinson stability - Eq. (6.1.1) [1]
@@ -1220,11 +1273,14 @@ class ProportionalIntegralLoop():
         In case of super conducting cavity (QL > 1e6), the Pgain of ~100 
         can be used.
         In a "bad" parameter set, unstable oscillation of the cavity voltage 
-        can be caused. So, a parameter scan of the gain should be made. 
+        can be caused. So, a parameter scan of the gain should be made.
+        Igain * ring.T1 / dtau is Ki defined as a the coefficients for 
+        integral part in of [1], where dtau is a clock period of the PI controller.
     sample_num : int
         Number of bunch over which the mean cavity voltage is computed.
         Units are in bucket numbers.
     every : int
+        Sampling and Clock period of the feedback controller
         Time interval between two cavity voltage monitoring and feedback.
         Units are in bucket numbers.
     delay : int
@@ -1278,6 +1334,8 @@ class ProportionalIntegralLoop():
         QL=11800, fs0=23kHz
         ==> gain=[0.5,1e4], sample_num=8, every=7(13ns), delay=500(1us)
                      is one reasonable parameter set.
+        The practical clock period is 13ns.
+            ==> Igain_PF = Igain_mbtrack * Trf / 13ns = Igain_mbtrack * 0.153
      
     References
     ----------
@@ -1287,7 +1345,7 @@ class ProportionalIntegralLoop():
     of synchrotron light sources. PRAB, 21(1), 012001.
     
     """
-    def __init__(self, ring, cav_res, gain, sample_num, every, delay, FF=True):
+    def __init__(self, ring, cav_res, gain, sample_num, every, delay, IIR_cutoff=0,FF=True):
         self.ring = ring
         self.cav_res = cav_res
         self.Ig2Vg_mat = np.zeros((self.ring.h, self.ring.h), dtype=complex)
@@ -1307,6 +1365,14 @@ class ProportionalIntegralLoop():
             raise ValueError("Bad parameter set : delay or every")
         self.sample_num = int(sample_num)
 
+        # IIR filter
+        ## IIR_cutoff ; cutoff frequecny in Hz. If 0, cutoff frequency is infinity. 
+        if IIR_cutoff == 0:
+            self.IIRcoef = 1.0
+        else:
+            self.IIRcoef = self.every * self.ring.T1 * IIR_cutoff 
+        self.IIRout = self.cav_res.Vc
+
         # init lists for FB process
         self.ig_phasor = np.ones(self.ring.h, dtype=complex) * self.Vg2Ig(
             self.cav_res.generator_phasor)
@@ -1318,6 +1384,9 @@ class ProportionalIntegralLoop():
 
         self.sample_list = range(0, self.ring.h, self.every)
 
+        # IIR filter
+        ## IIR_cutoff ; cutoff frequecny in Hz. If 0, cutoff frequency is infinity.
+        self.IIR_init(IIR_cutoff)
         self.init_FFconst()
 
         # Pre caclulation for Ig2Vg
@@ -1348,7 +1417,7 @@ class ProportionalIntegralLoop():
             # 1) recording diff as a first item of the list
             mean_vc = np.mean(vc_list[index:self.sample_num + index]) * np.exp(
                 -1j * self.cav_res.theta)
-            self.diff_record[0] = self.cav_res.Vc - mean_vc
+            self.diff_record[0] = self.cav_res.Vc - self.IIR(mean_vc)
         # update sample_list for next turn
         self.sample_list = range(index + self.every - self.ring.h, self.ring.h,
                                  self.every)
@@ -1415,6 +1484,29 @@ class ProportionalIntegralLoop():
         """
         return Vg * (1 - 1j * np.tan(self.cav_res.psi)) / self.cav_res.RL
 
+    def IIR_init(self,cutoff):
+        """
+        f = 1/2/pi/T arccos((2-2r-r*r)/2(1-r))
+            T: Sample time
+            r: IIRcoef
+            f: cutoff freqeuncy
+        """
+        if cutoff == 0:
+            self.IIRcoef = 1.0
+        else:
+            omega = 2.0 * np.pi * cutoff
+            T = self.ring.T1 * self.every
+            alpha = np.cos(omega*T)-1
+            tmp = alpha * alpha - 2 * alpha
+            if alpha > 0:
+                self.IIRcoef = alpha + np.sqrt(tmp)
+            else:
+                self.IIRcoef = self.every * self.ring.T1 * cutoff * 2 * np.pi
+        self.IIRout = self.cav_res.Vc
+
+    def IIR(self,input):
+        self.IIRout = (1-self.IIRcoef)*self.IIRout+self.IIRcoef*input
+        return self.IIRout
 
 class DirectFeedback(ProportionalIntegralLoop):
     """
-- 
GitLab