diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..17454db --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,33 @@ +name: CI + +on: + push: + branches: + - main + - master + pull_request: + +jobs: + rust: + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Install stable toolchain + uses: dtolnay/rust-toolchain@stable + with: + components: rustfmt + + - name: Cache cargo artifacts + uses: Swatinem/rust-cache@v2 + + - name: Check formatting + run: cargo fmt --all --check + + - name: Build + run: cargo check --workspace --all-targets + + - name: Test + run: cargo test --workspace --all-targets diff --git a/Cargo.toml b/Cargo.toml index 9d79fda..a6f16e7 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,3 +1,6 @@ [workspace] members = ["utilities"] -resolver = "2" \ No newline at end of file +resolver = "2" + +[workspace.dependencies] +serde = { version = "1.0.228", features = ["derive"] } diff --git a/utilities/Cargo.toml b/utilities/Cargo.toml index 472fd26..5898309 100644 --- a/utilities/Cargo.toml +++ b/utilities/Cargo.toml @@ -3,4 +3,5 @@ name = "utilities" version = "0.1.0" edition = "2024" -[dependencies] \ No newline at end of file +[dependencies] +serde = { workspace = true } diff --git a/utilities/src/lib.rs b/utilities/src/lib.rs index e69de29..4fb6cfe 100644 --- a/utilities/src/lib.rs +++ b/utilities/src/lib.rs @@ -0,0 +1 @@ +pub mod lowpass; diff --git a/utilities/src/lowpass.rs b/utilities/src/lowpass.rs new file mode 100644 index 0000000..1f360c5 --- /dev/null +++ b/utilities/src/lowpass.rs @@ -0,0 +1,165 @@ +//! Lowpass filter. +//! +//! This module provides a simple first-order lowpass filter (alpha filter) +//! with constructors based on either a cutoff frequency or a direct alpha +//! coefficient. +//! +//! # Examples +//! +//! ```rust +//! use utilities::lowpass::LowPassFilter; +//! +//! // 5 Hz cutoff, 100 Hz sample rate. +//! let mut filter = LowPassFilter::from_frequency(5.0, 100.0); +//! +//! let y0 = filter.update(1.0); +//! let y1 = filter.update(1.0); +//! +//! assert!(y1 >= y0); +//! assert!(y1 <= 1.0); +//! ``` + +use serde::{Deserialize, Serialize}; + +/// First order lowpass filter, "alpha filter" +#[derive(Debug, Serialize, Deserialize)] +pub struct LowPassFilter { + /// Filter coefficient, 0 -> no update, 1 -> passthrough + alpha: f64, + /// Current filtered state + value: f64, +} + +impl LowPassFilter { + /// Create a lowpass filter from cutoff frequency and sample rate + #[inline] + pub fn from_frequency(cutoff_hz: f64, sample_rate: f64) -> Self { + // Avoid division by zero + if sample_rate.abs() < 1e-9 { + return LowPassFilter::from_alpha(0.0); + } + + let dt = 1.0 / sample_rate; + let rc = 1.0 / (2.0 * std::f64::consts::PI * cutoff_hz); + let alpha = dt / (rc + dt); + + let clamped_alpha = alpha.clamp(0.0, 1.0); + Self { + alpha: clamped_alpha, + value: 0.0, + } + } + + /// Create lowa pass filter from alpha coefficient + #[inline] + pub fn from_alpha(filter_alpha: f64) -> Self { + let clamped_alpha = filter_alpha.clamp(0.0, 1.0); + Self { + alpha: clamped_alpha, + value: 0.0, + } + } + + /// Update lowpass with new sample + #[inline] + pub fn update(&mut self, input: f64) -> f64 { + let new_value = self.value * (1.0 - self.alpha) + self.alpha * input; + self.value = new_value; + self.value + } + + /// Get current filtered value + #[inline] + pub fn get_value(&self) -> f64 { + self.value + } + + /// Get alpha coefficient + #[inline] + pub fn get_alpha(&self) -> f64 { + self.alpha + } +} + +#[cfg(test)] +mod tests { + use super::*; + const KINDA_SMALL_NUMBER: f64 = 1e-4; + + #[test] + fn convergence_test() { + let mut filter = LowPassFilter::from_alpha(0.1); + for _ in 0..1000 { + filter.update(3.0); + } + // Should converge near 3.0 + + assert!((filter.get_value() - 3.0).abs() < KINDA_SMALL_NUMBER); + } + + #[test] + fn alpha_edge_cases() { + let mut f = LowPassFilter::from_alpha(0.0); + assert_eq!(f.update(10.0), 0.0); // no change + + let mut f = LowPassFilter::from_alpha(1.0); + assert_eq!(f.update(10.0), 10.0); // passthrough + } + + #[test] + fn from_frequency_computes_alpha() { + let cutoff_hz = 5.0; + let sample_rate = 100.0; + let filter = LowPassFilter::from_frequency(cutoff_hz, sample_rate); + + let dt = 1.0 / sample_rate; + let rc = 1.0 / (2.0 * std::f64::consts::PI * cutoff_hz); + let expected_alpha = dt / (rc + dt); + + assert!((filter.get_alpha() - expected_alpha).abs() < KINDA_SMALL_NUMBER); + } + + #[test] + fn from_frequency_clamps_alpha_for_edge_cases() { + let cases = [(0.0, 100.0), (-10.0, 100.0), (10.0, 0.0), (10.0, -100.0)]; + + for (cutoff_hz, sample_rate) in cases { + let filter = LowPassFilter::from_frequency(cutoff_hz, sample_rate); + let alpha = filter.get_alpha(); + assert!( + (0.0..=1.0).contains(&alpha), + "alpha out of range for cutoff {cutoff_hz}, sample_rate {sample_rate}" + ); + } + } + + #[test] + fn from_frequency_monotonic_in_cutoff() { + let sample_rate = 100.0; + let low = LowPassFilter::from_frequency(1.0, sample_rate); + let high = LowPassFilter::from_frequency(10.0, sample_rate); + assert!(high.get_alpha() > low.get_alpha()); + } + + #[test] + fn step_response_moves_toward_input_without_overshoot() { + let mut filter = LowPassFilter::from_alpha(0.25); + let mut last = filter.get_value(); + for _ in 0..50 { + let value = filter.update(1.0); + assert!(value >= last, "response should be non-decreasing"); + assert!(value <= 1.0, "response should not overshoot input"); + last = value; + } + } + + #[test] + fn from_frequency_extremes_approximate_alpha_bounds() { + let sample_rate = 100.0; + let near_zero = LowPassFilter::from_frequency(1e-9, sample_rate); + let near_one = LowPassFilter::from_frequency(1e9, sample_rate); + + assert!(near_zero.get_alpha() < KINDA_SMALL_NUMBER); + assert!((1.0 - near_one.get_alpha()) < KINDA_SMALL_NUMBER); + } +}