94 lines
2.7 KiB
Rust
94 lines
2.7 KiB
Rust
use nalgebra::base::{DMatrix, DVector};
|
|
use num::complex::Complex;
|
|
|
|
pub fn firls(lenght: usize, f_samp: f32, points: &Vec<(f32, f32)>) -> Result<Vec<f32>, &str> {
|
|
if lenght % 2 != 1 {
|
|
return Result::Err("Filter should have an odd length");
|
|
}
|
|
|
|
let half_lenght = lenght / 2 + 1;
|
|
|
|
let response = generate_frequency_response(f_samp, points, 2 * lenght);
|
|
|
|
let matrix = generate_design_matrix(half_lenght, 2 * lenght);
|
|
let pseudo_inverse = matrix.pseudo_inverse(0.0).unwrap();
|
|
let half_coeffs = pseudo_inverse * response;
|
|
|
|
let mut coeffs: Vec<f32> = Vec::with_capacity(lenght);
|
|
for i in 1..half_lenght {
|
|
coeffs.push(half_coeffs[half_lenght - i]);
|
|
}
|
|
for i in 0..half_lenght {
|
|
coeffs.push(half_coeffs[i]);
|
|
}
|
|
|
|
Result::Ok(coeffs)
|
|
}
|
|
|
|
pub fn frequency_shift_coeffs(coeffs: &Vec<f32>, f_samp: f32, f_center: f32) -> Vec<Complex<f32>> {
|
|
let mut result = Vec::with_capacity(coeffs.len());
|
|
|
|
let f0 = f_center / f_samp;
|
|
for i in 0..coeffs.len() {
|
|
let shifted_coeff = coeffs[i]
|
|
* (Complex::<f32>::new(0.0, 2.0) * std::f32::consts::PI * f0 * i as f32).exp();
|
|
result.push(shifted_coeff);
|
|
}
|
|
|
|
result
|
|
}
|
|
|
|
fn generate_frequency_response(
|
|
f_samp: f32,
|
|
points: &Vec<(f32, f32)>,
|
|
num_points: usize,
|
|
) -> DVector<f32> {
|
|
let step_size = f_samp / 2.0 / (num_points - 1) as f32;
|
|
|
|
let mut response = DVector::<f32>::zeros(num_points);
|
|
|
|
for step in 0..num_points {
|
|
let step_freq = step as f32 * step_size;
|
|
|
|
response[step] = interpolate_between_points(step_freq, &points)
|
|
}
|
|
|
|
response
|
|
}
|
|
|
|
fn interpolate_between_points(input: f32, points: &Vec<(f32, f32)>) -> f32 {
|
|
if input <= points.first().unwrap().0 {
|
|
return points.first().unwrap().1;
|
|
};
|
|
if input >= points.last().unwrap().0 {
|
|
return points.last().unwrap().1;
|
|
};
|
|
|
|
let mut result = f32::NAN;
|
|
for idx in 0..points.len() {
|
|
if input >= points[idx].0 && points[idx + 1].0 > input {
|
|
let slope = (points[idx + 1].1 - points[idx].1) / (points[idx + 1].0 - points[idx].0);
|
|
result = points[idx].1 + (input - points[idx].0) * slope;
|
|
break;
|
|
}
|
|
}
|
|
|
|
result
|
|
}
|
|
|
|
fn generate_design_matrix(num_coeffs: usize, num_freqs: usize) -> DMatrix<f32> {
|
|
let step_size = 1.0 / 2.0 / (num_freqs - 1) as f32;
|
|
|
|
let mut result = DMatrix::<f32>::zeros(num_freqs, num_coeffs);
|
|
result.fill_column(0, 1.0);
|
|
|
|
for row_idx in 0..num_freqs {
|
|
let omega = row_idx as f32 * step_size * 2.0 * std::f32::consts::PI;
|
|
for col_idx in 1..num_coeffs {
|
|
result[(row_idx, col_idx)] = 2.0 * (omega * col_idx as f32).cos()
|
|
}
|
|
}
|
|
|
|
result
|
|
}
|