goertzel_filter/
lib.rs

1extern crate num;
2
3use num::Complex;
4use std::f64::consts::{PI, E};
5
6/// Computes the 'intermediate' filter sequence used in the Goertzel filter.
7fn intermediate_filter(input: &Vec<f64>, angular_freq: &f64) -> Vec<f64> {
8  let freq_term = 2.0 * angular_freq.cos();
9
10  // {\displaystyle s[n]=x[n]+2\cos(\omega _{0})s[n-1]-s[n-2].}
11  let mut intermediate_vec: Vec<f64> =
12    Vec::with_capacity(input.len());
13
14  // initialize intermediate vector with first two states
15  intermediate_vec.push(input[0]);
16  intermediate_vec.push(input[1] + freq_term * input[0]);
17
18  for (ind, val) in input.iter().enumerate().skip(2) {
19    let filter_term = val + freq_term * intermediate_vec[ind - 1] -
20      intermediate_vec[ind - 2];
21    intermediate_vec.push(filter_term);
22  }
23
24  intermediate_vec
25}
26
27/// Applies the two Goertzel filter steps to an input sequence.
28pub fn filter_naive(input: &Vec<f64>,
29                    linear_freq: f64) -> Vec<Complex<f64>> {
30  let angular_freq = linear_freq / 2.0 / PI;
31  
32  let intermediate_vec = intermediate_filter(&input, &angular_freq);
33
34  // {\displaystyle y[n]=s[n]-e^{-j\omega _{0}}s[n-1].}
35  let mut output_vec: Vec<Complex<f64>> =
36    Vec::with_capacity(input.len());
37
38  // initialize output vector with first state
39  output_vec.push(Complex::from(intermediate_vec[0]));
40  
41  // backwards from the usual signature, this is actually e^(jw).
42  let exponential_term = (- Complex::i() * angular_freq).expf(E);
43  
44  for (ind, val) in intermediate_vec.iter().enumerate().skip(1) {
45    output_vec.push(val - exponential_term * intermediate_vec[ind - 1]);
46  }
47
48  output_vec
49}
50
51fn bin_freq(linear_freq: f64, signal_length: usize) -> (usize, f64) {
52  let signal_len = signal_length as f64;
53
54  let angular_freq = linear_freq / 2.0 / PI;
55  
56  let approx_bin_num = angular_freq * &signal_len / 2.0 / PI;
57  let bin_num: usize = (approx_bin_num) as usize;
58  let bin_freq = bin_num as f64 / &signal_len * 2.0 * PI;
59    
60  if approx_bin_num != bin_num as f64 {
61    println!("Warning: aliasing frequency to {} to fit in bin...",
62             &bin_freq * 2.0 * PI);
63  }
64
65  (bin_num, bin_freq)
66}
67
68/// Computes the DFT term of a signal for a single given frequency.
69pub fn dft(input: &Vec<f64>, linear_freq: f64) -> Complex<f64> {
70  // Constrain frequency to 2 pi k / N
71  let bin_freq = bin_freq(linear_freq, input.len()).1;
72  
73  let mut intermediate_vec = intermediate_filter(&input, &bin_freq);
74  intermediate_vec.pop();
75  
76  // backwards from the usual signature, this is actually e^(jw).
77  let exponential_term = (Complex::i() * bin_freq).expf(E);
78  
79  exponential_term * Complex::from(intermediate_vec.pop().unwrap()) -
80    Complex::from(intermediate_vec.pop().unwrap())
81}
82
83/// Computes the power at a single DFT frequency.
84pub fn dft_power(input: &Vec<f64>, linear_freq: f64) -> f64 {
85  // Constrain frequency to 2 pi k / N
86  let bin_freq = bin_freq(linear_freq, input.len()).1;
87
88  // sequence s[n], where we throw away s[N]
89  let mut intermediate_vec = intermediate_filter(&input, &bin_freq);
90  intermediate_vec.pop();
91  
92  // s[n - 1]
93  let state_1 = intermediate_vec.pop().unwrap();
94  // s[n - 2]
95  let state_2 = intermediate_vec.pop().unwrap();
96  
97  state_1.powi(2) + state_2.powi(2) - 2.0 * bin_freq.cos() * state_1 * state_2
98}
goertzel_filter/lib.rs

goertzel_filter/
lib.rs
