Last active
June 10, 2023 09:36
-
-
Save JasonLG1979/c95b035ed271bfcbbe10a8047cf9e580 to your computer and use it in GitHub Desktop.
Super basic FIR low pass filter with no dependencies outside std.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// This is free and unencumbered software released into the public domain. | |
// | |
// Anyone is free to copy, modify, publish, use, compile, sell, or | |
// distribute this software, either in source code form or as a compiled | |
// binary, for any purpose, commercial or non-commercial, and by any | |
// means. | |
// | |
// In jurisdictions that recognize copyright laws, the author or authors | |
// of this software dedicate any and all copyright interest in the | |
// software to the public domain. We make this dedication for the benefit | |
// of the public at large and to the detriment of our heirs and | |
// successors. We intend this dedication to be an overt act of | |
// relinquishment in perpetuity of all present and future rights to this | |
// software under copyright law. | |
// | |
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, | |
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF | |
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. | |
// IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR | |
// OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, | |
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR | |
// OTHER DEALINGS IN THE SOFTWARE. | |
// | |
// For more information, please refer to <http://unlicense.org/> | |
// Hann coefficients | |
const HANN_A0: f64 = 0.5; | |
const HANN_A1: f64 = 1.0; | |
// Hamming coefficients | |
const HAMMING_A0: f64 = 0.53836; | |
const HAMMING_A1: f64 = 0.46164; | |
// Nuttall coefficients | |
const NUTTALL_A0: f64 = 0.355768; | |
const NUTTALL_A1: f64 = 0.487396; | |
const NUTTALL_A2: f64 = 0.144232; | |
const NUTTALL_A3: f64 = 0.012604; | |
// Blackman coefficients | |
const BLACKMAN_A0: f64 = 0.42; | |
const BLACKMAN_A1: f64 = 0.5; | |
const BLACKMAN_A2: f64 = 0.08; | |
// Blackman-Harris coefficients | |
const BLACKMAN_HARRIS_A0: f64 = 0.355768; | |
const BLACKMAN_HARRIS_A1: f64 = 0.487396; | |
const BLACKMAN_HARRIS_A2: f64 = 0.144232; | |
const BLACKMAN_HARRIS_A3: f64 = 0.012604; | |
// Blackman-Nuttall coefficients | |
const BLACKMAN_NUTTALL_A0: f64 = 0.3635819; | |
const BLACKMAN_NUTTALL_A1: f64 = 0.4891775; | |
const BLACKMAN_NUTTALL_A2: f64 = 0.1365995; | |
const BLACKMAN_NUTTALL_A3: f64 = 0.0106411; | |
// Constants for calculations | |
const TWO_PI: f64 = 2.0 * std::f64::consts::PI; | |
const FOUR_PI: f64 = 4.0 * std::f64::consts::PI; | |
const SIX_PI: f64 = 6.0 * std::f64::consts::PI; | |
#[derive(Clone, Copy, Debug, PartialOrd, Ord, PartialEq, Eq)] | |
pub enum Window { | |
Hann, | |
Hamming, | |
Nuttall, | |
Blackman, | |
BlackmanHarris, | |
BlackmanNuttall, | |
} | |
impl Window { | |
fn value(&self, index: usize, taps: usize) -> f64 { | |
// Generate window values for a Window variant based on index and taps, | |
// which represent n and N in the window function equations. | |
use Window::*; | |
// Common values shared by all windows | |
// n | |
let n = index as f64; | |
// 2πn | |
let two_pi_n = TWO_PI * n; | |
// N-1 | |
let cap_n_minus_one = taps as f64 - 1.0; | |
match self { | |
Hann => { | |
// Calculate the Hann window function for the given center offset | |
// w(n) = 0.5 * (1 - cos(2πn / (N-1))), | |
// where n is the center offset and N is the window size | |
HANN_A0 * (HANN_A1 - (two_pi_n / cap_n_minus_one).cos()) | |
} | |
Hamming => { | |
// Calculate the Hamming window function for the given center offset | |
// w(n) = A0 - A1 * cos(2πn / (N-1)), | |
// where n is the center offset, N is the window size, | |
// and A0, A1 are precalculated coefficients | |
HAMMING_A0 - HAMMING_A1 * (two_pi_n / cap_n_minus_one).cos() | |
} | |
Nuttall => { | |
// Calculate the Nuttall window function for the given center offset | |
// w(n) = A0 - A1*cos(2πn / (N-1)) + A2*cos(4πn / (N-1)) - A3*cos(6πn / (N-1)), | |
// where n is the center offset, N is the window size, | |
// and A0, A1, A2, A3 are precalculated coefficients | |
let four_pi_n = FOUR_PI * n; | |
let six_pi_n = SIX_PI * n; | |
NUTTALL_A0 - NUTTALL_A1 * (two_pi_n / cap_n_minus_one).cos() | |
+ NUTTALL_A2 * (four_pi_n / cap_n_minus_one).cos() | |
- NUTTALL_A3 * (six_pi_n / cap_n_minus_one).cos() | |
} | |
Blackman => { | |
// Calculate the Blackman window function for the given center offset | |
// w(n) = A0 - A1*cos(2πn / (N-1)) + A2*cos(4πn / (N-1)), | |
// where n is the center offset, N is the window size, | |
// and A0, A1, A2 are precalculated coefficients | |
let four_pi_n = FOUR_PI * n; | |
BLACKMAN_A0 - BLACKMAN_A1 * (two_pi_n / cap_n_minus_one).cos() | |
+ BLACKMAN_A2 * (four_pi_n / cap_n_minus_one).cos() | |
} | |
BlackmanHarris => { | |
// Calculate the Blackman-Harris window function for the given center offset | |
// w(n) = A0 - A1*cos(2πn / (N-1)) + A2*cos(4πn / (N-1)) - A3*cos(6πn / (N-1)), | |
// where n is the center offset, N is the window size, | |
// and A0, A1, A2, A3 are precalculated coefficients | |
let four_pi_n = FOUR_PI * n; | |
let six_pi_n = SIX_PI * n; | |
BLACKMAN_HARRIS_A0 - BLACKMAN_HARRIS_A1 * (two_pi_n / cap_n_minus_one).cos() | |
+ BLACKMAN_HARRIS_A2 * (four_pi_n / cap_n_minus_one).cos() | |
- BLACKMAN_HARRIS_A3 * (six_pi_n / cap_n_minus_one).cos() | |
} | |
BlackmanNuttall => { | |
// Calculate the Blackman-Nuttall window function for the given center offset | |
// w(n) = A0 - A1*cos(2πn / (N-1)) + A2*cos(4πn / (N-1)) - A3*cos(6πn / (N-1)), | |
// where n is the center offset, N is the window size, | |
// and A0, A1, A2, A3 are precalculated coefficients | |
let four_pi_n = FOUR_PI * n; | |
let six_pi_n = SIX_PI * n; | |
BLACKMAN_NUTTALL_A0 - BLACKMAN_NUTTALL_A1 * (two_pi_n / cap_n_minus_one).cos() | |
+ BLACKMAN_NUTTALL_A2 * (four_pi_n / cap_n_minus_one).cos() | |
- BLACKMAN_NUTTALL_A3 * (six_pi_n / cap_n_minus_one).cos() | |
} | |
} | |
} | |
} | |
#[derive(Debug)] | |
pub enum FilterError { | |
InvalidTaps(u8), | |
InvalidCutoffFrequency(u32, u32), | |
ZeroSampleRate, | |
ZeroCutoffFrequency, | |
} | |
impl std::error::Error for FilterError {} | |
impl std::fmt::Display for FilterError { | |
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { | |
use FilterError::*; | |
match self { | |
InvalidTaps(taps) => write!( | |
f, | |
"Invalid number of taps: {}. Taps must be at least 3 and an odd number", | |
taps | |
), | |
InvalidCutoffFrequency(cutoff_freq, nyquist_freq) => write!( | |
f, | |
"Invalid cutoff frequency: {}. Cutoff frequency must not exceed the Nyquist frequency ({} Hz) for the given sample rate", | |
cutoff_freq, nyquist_freq | |
), | |
ZeroSampleRate => write!(f, "Sample rate cannot be zero"), | |
ZeroCutoffFrequency => write!(f, "Cutoff frequency cannot be zero"), | |
} | |
} | |
} | |
struct DelayLine { | |
buffer: std::collections::VecDeque<f64>, | |
capacity: usize, | |
} | |
impl DelayLine { | |
fn new(capacity: usize) -> DelayLine { | |
// A delay line is a First-In-First-Out (FIFO) buffer that stores previous input samples in a FIR filter. | |
// It's purpose is to introduce a time delay between the input samples and their corresponding filter coefficients. | |
// The delay line maintains a fixed-length history of samples, preserving their order. | |
// This history is accessed and processed by the filter to perform temporal convolution and shape the filter's frequency response. | |
DelayLine { | |
buffer: std::collections::VecDeque::with_capacity(capacity), | |
capacity, | |
} | |
} | |
fn push(&mut self, input_sample: f64) { | |
self.buffer.push_back(input_sample); | |
while self.buffer.len() > self.capacity { | |
self.buffer.pop_front(); | |
} | |
} | |
fn clear(&mut self) { | |
self.buffer.clear(); | |
} | |
} | |
impl<'a> IntoIterator for &'a DelayLine { | |
type Item = &'a f64; | |
type IntoIter = std::collections::vec_deque::Iter<'a, f64>; | |
fn into_iter(self) -> Self::IntoIter { | |
self.buffer.iter() | |
} | |
} | |
pub struct LowPassFIRFilter { | |
coefficients: Vec<f64>, | |
delay_line: DelayLine, | |
} | |
impl LowPassFIRFilter { | |
/// Creates a new instance of a low-pass FIR filter. | |
/// | |
/// The `sample_rate_hz` parameter specifies the sample rate in Hertz (Hz) of the input signal. | |
/// The `cutoff_freq_hz` parameter specifies the cutoff frequency in Hertz (Hz) of the filter. | |
/// The `window` parameter specifies the windowing function to shape the filter's frequency response. | |
/// The `taps` parameter specifies the number of filter coefficients, which determines the filter length. | |
/// | |
/// # Errors | |
/// | |
/// Returns an `Err` variant with a `FilterError` if any of the following conditions are met: | |
/// - The `sample_rate_hz` is zero. | |
/// - The `cutoff_freq_hz` is zero. | |
/// - The `taps` is less than 3 or an even number. | |
/// - The `cutoff_freq_hz` exceeds the Nyquist frequency (half the sample rate). | |
/// | |
/// # Examples | |
/// | |
/// ``` | |
/// use my_library::filter::{LowPassFIRFilter, Window}; | |
/// | |
/// let sample_rate = 44100; // 44.1 kHz | |
/// let cutoff_freq = 5000; // 5 kHz | |
/// let window = Window::Hamming; | |
/// let taps = 21; | |
/// | |
/// match LowPassFIRFilter::new(sample_rate, cutoff_freq, window, taps) { | |
/// Ok(filter) => { | |
/// // Filter successfully created | |
/// // Use the filter to process input samples | |
/// } | |
/// Err(err) => { | |
/// // Error creating the filter | |
/// eprintln!("Filter creation error: {}", err); | |
/// } | |
/// } | |
/// ``` | |
pub fn new( | |
sample_rate_hz: u32, | |
cutoff_freq_hz: u32, | |
window: Window, | |
taps: u8, | |
) -> Result<Self, FilterError> { | |
use FilterError::*; | |
// Ensure sample_rate_hz is not zero | |
if sample_rate_hz == 0 { | |
return Err(ZeroSampleRate); | |
} | |
// Ensure cutoff_freq_hz is not zero | |
if cutoff_freq_hz == 0 { | |
return Err(ZeroCutoffFrequency); | |
} | |
// Ensure taps is at least 3 and an odd number. | |
// For the filter to exhibit linear phase characteristics it must be an odd length. | |
if taps < 3 || taps % 2 == 0 { | |
return Err(InvalidTaps(taps)); | |
} | |
// Calculate the Nyquist frequency | |
let nyquist_freq = sample_rate_hz / 2; | |
// Ensure cutoff_freq_hz is within the valid range | |
if cutoff_freq_hz > nyquist_freq { | |
return Err(InvalidCutoffFrequency(cutoff_freq_hz, nyquist_freq)); | |
} | |
let cutoff_freq = cutoff_freq_hz as f64 / sample_rate_hz as f64; | |
let coefficients = Self::sinc_window(cutoff_freq, window, taps as usize); | |
let delay_line = DelayLine::new(taps as usize); | |
Ok(Self { | |
coefficients, | |
delay_line, | |
}) | |
} | |
fn sinc_window(cutoff_freq: f64, window: Window, taps: usize) -> Vec<f64> { | |
// Generates the normalized filter coefficients for a low-pass FIR filter using the windowed sinc method. | |
// The window function is applied to shape the ideal frequency response and ensure a finite filter length. | |
let mut sum = 0.0; | |
let mut window: Vec<f64> = (0..taps) | |
.map(|index| { | |
let x = (index as f64 - (taps as f64 - 1.0) / 2.0) * cutoff_freq; | |
let sinc_value = Self::sinc(x); | |
let window_value = window.value(index, taps); | |
let sinc_window = sinc_value * window_value; | |
sum += sinc_window; | |
sinc_window | |
}) | |
.collect(); | |
window | |
.iter_mut() | |
.for_each(|sinc_window| *sinc_window /= sum); | |
window | |
} | |
fn sinc(x: f64) -> f64 { | |
// Calculates a single value of the sinc function for a given input `x`. | |
// The sinc function represents the desired frequency response of an ideal low-pass filter. | |
if x.abs() < f64::EPSILON { | |
1.0 | |
} else { | |
let pi_x = std::f64::consts::PI * x; | |
pi_x.sin() / pi_x | |
} | |
} | |
fn process_sample(&mut self, input_sample: f64) -> f64 { | |
// Processes an input sample through the filter via temporal convolution. | |
self.delay_line.push(input_sample); | |
let mut output_sample = 0.0; | |
for (coefficient, delay_line_sample) in self.coefficients.iter().zip(&self.delay_line) { | |
output_sample += coefficient * delay_line_sample; | |
} | |
output_sample | |
} | |
/// Applies the low-pass FIR filter to the provided input slice and returns the filtered output. | |
/// | |
/// # Arguments | |
/// | |
/// * `input` - A slice containing the input signal to be filtered. | |
/// | |
/// # Returns | |
/// | |
/// A new vector containing the filtered output signal. | |
/// | |
/// # Examples | |
/// | |
/// ``` | |
/// use my_library::filter::{LowPassFIRFilter, Window}; | |
/// | |
/// let sample_rate = 44100; // 44.1 kHz | |
/// let cutoff_freq = 5000; // 5 kHz | |
/// let window = Window::Hamming; | |
/// let taps = 21; | |
/// | |
/// let filter = LowPassFIRFilter::new(sample_rate, cutoff_freq, window, taps).unwrap(); | |
/// | |
/// let input_signal = vec![0.1, 0.5, 0.3, 0.8, 0.2]; | |
/// let filtered_output = filter.apply(&input_signal); | |
/// println!("Filtered output: {:?}", filtered_output); | |
/// ``` | |
pub fn apply(&mut self, input_samples: &[f64]) -> Vec<f64> { | |
input_samples | |
.iter() | |
.map(|&input_sample| self.process_sample(input_sample)) | |
.collect() | |
} | |
/// Clears the internal state of the filter, returning the leftover filtered buffered samples. | |
/// | |
/// # Examples | |
/// | |
/// ``` | |
/// use my_library::filter::{LowPassFIRFilter, Window}; | |
/// | |
/// let sample_rate = 44100; // 44.1 kHz | |
/// let cutoff_freq = 5000; // 5 kHz | |
/// let window = Window::Hamming; | |
/// let taps = 21; | |
/// | |
/// let mut filter = LowPassFIRFilter::new(sample_rate, cutoff_freq, window, taps).unwrap(); | |
/// | |
/// // Apply filter to some input data | |
/// let input_signal = vec![0.1, 0.5, 0.3, 0.8, 0.2]; | |
/// let filtered_output = filter.apply(&input_signal); | |
/// println!("Filtered output: {:?}", filtered_output); | |
/// | |
/// // Clear the filter state, returning the leftover filtered buffer samples and start filtering a new signal | |
/// let leftovers = filter.drain(); | |
/// println!("Leftover output: {:?}", leftovers); | |
/// | |
/// let new_input_signal = vec![0.2, 0.4, 0.6, 0.8, 1.0]; | |
/// let new_filtered_output = filter.apply(&new_input_signal); | |
/// println!("New filtered output: {:?}", new_filtered_output); | |
/// ``` | |
pub fn drain(&mut self) -> Vec<f64> { | |
let drain_cleaner = vec![0.0; self.coefficients.len()]; | |
let leftover_output_samples = self.apply(&drain_cleaner); | |
self.delay_line.clear(); | |
leftover_output_samples | |
} | |
/// Clears the internal state of the filter, discarding any buffered samples. | |
/// | |
/// # Examples | |
/// | |
/// ``` | |
/// use my_library::filter::{LowPassFIRFilter, Window}; | |
/// | |
/// let sample_rate = 44100; // 44.1 kHz | |
/// let cutoff_freq = 5000; // 5 kHz | |
/// let window = Window::Hamming; | |
/// let taps = 21; | |
/// | |
/// let mut filter = LowPassFIRFilter::new(sample_rate, cutoff_freq, window, taps).unwrap(); | |
/// | |
/// // Apply filter to some input data | |
/// let input_signal = vec![0.1, 0.5, 0.3, 0.8, 0.2]; | |
/// let filtered_output = filter.apply(&input_signal); | |
/// println!("Filtered output: {:?}", filtered_output); | |
/// | |
/// // Clear the filter state, discarding any buffered samples and start filtering a new signal | |
/// filter.clear(); | |
/// | |
/// let new_input_signal = vec![0.2, 0.4, 0.6, 0.8, 1.0]; | |
/// let new_filtered_output = filter.apply(&new_input_signal); | |
/// println!("New filtered output: {:?}", new_filtered_output); | |
/// ``` | |
pub fn clear(&mut self) { | |
self.delay_line.clear(); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment