Skip to content

Instantly share code, notes, and snippets.

@guglie

guglie/ardupilot_LPF_test.py

Last active May 29, 2019 09:39
Show Gist options
  • Save guglie/8b8242ede6c34539e47cd9a7cdc9421f to your computer and use it in GitHub Desktop.
Save guglie/8b8242ede6c34539e47cd9a7cdc9421f to your computer and use it in GitHub Desktop.
Download ZIP
Ardupilot Low Pass Filter Test and standard Biquad Low Pass Filter implementation with parameters calculated as in https://www.w3.org/2011/audio/audio-eq-cookbook.html
Raw
ardupilot_LPF_test.py
#!/usr/bin/env python
""" Ardupilot Low Pass Filter Test
This program is free software: you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation, either version 3 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
You should have received a copy of the GNU General Public License along with
this program. If not, see <http://www.gnu.org/licenses/>.
"""
__author__ = "Guglielmo Cassinelli"
__contact__ = "gdguglie@gmail.com"
import numpy as np
class DigitalLPF:
def __init__(self, cutoff_freq, sample_freq):
self._cutoff_freq = cutoff_freq
self._sample_freq = sample_freq
self._output = 0
self.compute_alpha()
def compute_alpha(self):
if self._cutoff_freq <= 0 or self._sample_freq <= 0:
self.alpha = 1.
else:
dt = 1. / self._sample_freq
rc = 1. / (np.pi * 2 * self._cutoff_freq)
a = dt / (dt+rc)
self.alpha = np.clip(a, 0, 1)
def apply(self, sample):
self._output += (sample - self._output) * self.alpha
return self._output
class Biquad_LPF:
def __init__(self, cutoff_freq, sample_freq):
self._cutoff_freq = cutoff_freq
self._sample_freq = sample_freq
self._delayed_sample1 = 0
self._delayed_sample2 = 0
self._delayed_output1 = 0
self._delayed_output2 = 0
self.b0 = 0.
self.b1 = 0.
self.b2 = 0.
self.a1 = 0.
self.a2 = 0.
self.compute_params()
def compute_params(self):
if self._cutoff_freq > 0:
Q = 1 / np.sqrt(2)
omega = 2 * np.pi * self._cutoff_freq / self._sample_freq
sn = np.sin(omega)
cs = np.cos(omega)
alpha = sn / (2 * Q)
self.b0 = (1 - cs) / 2
self.b1 = 1 - cs
self.b2 = self.b0
self.a0 = 1 + alpha
self.a1 = -2 * cs
self.a2 = 1 - alpha
self.b0 /= self.a0
self.b1 /= self.a0
self.b2 /= self.a0
self.a1 /= self.a0
self.a2 /= self.a0
def apply(self, sample):
if self._cutoff_freq <= 0:
return sample
output = self.b0 * sample + self.b1 * self._delayed_sample1 + self.b2 * self._delayed_sample2 - self.a1 * self._delayed_output1 - self.a2 * self._delayed_output2
self._delayed_sample2 = self._delayed_sample1
self._delayed_sample1 = sample
self._delayed_output2 = self._delayed_output1
self._delayed_output1 = output
return output
def get_params(self):
return {
"a1": self.a1,
"a2": self.a2,
"b0": self.b0,
"b1": self.b1,
"b2": self.b2,
}
def freq_response(self, f, sample_freq):
phi = (np.sin(np.pi * f * 2 / (2 * sample_freq))) ** 2
r = ((self.b0 + self.b1 + self.b2) ** 2 - \
4 * (self.b0 * self.b1 + 4 * self.b0 * self.b2 + \
self.b1 * self.b2) * phi + 16 * self.b0 * self.b2 * phi * phi) / \
((1 + self.a1 + self.a2) ** 2 - 4 * (self.a1 + 4 * self.a2 + \
self.a1 * self.a2) * phi + 16 * self.a2 * phi * phi)
#if r < 0:
# r = 0
return r ** .5
class Ardupilot_Biquad_LPF:
def __init__(self, cutoff_freq, sample_freq):
self._cutoff_freq = cutoff_freq
self._sample_freq = sample_freq
self._delayed_element1 = 0
self._delayed_element2 = 0
self.b0 = 0.
self.b1 = 0.
self.b2 = 0.
self.a1 = 0.
self.a2 = 0.
self.compute_params()
def compute_params(self):
if self._cutoff_freq > 0:
fr = self._sample_freq / self._cutoff_freq
ohm = np.tan(np.pi / fr) # "circuit resistance"
c = 1. + 2. * np.cos( np.pi / 4.) * ohm + ohm * ohm # "circuit capacitance", farads
self.b0 = ohm * ohm / c
self.b1 = 2. * self.b0
self.b2 = self.b0
self.a1 = 2. * (ohm * ohm - 1) / c
self.a2 = (1. - 2. * np.cos(np.pi / 4) * ohm + ohm * ohm) / c
def apply(self, sample):
if self._cutoff_freq <= 0:
return sample
delayed_element0 = sample - self._delayed_element1 * self.a1 - self._delayed_element2 * self.a2
output = delayed_element0 * self.b0 + self._delayed_element1 * self.b1 + self._delayed_element2 * self.b2
self._delayed_element2 = self._delayed_element1
self._delayed_element1 = delayed_element0
return output
def get_params(self):
return {
"a1": self.a1,
"a2": self.a2,
"b0": self.b0,
"b1": self.b1,
"b2": self.b2,
}
if __name__ == "__main__":
import matplotlib.pyplot as plt
from matplotlib.ticker import ScalarFormatter
CUTOFF_FREQ = 25 #hz
SAMPLE_FREQ = 5000 #hz
SAMPLES = int(12 * SAMPLE_FREQ / CUTOFF_FREQ)
biquad_lpf = Biquad_LPF(CUTOFF_FREQ, SAMPLE_FREQ)
ardupilot_lpf = Ardupilot_Biquad_LPF(CUTOFF_FREQ, SAMPLE_FREQ)
digital_lpf = DigitalLPF(CUTOFF_FREQ, SAMPLE_FREQ)
sin1 = []
raw = []
biquad_filtered = []
ardupilot_filtered = []
digitalLPF_filtered = []
ts = []
sin1_freq = CUTOFF_FREQ / 4 #np.sqrt(CUTOFF_FREQ) #low frequency wave (below LPF cutoff)
sin2_freq = CUTOFF_FREQ * 10 #high frequency wave (above LPF cutoff)
sin3_freq = sin2_freq * 3.1 #another high frequency wave (above LPF cutoff)
dt = 1. / SAMPLE_FREQ
def wave(t, freq):
return np.sin(t * 2 * np.pi * freq)
print("testing low pass filtering of a three sin wave signal")
print("wave 1 frequency = ", sin1_freq)
print("wave 2 frequency = ", sin2_freq)
print("wave 3 frequency = ", sin3_freq)
print("LowPass Biquad params:\n", biquad_lpf.get_params())
print("Ardupilot Biquad params:\n", ardupilot_lpf.get_params())
for i in range(SAMPLES):
t = i * dt
ts.append(t)
sin1_sample = wave(t, sin1_freq)
sin2_sample = wave(t, sin2_freq)
sin3_sample = wave(t, sin3_freq)
raw_sample = sin1_sample + 0.3 * sin2_sample + 0.1 * sin3_sample
biquad_filtered_sample = biquad_lpf.apply(raw_sample)
ardupilot_filtered_sample = ardupilot_lpf.apply(raw_sample)
digitalLPF_filtered_sample = digital_lpf.apply(raw_sample)
sin1.append(sin1_sample)
raw.append(raw_sample)
biquad_filtered.append(biquad_filtered_sample)
ardupilot_filtered.append(ardupilot_filtered_sample)
digitalLPF_filtered.append(digitalLPF_filtered_sample)
#signal plot
fig = plt.figure(figsize=(11, 9), num="Low Pass Filter")
ax = fig.add_subplot(211)
ax.plot(ts, sin1, linewidth=4, label="low freq signal", zorder=10)
ax.plot(ts, raw, linewidth=2, label="raw (multifreq)")
ax.plot(ts, ardupilot_filtered, linewidth=4, linestyle="--", label="ardupilot LPF", zorder=12)
ax.plot(ts, digitalLPF_filtered, linewidth=4, label="digital LPF")
ax.plot(ts, biquad_filtered, linewidth=2, color="black", label="biquad LPF", zorder=11)
ax.set_xlabel("time")
ax.legend()
#FFT plot
x_fft = np.linspace(0.0, 1.0 / (2.0 * dt), SAMPLES / 2)
raw_fft = np.fft.fft(raw)
filtered2_fft = np.fft.fft(biquad_filtered)
y_lpf_shape = biquad_lpf.freq_response(x_fft, SAMPLE_FREQ)
ax_fft = fig.add_subplot(212)
ax_fft.plot(x_fft, 2.0/SAMPLES * np.abs(raw_fft[:SAMPLES//2]), label="raw FFT")
ax_fft.plot(x_fft, 2.0/SAMPLES * np.abs(filtered2_fft[:SAMPLES//2]), label="LPF FFT")
ax_fft.plot(x_fft, y_lpf_shape, label="LPF shape")
ax_fft.axvline(x=CUTOFF_FREQ, linestyle="--", c="green") # LPF cutoff freq
ax_fft.text(CUTOFF_FREQ+1, 1, "cutoff", color="green")
ax_fft.set_xlabel("frequency")
ax_fft.set_xscale("log")
ax_fft.xaxis.set_major_formatter(ScalarFormatter())
ax_fft.legend()
fig.tight_layout()
plt.show()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment