90 lines
2.3 KiB
Rust
90 lines
2.3 KiB
Rust
use std::println;
|
|
|
|
use nalgebra::base::{DMatrix, DVector};
|
|
|
|
fn main() {
|
|
let num_coeffs = 6;
|
|
|
|
let response = generate_frequency_response(
|
|
8000.0,
|
|
&vec![(0.0, 1.0), (1900.0, 1.0), (3000.0, 0.0), (4000.0, 0.0)],
|
|
128,
|
|
);
|
|
println!("{:?}", response);
|
|
|
|
let matrix = generate_design_matrix(num_coeffs, 128);
|
|
|
|
println!("{} {}", matrix.ncols(), matrix.nrows());
|
|
|
|
let pseudo_inverse = matrix.pseudo_inverse(0.0).unwrap();
|
|
|
|
println!("{} {}", pseudo_inverse.ncols(), pseudo_inverse.nrows());
|
|
println!("{} {}", response.ncols(), response.nrows());
|
|
|
|
let coeffs = pseudo_inverse * response;
|
|
|
|
println!("[");
|
|
for i in 1..num_coeffs {
|
|
println!("{},", coeffs[num_coeffs - i])
|
|
}
|
|
for i in 0..num_coeffs {
|
|
println!("{},", coeffs[i])
|
|
}
|
|
println!("]");
|
|
}
|
|
|
|
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;
|
|
println!("Step size is {}Hz", step_size);
|
|
|
|
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
|
|
}
|