First working filter

2023-06-18 14:43:04 +02:00 · 2023-06-18 14:43:04 +02:00 · eb6f22be87
commit eb6f22be87
5 changed files with 326 additions and 0 deletions
--- a/.gitignore
+++ b/.gitignore
@ -0,0 +1 @@
+/target
--- a/Cargo.lock
+++ b/Cargo.lock
@ -0,0 +1,192 @@
+# This file is automatically @generated by Cargo.
+# It is not intended for manual editing.
+version = 3
+
+[[package]]
+name = "approx"
+version = "0.5.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6"
+dependencies = [
+ "num-traits",
+]
+
+[[package]]
+name = "autocfg"
+version = "1.1.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa"
+
+[[package]]
+name = "bytemuck"
+version = "1.13.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "17febce684fd15d89027105661fec94afb475cb995fbc59d2865198446ba2eea"
+
+[[package]]
+name = "firls-rs"
+version = "0.1.0"
+dependencies = [
+ "nalgebra",
+]
+
+[[package]]
+name = "matrixmultiply"
+version = "0.3.7"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "090126dc04f95dc0d1c1c91f61bdd474b3930ca064c1edc8a849da2c6cbe1e77"
+dependencies = [
+ "autocfg",
+ "rawpointer",
+]
+
+[[package]]
+name = "nalgebra"
+version = "0.32.2"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d68d47bba83f9e2006d117a9a33af1524e655516b8919caac694427a6fb1e511"
+dependencies = [
+ "approx",
+ "matrixmultiply",
+ "nalgebra-macros",
+ "num-complex",
+ "num-rational",
+ "num-traits",
+ "simba",
+ "typenum",
+]
+
+[[package]]
+name = "nalgebra-macros"
+version = "0.2.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d232c68884c0c99810a5a4d333ef7e47689cfd0edc85efc9e54e1e6bf5212766"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn",
+]
+
+[[package]]
+name = "num-complex"
+version = "0.4.3"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "02e0d21255c828d6f128a1e41534206671e8c3ea0c62f32291e808dc82cff17d"
+dependencies = [
+ "num-traits",
+]
+
+[[package]]
+name = "num-integer"
+version = "0.1.45"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "225d3389fb3509a24c93f5c29eb6bde2586b98d9f016636dff58d7c6f7569cd9"
+dependencies = [
+ "autocfg",
+ "num-traits",
+]
+
+[[package]]
+name = "num-rational"
+version = "0.4.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "0638a1c9d0a3c0914158145bc76cff373a75a627e6ecbfb71cbe6f453a5a19b0"
+dependencies = [
+ "autocfg",
+ "num-integer",
+ "num-traits",
+]
+
+[[package]]
+name = "num-traits"
+version = "0.2.15"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "578ede34cf02f8924ab9447f50c28075b4d3e5b269972345e7e0372b38c6cdcd"
+dependencies = [
+ "autocfg",
+]
+
+[[package]]
+name = "paste"
+version = "1.0.12"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "9f746c4065a8fa3fe23974dd82f15431cc8d40779821001404d10d2e79ca7d79"
+
+[[package]]
+name = "proc-macro2"
+version = "1.0.60"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "dec2b086b7a862cf4de201096214fa870344cf922b2b30c167badb3af3195406"
+dependencies = [
+ "unicode-ident",
+]
+
+[[package]]
+name = "quote"
+version = "1.0.28"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "1b9ab9c7eadfd8df19006f1cf1a4aed13540ed5cbc047010ece5826e10825488"
+dependencies = [
+ "proc-macro2",
+]
+
+[[package]]
+name = "rawpointer"
+version = "0.2.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
+
+[[package]]
+name = "safe_arch"
+version = "0.7.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "62a7484307bd40f8f7ccbacccac730108f2cae119a3b11c74485b48aa9ea650f"
+dependencies = [
+ "bytemuck",
+]
+
+[[package]]
+name = "simba"
+version = "0.8.1"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "061507c94fc6ab4ba1c9a0305018408e312e17c041eb63bef8aa726fa33aceae"
+dependencies = [
+ "approx",
+ "num-complex",
+ "num-traits",
+ "paste",
+ "wide",
+]
+
+[[package]]
+name = "syn"
+version = "1.0.109"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "unicode-ident",
+]
+
+[[package]]
+name = "typenum"
+version = "1.16.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "497961ef93d974e23eb6f433eb5fe1b7930b659f06d12dec6fc44a8f554c0bba"
+
+[[package]]
+name = "unicode-ident"
+version = "1.0.9"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b15811caf2415fb889178633e7724bad2509101cde276048e013b9def5e51fa0"
+
+[[package]]
+name = "wide"
+version = "0.7.10"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "40018623e2dba2602a9790faba8d33f2ebdebf4b86561b83928db735f8784728"
+dependencies = [
+ "bytemuck",
+ "safe_arch",
+]
--- a/Cargo.toml
+++ b/Cargo.toml
@ -0,0 +1,9 @@
+[package]
+name = "firls-rs"
+version = "0.1.0"
+edition = "2021"
+
+# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
+
+[dependencies]
+nalgebra = "0.32.2"
--- a/src/main.rs
+++ b/src/main.rs
@ -0,0 +1,89 @@
+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
+}
--- a/test.py
+++ b/test.py
@ -0,0 +1,35 @@
+#!/usr/bin/env python3
+
+from scipy import signal
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+def main():
+    fs = 8000.0
+
+    coeffs = [
+            -0.0047233836,
+            0.04476544,
+            -0.038443737,
+            -0.0911246,
+            0.2894902,
+            0.6123418,
+            0.2894902,
+            -0.0911246,
+            -0.038443737,
+            0.04476544,
+            -0.0047233836,
+        ]
+
+    freq_space = np.linspace(-fs/2 / (fs/2)*np.pi, fs/2 / (fs/2)*np.pi, 512)
+    freqs, response = signal.freqz(coeffs, worN=freq_space)
+    response = 10 * np.log(abs(response))
+    plt.plot(fs/2*freqs/(np.pi), response)
+    plt.grid()
+    plt.show()
+
+ 
+
+if __name__ == '__main__':
+    main()