diff --git a/BiquadFilter.py b/BiquadFilter.py index f7bd1c5..cccffe7 100644 --- a/BiquadFilter.py +++ b/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 diff --git a/RateLimiter.py b/RateLimiter.py new file mode 100644 index 0000000..2be5469 --- /dev/null +++ b/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 diff --git a/concept_switched_adaptive_control.txt b/concept_switched_adaptive_control.txt new file mode 100644 index 0000000..37b48c9 --- /dev/null +++ b/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. diff --git a/test_biquad.py b/test_biquad.py index 54890ab..3a7f93c 100644 --- a/test_biquad.py +++ b/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()