Added rate limiter and generic 2nd order TF

BiquadFilter.py

@@ -1,24 +1,32 @@
 ''' Digital Biquadratic filter implementation '''
 
 class BiquadFilter:
-    ''' Implementation of a digital biquadratic filter. '''
-    def __init__(self, omega, q, dt, ftype):
+    ''' Implementation of a digital second-order biquadratic filter. '''
+    def __init__(self, omega, q, dt, ftype='lowpass'):
         ''' Builds a biquad filter from the given parameters. '''
-        self.omega = omega
-        self.q     = q
-        self.dt    = dt
-        self.ftype = ftype
         self.InitFilter(0)
-        self.ComputeContinuousTF(omega, q, dt, ftype)
+        self.ComputeContinuousTFBiquad(omega, q, dt, ftype)
         self.ConvertContinuousToDiscrete()
 
+    def SetContinuousTF(self, b0, b1, b2, a0, a1, a2, dt):
+        ''' Sets the continuous-time coefficients of the second order transfer function. '''
+        self.dt  = dt
+        self.b0c = b0
+        self.b1c = b1
+        self.b2c = b2
+        self.a0c = a0
+        self.a1c = a1
+        self.a2c = a2
 
-    def ComputeContinuousTF(self, omega, q, dt, ftype='lowpass'):
+    def ComputeContinuousTFBiquad(self, omega, q, dt, ftype='lowpass'):
         ''' Computes the continuous time transfer function from the given parameters.
             b0 + b1*s + b2*s^2
             ------------------
             a0 + a1*s + a2*s^2
         '''
+        self.omega = omega
+        self.q     = q
+        self.dt    = dt
         if ftype == 'highpass':
             self.b0c = 0
             self.b1c = 0
@@ -59,11 +67,24 @@ class BiquadFilter:
         self.a2d = 4*self.a2c + 2*self.a1c*self.dt + self.a0c*self.dt**2
 
     def PrintContinuousTF(self):
-        ''' Prints the continuous time transfer function coefficients. '''
+        ''' Prints the continuous-time transfer function coefficients. '''
+        print('Continuous-time transfer function :')
         print('%f + %f s + %f s**2' % (self.b0c, self.b1c, self.b2c))
         print('----------------------------------------')
         print('%f + %f s + %f s**2' % (self.a0c, self.a1c, self.a2c))
 
+    def PrintDiscreteTF(self):
+        ''' Prints the discrete-time transfer function coefficients. '''
+        print('Discrete-time transfer function :')
+        print('%f + %f z + %f z**2' % (self.b0d, self.b1d, self.b2d))
+        print('----------------------------------------')
+        print('%f + %f z + %f z**2' % (self.a0d, self.a1d, self.a2d))
+        print('dt = %f' % self.dt)
+
+    def PrintAllTF(self):
+        self.PrintContinuousTF()
+        self.PrintDiscreteTF()
+
     def InitFilter(self, v):
         ''' Initializes the filter with the value v. '''
         self.xn_0 = v

RateLimiter.py

@@ -0,0 +1,29 @@
+''' Digital rate limiter filter implementation '''
+
+class RateLimiter:
+    ''' Implementation of a digital rate limiter filter. '''
+    def __init__(self, rateDown, rateUp, dt):
+        ''' Builds a biquad filter from the given parameters. '''
+        self.InitFilter(0)
+        self.rateUp = rateUp
+        self.rateDown = rateDown
+        self.dt = dt
+
+        if self.rateDown > self.rateUp:
+            print('Warning : rate limiter with id ' + str(id(self)) + ' : lower limit is higher than upper limit !')
+
+    def InitFilter(self, v):
+        ''' Initializes the filter with the value v. '''
+        self.xn_0 = v
+        self.yn_1 = v
+
+    def Filter(self, xn_0):
+        ''' Computes the next value of the filter. '''
+        self.xn_0 = xn_0
+        if ((self.xn_0 - self.yn_1)/self.dt) > self.rateUp:
+            self.yn_1 = self.yn_1 + self.rateUp * self.dt       # next output value is set at max up rate
+        elif ((self.xn_0 - self.yn_1)/self.dt) < self.rateDown:
+            self.yn_1 = self.yn_1 + self.rateDown * self.dt     # next output value is set at max down rate
+        else:
+            self.yn_1 = self.xn_0
+        return self.yn_1

concept_switched_adaptive_control.txt

@@ -0,0 +1,9 @@
+Concept : Switched adaptive controller
+
+- a bank of transfer functions, or candidate system models, are being simulated with the current inputs to the real system
+- for every model, we compare its output with the output measured from the real system, and integrate the difference over some time
+- every one in a while, we select the controller that corresponds to the model with the best fitness
+- we reset the models with the initial conditions measured from the real system periodically as well
+
+As a result, we should get a somewhat okay adaptation of the control, since for every model, we have a properly tuned controller.
+The switch can be made smooth by either going through a low-pass filter, or, better, smoothly transitioned from one input to the other with a variable going linearly in time.

test_biquad.py

@@ -1,21 +1,29 @@
 # test of biquad filter
 import numpy as np
 from BiquadFilter import BiquadFilter
+from RateLimiter import RateLimiter
 import matplotlib.pyplot as plt
 
 omega = 10
 q = 2
-dt = 0.01
+dt = 0.02
 
-tflow = BiquadFilter(omega, q, dt, "lowpass")
-tfhigh = BiquadFilter(omega, q, dt, "highpass")
-tfband = BiquadFilter(omega, q, dt, "bandpass")
-tfnotch = BiquadFilter(omega, q, dt, "notch")
+tflow   = BiquadFilter(omega, q, dt, 'lowpass')
+tfhigh  = BiquadFilter(omega, q, dt, 'highpass')
+tfband  = BiquadFilter(omega, q, dt, 'bandpass')
+tfnotch = BiquadFilter(omega, q, dt, 'notch')
 
-tflow.PrintContinuousTF(); print('')
-tfhigh.PrintContinuousTF(); print('')
-tfband.PrintContinuousTF(); print('')
-tfnotch.PrintContinuousTF(); print('')
+tfint   = BiquadFilter(omega, q, dt)
+tfint.SetContinuousTF(1, 0, 0, 0, 1, 0, dt)     # 1/s : simple integrator
+tfint.ConvertContinuousToDiscrete()
+
+tfRateLim = RateLimiter(-0.3, 0.5, dt)
+
+tflow.PrintAllTF(); print('')
+tfhigh.PrintAllTF(); print('')
+tfband.PrintAllTF(); print('')
+tfnotch.PrintAllTF(); print('')
+tfint.PrintAllTF(); print('')
 
 # simulate filter response
 tend = 10
@@ -24,21 +32,25 @@ Yl = np.zeros(Npts)
 Yh = np.zeros(Npts)
 Yband = np.zeros(Npts)
 Ynotch = np.zeros(Npts)
+Yint = np.zeros(Npts)
+Yratelim = np.zeros(Npts)
 T = np.zeros(Npts)
 
 for i in range(Npts):
     t = i*dt
-    if t >= 1:
+    if t >= 1 and t < 6:
         x = 1
     else:
         x = 0
 
-    T[i] = t
-    Yl[i] = tflow.Filter(x)
-    Yh[i] = tfhigh.Filter(x)
-    Yband[i] = tfband.Filter(x)
-    Ynotch[i] = tfnotch.Filter(x)
+    T[i]        = t
+    Yl[i]       = tflow.Filter(x)
+    Yh[i]       = tfhigh.Filter(x)
+    Yband[i]    = tfband.Filter(x)
+    Ynotch[i]   = tfnotch.Filter(x)
+    Yint[i]     = tfint.Filter(x)
+    Yratelim[i] = tfRateLim.Filter(x)
 
-plt.plot(T, Yl, T, Yh, T, Yband, T, Ynotch)
+plt.plot(T, Yl, T, Yh, T, Yband, T, Ynotch, T, Yint, T, Yratelim)
 plt.grid(True)
 plt.show()