diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 709ca3d..9ca6630 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -27,11 +27,14 @@ jobs: - name: Check code formatting run: cargo fmt -- --check - - name: Run Clippy Linter - run: cargo clippy --all-targets --all-features -- -D warnings + - name: Run Clippy Linter (Standard f64) + run: cargo clippy --all-targets --features std,serde,ffi -- -D warnings + + - name: Run Clippy Linter (Single-Precision f32) + run: cargo clippy --lib --features single-precision,ffi -- -D warnings - name: Run Test Suite (Standard Library) - run: cargo test --all-features --verbose + run: cargo test --features std,serde,ffi --verbose - name: Verify Pure #![no_std] Core Compilation (Bare-Metal target) run: cargo check --no-default-features --target thumbv7em-none-eabihf @@ -39,3 +42,9 @@ jobs: - name: Verify Bare-Metal Release Build & Hardening (panic=abort) run: cargo build --release --no-default-features --target thumbv7em-none-eabihf + - name: Verify Cortex-M Single-Precision FPU Target (#![no_std] f32 build) + run: cargo build --release --no-default-features --features single-precision --target thumbv7em-none-eabihf + + - name: Run Test Suite (Single-Precision Mode) + run: cargo test --lib --features single-precision --verbose + diff --git a/Cargo.toml b/Cargo.toml index 387f048..f35b804 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -29,6 +29,7 @@ default = ["std", "ffi"] std = [] serde = ["dep:serde"] ffi = [] +single-precision = [] [profile.dev] panic = "abort" diff --git a/README.md b/README.md index 868f9d9..82f0e5c 100644 --- a/README.md +++ b/README.md @@ -55,6 +55,13 @@ With optional `serde` support for state persistence and telemetry serialization: aegis-filter = { version = "0.4.0", features = ["serde"] } ``` +For **ARM Cortex-M4 / Cortex-M33** and single-precision hardware FPUs, enable `single-precision` to compile state representations, covariances, and math routines as `f32`: + +```toml +[dependencies] +aegis-filter = { version = "0.4.0", default-features = false, features = ["single-precision"] } +``` + --- ## Example Usage diff --git a/include/aegis_filter.h b/include/aegis_filter.h index 029c7b3..84e5c8a 100644 --- a/include/aegis_filter.h +++ b/include/aegis_filter.h @@ -14,6 +14,23 @@ #include #include +/** + * @brief Configurable floating-point precision type. + * + * Defaults to aegis_real (f64). Define AEGIS_SINGLE_PRECISION or compile with + * the `single-precision` feature to switch to float (f32) for ARM Cortex-M + * single-precision FPUs. + */ +#if defined(AEGIS_SINGLE_PRECISION) +typedef float aegis_real; +#define AEGIS_DEFAULT_GATE_SIGMA 3.0f +#define AEGIS_GATE_DISABLE (-1.0f) +#else +typedef aegis_real aegis_real; +#define AEGIS_DEFAULT_GATE_SIGMA 3.0 +#define AEGIS_GATE_DISABLE (-1.0) +#endif + #ifdef __cplusplus extern "C" { #endif @@ -45,9 +62,7 @@ typedef enum { AEGIS_FUSION_UNREGISTERED = 5 /**< Unknown sensor ID */ } AegisFusionStatusCode; -/* Sentinel constants for outlier gating */ -#define AEGIS_DEFAULT_GATE_SIGMA 3.0 -#define AEGIS_GATE_DISABLE (-1.0) +/* Sentinel constants defined above with aegis_real */ /* Forward-declared opaque handles */ typedef struct AegisScalarFilter AegisScalarFilter; @@ -70,10 +85,10 @@ typedef struct AegisFusion4 AegisFusion4; */ AegisStatusCode aegis_scalar_filter_init( AegisScalarFilter* filter, - double initial_state, - double initial_variance, - double process_noise_q, - double measurement_noise_r + aegis_real initial_state, + aegis_real initial_variance, + aegis_real process_noise_q, + aegis_real measurement_noise_r ); /** @@ -85,7 +100,7 @@ AegisStatusCode aegis_scalar_filter_init( */ AegisStatusCode aegis_scalar_filter_set_default_gate( AegisScalarFilter* filter, - double gate_threshold_sigma + aegis_real gate_threshold_sigma ); /** @@ -93,7 +108,7 @@ AegisStatusCode aegis_scalar_filter_set_default_gate( */ AegisStatusCode aegis_scalar_filter_predict( AegisScalarFilter* filter, - double control_input + aegis_real control_input ); /** @@ -108,20 +123,20 @@ AegisStatusCode aegis_scalar_filter_predict( */ AegisStatusCode aegis_scalar_filter_update( AegisScalarFilter* filter, - double measurement, - double gate_threshold_sigma, - double* out_state + aegis_real measurement, + aegis_real gate_threshold_sigma, + aegis_real* out_state ); /** * @brief Retrieves current state mean. */ -double aegis_scalar_filter_get_state(const AegisScalarFilter* filter); +aegis_real aegis_scalar_filter_get_state(const AegisScalarFilter* filter); /** * @brief Retrieves current error variance. */ -double aegis_scalar_filter_get_variance(const AegisScalarFilter* filter); +aegis_real aegis_scalar_filter_get_variance(const AegisScalarFilter* filter); /* ========================================================================= */ /* 2. H-INFINITY MINIMAX ROBUST FILTER (Bounded Disturbance) */ @@ -139,11 +154,11 @@ double aegis_scalar_filter_get_variance(const AegisScalarFilter* filter); */ AegisStatusCode aegis_h_inf_filter_init( AegisHInfFilter* filter, - double initial_state, - double initial_variance, - double process_bound_q, - double measurement_bound_r, - double gamma + aegis_real initial_state, + aegis_real initial_variance, + aegis_real process_bound_q, + aegis_real measurement_bound_r, + aegis_real gamma ); /** @@ -151,7 +166,7 @@ AegisStatusCode aegis_h_inf_filter_init( */ AegisStatusCode aegis_h_inf_filter_predict( AegisHInfFilter* filter, - double control_input + aegis_real control_input ); /** @@ -159,19 +174,19 @@ AegisStatusCode aegis_h_inf_filter_predict( */ AegisStatusCode aegis_h_inf_filter_update( AegisHInfFilter* filter, - double measurement, - double* out_state + aegis_real measurement, + aegis_real* out_state ); /** * @brief Retrieves current H-infinity state estimate. */ -double aegis_h_inf_filter_get_state(const AegisHInfFilter* filter); +aegis_real aegis_h_inf_filter_get_state(const AegisHInfFilter* filter); /** * @brief Retrieves current error bound (P). */ -double aegis_h_inf_filter_get_bound(const AegisHInfFilter* filter); +aegis_real aegis_h_inf_filter_get_bound(const AegisHInfFilter* filter); /* ========================================================================= */ /* 3. MULTI-SENSOR FDIR FUSION ENGINE (4 Channels) */ @@ -182,9 +197,9 @@ double aegis_h_inf_filter_get_bound(const AegisHInfFilter* filter); */ AegisStatusCode aegis_fusion4_init( AegisFusion4* fusion, - double x0, - double p0, - double q + aegis_real x0, + aegis_real p0, + aegis_real q ); /** @@ -200,9 +215,9 @@ AegisStatusCode aegis_fusion4_init( AegisStatusCode aegis_fusion4_register_sensor( AegisFusion4* fusion, uint8_t sensor_id, - double variance, + aegis_real variance, uint32_t max_consecutive_anomalies, - double gate_threshold_sigma, + aegis_real gate_threshold_sigma, uint32_t auto_recovery_threshold ); @@ -211,7 +226,7 @@ AegisStatusCode aegis_fusion4_register_sensor( */ AegisStatusCode aegis_fusion4_predict( AegisFusion4* fusion, - double control_input + aegis_real control_input ); /** @@ -225,24 +240,24 @@ AegisStatusCode aegis_fusion4_predict( AegisStatusCode aegis_fusion4_fuse( AegisFusion4* fusion, uint8_t sensor_id, - double value, + aegis_real value, AegisFusionStatusCode* out_status ); /** * @brief Retrieves fused state estimate. */ -double aegis_fusion4_get_state(const AegisFusion4* fusion); +aegis_real aegis_fusion4_get_state(const AegisFusion4* fusion); /** * @brief Retrieves fused error covariance. */ -double aegis_fusion4_get_variance(const AegisFusion4* fusion); +aegis_real aegis_fusion4_get_variance(const AegisFusion4* fusion); /** * @brief Retrieves real-time health score of a sensor (1.0 = nominal, 0.0 = quarantined). */ -double aegis_fusion4_get_sensor_health(const AegisFusion4* fusion, uint8_t sensor_id); +aegis_real aegis_fusion4_get_sensor_health(const AegisFusion4* fusion, uint8_t sensor_id); #ifdef __cplusplus } diff --git a/src/common.rs b/src/common.rs index 94e1d0a..cb4289a 100644 --- a/src/common.rs +++ b/src/common.rs @@ -1,7 +1,15 @@ //! Core mathematical types, errors, and `#![no_std]` floating point helpers. +#[cfg(not(feature = "single-precision"))] +/// Type alias for floating-point precision used across aegis-filter (f64 default). +pub type Real = f64; + +#[cfg(feature = "single-precision")] +/// Type alias for floating-point precision used across aegis-filter (f32 for single-precision FPUs). +pub type Real = f32; + /// Standard recommended 3-sigma innovation threshold for outlier and anomaly rejection. -pub const DEFAULT_OUTLIER_GATE_SIGMA: f64 = 3.0; +pub const DEFAULT_OUTLIER_GATE_SIGMA: Real = 3.0; #[derive(Debug, Clone, Copy, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] @@ -50,11 +58,11 @@ impl core::fmt::Display for FilterError { #[cfg(feature = "std")] impl std::error::Error for FilterError {} -/// Trait providing safe, deterministic `#![no_std]` math operations for f64. +/// Trait providing safe, deterministic `#![no_std]` math operations for floating-point types. pub trait FloatOps { fn is_valid_float(&self) -> bool; - fn abs_val(self) -> f64; - fn sqrt_val(self) -> f64; + fn abs_val(self) -> Self; + fn sqrt_val(self) -> Self; } impl FloatOps for f64 { @@ -97,3 +105,44 @@ impl FloatOps for f64 { } } } + +impl FloatOps for f32 { + #[inline(always)] + fn is_valid_float(&self) -> bool { + self.is_finite() + } + + #[inline(always)] + fn abs_val(self) -> f32 { + f32::from_bits(self.to_bits() & !(1u32 << 31)) + } + + #[inline] + fn sqrt_val(self) -> f32 { + #[cfg(feature = "std")] + { + self.sqrt() + } + #[cfg(not(feature = "std"))] + { + if self <= 0.0 { + return 0.0; + } + if !self.is_finite() { + return self; + } + // Fast IEEE-754 single-precision exponent halving initial guess, + // followed by quadratic Newton-Raphson iterations. + let init_bits = (self.to_bits() >> 1) + (127u32 << 22); + let mut curr = f32::from_bits(init_bits); + for _ in 0..6 { + let prev = curr; + curr = 0.5 * (curr + self / curr); + if (curr - prev).abs_val() <= 1e-6 * curr { + break; + } + } + curr + } + } +} diff --git a/src/eskf.rs b/src/eskf.rs index 778205e..e989b7f 100644 --- a/src/eskf.rs +++ b/src/eskf.rs @@ -23,7 +23,7 @@ //! - First-order exponential map rotation updates and error-reset covariance correction. //! - Joseph-form stabilized scalar and 3-axis observation updates. -use crate::common::{FilterError, FloatOps, DEFAULT_OUTLIER_GATE_SIGMA}; +use crate::common::{FilterError, FloatOps, Real, DEFAULT_OUTLIER_GATE_SIGMA}; /// Standard gravitational acceleration vector in Earth navigation frame (z-down or z-up). pub const EARTH_GRAVITY_Z_UP: Vector3 = Vector3::new(0.0, 0.0, -9.80665); @@ -32,16 +32,16 @@ pub const EARTH_GRAVITY_Z_UP: Vector3 = Vector3::new(0.0, 0.0, -9.80665); #[derive(Debug, Clone, Copy, PartialEq)] #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] pub struct Vector3 { - pub x: f64, - pub y: f64, - pub z: f64, + pub x: Real, + pub y: Real, + pub z: Real, } impl Vector3 { pub const ZERO: Self = Self::new(0.0, 0.0, 0.0); #[inline(always)] - pub const fn new(x: f64, y: f64, z: f64) -> Self { + pub const fn new(x: Real, y: Real, z: Real) -> Self { Self { x, y, z } } @@ -61,12 +61,12 @@ impl Vector3 { } #[inline(always)] - pub fn scale(&self, s: f64) -> Self { + pub fn scale(&self, s: Real) -> Self { Self::new(self.x * s, self.y * s, self.z * s) } #[inline(always)] - pub fn dot(&self, rhs: &Self) -> f64 { + pub fn dot(&self, rhs: &Self) -> Real { self.x * rhs.x + self.y * rhs.y + self.z * rhs.z } @@ -80,12 +80,12 @@ impl Vector3 { } #[inline(always)] - pub fn norm_squared(&self) -> f64 { + pub fn norm_squared(&self) -> Real { self.dot(self) } #[inline(always)] - pub fn norm(&self) -> f64 { + pub fn norm(&self) -> Real { self.norm_squared().sqrt_val() } @@ -103,17 +103,17 @@ impl Vector3 { #[derive(Debug, Clone, Copy, PartialEq)] #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] pub struct Quaternion { - pub w: f64, - pub x: f64, - pub y: f64, - pub z: f64, + pub w: Real, + pub x: Real, + pub y: Real, + pub z: Real, } impl Quaternion { pub const IDENTITY: Self = Self::new(1.0, 0.0, 0.0, 0.0); #[inline(always)] - pub const fn new(w: f64, x: f64, y: f64, z: f64) -> Self { + pub const fn new(w: Real, x: Real, y: Real, z: Real) -> Self { Self { w, x, y, z } } @@ -126,12 +126,12 @@ impl Quaternion { } #[inline(always)] - pub fn norm_squared(&self) -> f64 { + pub fn norm_squared(&self) -> Real { self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z } #[inline(always)] - pub fn norm(&self) -> f64 { + pub fn norm(&self) -> Real { self.norm_squared().sqrt_val() } @@ -205,7 +205,7 @@ impl Quaternion { /// Constructs 3x3 direction cosine matrix (rotation matrix from body to nav frame). #[inline] - pub fn to_rotation_matrix(&self) -> [[f64; 3]; 3] { + pub fn to_rotation_matrix(&self) -> [[Real; 3]; 3] { let (w, x, y, z) = (self.w, self.x, self.y, self.z); [ [ @@ -232,21 +232,21 @@ impl Quaternion { #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] pub struct EskfNoiseParameters { /// Accelerometer continuous noise density ($m/s^2 / \sqrt{Hz}$). - pub accel_noise: f64, + pub accel_noise: Real, /// Gyroscope continuous noise density ($rad/s / \sqrt{Hz}$). - pub gyro_noise: f64, + pub gyro_noise: Real, /// Accelerometer bias random walk ($m/s^3 / \sqrt{Hz}$). - pub accel_bias_noise: f64, + pub accel_bias_noise: Real, /// Gyroscope bias random walk ($rad/s^2 / \sqrt{Hz}$). - pub gyro_bias_noise: f64, + pub gyro_bias_noise: Real, } impl EskfNoiseParameters { pub const fn new( - accel_noise: f64, - gyro_noise: f64, - accel_bias_noise: f64, - gyro_bias_noise: f64, + accel_noise: Real, + gyro_noise: Real, + accel_bias_noise: Real, + gyro_bias_noise: Real, ) -> Self { Self { accel_noise, @@ -290,7 +290,7 @@ impl EskfNominalState { #[derive(Debug, Clone)] pub struct ErrorStateKalmanFilter { state: EskfNominalState, - cov: [[f64; 15]; 15], + cov: [[Real; 15]; 15], noise: EskfNoiseParameters, gravity: Vector3, } @@ -299,7 +299,7 @@ impl ErrorStateKalmanFilter { /// Initializes ESKF with nominal state, diagonal covariance, and process noise parameters. pub fn new( initial_state: EskfNominalState, - initial_cov_diag: [f64; 15], + initial_cov_diag: [Real; 15], noise: EskfNoiseParameters, gravity: Vector3, ) -> Result { @@ -339,7 +339,7 @@ impl ErrorStateKalmanFilter { } #[inline(always)] - pub fn cov(&self) -> &[[f64; 15]; 15] { + pub fn cov(&self) -> &[[Real; 15]; 15] { &self.cov } @@ -348,7 +348,7 @@ impl ErrorStateKalmanFilter { &mut self, accel_meas: &Vector3, gyro_meas: &Vector3, - dt: f64, + dt: Real, ) -> Result<(), FilterError> { if !accel_meas.is_valid() || !gyro_meas.is_valid() || !dt.is_valid_float() || dt <= 0.0 { return Err(FilterError::NonFiniteInput); @@ -494,10 +494,10 @@ impl ErrorStateKalmanFilter { /// Performs scalar measurement update with Mahalanobis outlier gating and Joseph-form covariance reset. fn update_scalar( &mut self, - y: f64, - h: [f64; 15], - r_var: f64, - gate_sigma: Option, + y: Real, + h: [Real; 15], + r_var: Real, + gate_sigma: Option, ) -> Result<(), FilterError> { if !y.is_valid_float() || !r_var.is_valid_float() || r_var <= 0.0 { return Err(FilterError::NonFiniteInput); @@ -588,8 +588,8 @@ impl ErrorStateKalmanFilter { pub fn update_position( &mut self, meas_pos: &Vector3, - noise_var: f64, - gate_sigma: Option, + noise_var: Real, + gate_sigma: Option, ) -> Result<(), FilterError> { let y_x = meas_pos.x - self.state.position.x; let mut h_x = [0.0; 15]; @@ -613,8 +613,8 @@ impl ErrorStateKalmanFilter { pub fn update_velocity( &mut self, meas_vel: &Vector3, - noise_var: f64, - gate_sigma: Option, + noise_var: Real, + gate_sigma: Option, ) -> Result<(), FilterError> { let y_x = meas_vel.x - self.state.velocity.x; let mut h_x = [0.0; 15]; @@ -639,8 +639,8 @@ impl ErrorStateKalmanFilter { &mut self, mag_body: &Vector3, mag_ref_nav: &Vector3, - noise_var: f64, - gate_sigma: Option, + noise_var: Real, + gate_sigma: Option, ) -> Result<(), FilterError> { if !mag_body.is_valid() || !mag_ref_nav.is_valid() { return Err(FilterError::NonFiniteInput); diff --git a/src/ffi.rs b/src/ffi.rs index 47c660c..1159b52 100644 --- a/src/ffi.rs +++ b/src/ffi.rs @@ -3,7 +3,7 @@ //! Exposes zero-allocation, deterministic `#![no_std]` state estimation and FDIR pipelines //! with C-compatible ABI (`extern "C"`). -use crate::common::FilterError; +use crate::common::{FilterError, Real}; use crate::fusion::{FusionStatus, MultiSensorFusion1D, SensorMeasurement, SensorSource}; use crate::linear::ScalarKalmanFilter; use crate::robust::HInfinityFilter1D; @@ -94,10 +94,10 @@ pub struct AegisScalarFilter { #[no_mangle] pub unsafe extern "C" fn aegis_scalar_filter_init( filter: *mut AegisScalarFilter, - initial_state: f64, - initial_variance: f64, - process_noise_q: f64, - measurement_noise_r: f64, + initial_state: Real, + initial_variance: Real, + process_noise_q: Real, + measurement_noise_r: Real, ) -> AegisStatusCode { if filter.is_null() { return AegisStatusCode::NullPointer; @@ -124,7 +124,7 @@ pub unsafe extern "C" fn aegis_scalar_filter_init( #[no_mangle] pub unsafe extern "C" fn aegis_scalar_filter_predict( filter: *mut AegisScalarFilter, - control_input: f64, + control_input: Real, ) -> AegisStatusCode { if filter.is_null() { return AegisStatusCode::NullPointer; @@ -137,10 +137,10 @@ pub unsafe extern "C" fn aegis_scalar_filter_predict( } /// Default recommended 3-sigma innovation gating threshold for adversarial outlier rejection. -pub const AEGIS_DEFAULT_GATE_SIGMA: f64 = 3.0; +pub const AEGIS_DEFAULT_GATE_SIGMA: Real = 3.0; /// Sentinel value to explicitly disable innovation gating in C API calls. -pub const AEGIS_GATE_DISABLE: f64 = -1.0; +pub const AEGIS_GATE_DISABLE: Real = -1.0; /// Configures persistent default outlier gating threshold on the scalar filter (fail-closed posture). /// @@ -152,7 +152,7 @@ pub const AEGIS_GATE_DISABLE: f64 = -1.0; #[no_mangle] pub unsafe extern "C" fn aegis_scalar_filter_set_default_gate( filter: *mut AegisScalarFilter, - gate_threshold_sigma: f64, + gate_threshold_sigma: Real, ) -> AegisStatusCode { if filter.is_null() { return AegisStatusCode::NullPointer; @@ -184,9 +184,9 @@ pub unsafe extern "C" fn aegis_scalar_filter_set_default_gate( #[no_mangle] pub unsafe extern "C" fn aegis_scalar_filter_update( filter: *mut AegisScalarFilter, - measurement: f64, - gate_threshold_sigma: f64, - out_state: *mut f64, + measurement: Real, + gate_threshold_sigma: Real, + out_state: *mut Real, ) -> AegisStatusCode { if filter.is_null() { return AegisStatusCode::NullPointer; @@ -216,9 +216,9 @@ pub unsafe extern "C" fn aegis_scalar_filter_update( /// # Safety /// Caller must ensure `filter` points to a valid `AegisScalarFilter` or null. #[no_mangle] -pub unsafe extern "C" fn aegis_scalar_filter_get_state(filter: *const AegisScalarFilter) -> f64 { +pub unsafe extern "C" fn aegis_scalar_filter_get_state(filter: *const AegisScalarFilter) -> Real { if filter.is_null() { - return f64::NAN; + return Real::NAN; } (*filter).inner.state() } @@ -228,9 +228,11 @@ pub unsafe extern "C" fn aegis_scalar_filter_get_state(filter: *const AegisScala /// # Safety /// Caller must ensure `filter` points to a valid `AegisScalarFilter` or null. #[no_mangle] -pub unsafe extern "C" fn aegis_scalar_filter_get_variance(filter: *const AegisScalarFilter) -> f64 { +pub unsafe extern "C" fn aegis_scalar_filter_get_variance( + filter: *const AegisScalarFilter, +) -> Real { if filter.is_null() { - return f64::NAN; + return Real::NAN; } (*filter).inner.variance() } @@ -252,11 +254,11 @@ pub struct AegisHInfFilter { #[no_mangle] pub unsafe extern "C" fn aegis_h_inf_filter_init( filter: *mut AegisHInfFilter, - initial_state: f64, - initial_variance: f64, - process_bound_q: f64, - measurement_bound_r: f64, - gamma: f64, + initial_state: Real, + initial_variance: Real, + process_bound_q: Real, + measurement_bound_r: Real, + gamma: Real, ) -> AegisStatusCode { if filter.is_null() { return AegisStatusCode::NullPointer; @@ -284,7 +286,7 @@ pub unsafe extern "C" fn aegis_h_inf_filter_init( #[no_mangle] pub unsafe extern "C" fn aegis_h_inf_filter_predict( filter: *mut AegisHInfFilter, - control_input: f64, + control_input: Real, ) -> AegisStatusCode { if filter.is_null() { return AegisStatusCode::NullPointer; @@ -303,8 +305,8 @@ pub unsafe extern "C" fn aegis_h_inf_filter_predict( #[no_mangle] pub unsafe extern "C" fn aegis_h_inf_filter_update( filter: *mut AegisHInfFilter, - measurement: f64, - out_state: *mut f64, + measurement: Real, + out_state: *mut Real, ) -> AegisStatusCode { if filter.is_null() { return AegisStatusCode::NullPointer; @@ -326,9 +328,9 @@ pub unsafe extern "C" fn aegis_h_inf_filter_update( /// # Safety /// Caller must ensure `filter` points to a valid `AegisHInfFilter` or null. #[no_mangle] -pub unsafe extern "C" fn aegis_h_inf_filter_get_state(filter: *const AegisHInfFilter) -> f64 { +pub unsafe extern "C" fn aegis_h_inf_filter_get_state(filter: *const AegisHInfFilter) -> Real { if filter.is_null() { - return f64::NAN; + return Real::NAN; } (*filter).inner.state() } @@ -338,9 +340,9 @@ pub unsafe extern "C" fn aegis_h_inf_filter_get_state(filter: *const AegisHInfFi /// # Safety /// Caller must ensure `filter` points to a valid `AegisHInfFilter` or null. #[no_mangle] -pub unsafe extern "C" fn aegis_h_inf_filter_get_bound(filter: *const AegisHInfFilter) -> f64 { +pub unsafe extern "C" fn aegis_h_inf_filter_get_bound(filter: *const AegisHInfFilter) -> Real { if filter.is_null() { - return f64::NAN; + return Real::NAN; } (*filter).inner.bound() } @@ -362,9 +364,9 @@ pub struct AegisFusion4 { #[no_mangle] pub unsafe extern "C" fn aegis_fusion4_init( fusion: *mut AegisFusion4, - x0: f64, - p0: f64, - q: f64, + x0: Real, + p0: Real, + q: Real, ) -> AegisStatusCode { if fusion.is_null() { return AegisStatusCode::NullPointer; @@ -387,9 +389,9 @@ pub unsafe extern "C" fn aegis_fusion4_init( pub unsafe extern "C" fn aegis_fusion4_register_sensor( fusion: *mut AegisFusion4, sensor_id: u8, - variance: f64, + variance: Real, max_consecutive_anomalies: u32, - gate_threshold_sigma: f64, + gate_threshold_sigma: Real, auto_recovery_threshold: u32, ) -> AegisStatusCode { if fusion.is_null() { @@ -425,7 +427,7 @@ pub unsafe extern "C" fn aegis_fusion4_register_sensor( #[no_mangle] pub unsafe extern "C" fn aegis_fusion4_predict( fusion: *mut AegisFusion4, - control_input: f64, + control_input: Real, ) -> AegisStatusCode { if fusion.is_null() { return AegisStatusCode::NullPointer; @@ -445,7 +447,7 @@ pub unsafe extern "C" fn aegis_fusion4_predict( pub unsafe extern "C" fn aegis_fusion4_fuse( fusion: *mut AegisFusion4, sensor_id: u8, - value: f64, + value: Real, out_status: *mut AegisFusionStatusCode, ) -> AegisStatusCode { if fusion.is_null() { @@ -469,9 +471,9 @@ pub unsafe extern "C" fn aegis_fusion4_fuse( /// # Safety /// Caller must ensure `fusion` points to a valid `AegisFusion4` or null. #[no_mangle] -pub unsafe extern "C" fn aegis_fusion4_get_state(fusion: *const AegisFusion4) -> f64 { +pub unsafe extern "C" fn aegis_fusion4_get_state(fusion: *const AegisFusion4) -> Real { if fusion.is_null() { - return f64::NAN; + return Real::NAN; } (*fusion).inner.x } @@ -481,9 +483,9 @@ pub unsafe extern "C" fn aegis_fusion4_get_state(fusion: *const AegisFusion4) -> /// # Safety /// Caller must ensure `fusion` points to a valid `AegisFusion4` or null. #[no_mangle] -pub unsafe extern "C" fn aegis_fusion4_get_variance(fusion: *const AegisFusion4) -> f64 { +pub unsafe extern "C" fn aegis_fusion4_get_variance(fusion: *const AegisFusion4) -> Real { if fusion.is_null() { - return f64::NAN; + return Real::NAN; } (*fusion).inner.p } @@ -496,11 +498,14 @@ pub unsafe extern "C" fn aegis_fusion4_get_variance(fusion: *const AegisFusion4) pub unsafe extern "C" fn aegis_fusion4_get_sensor_health( fusion: *const AegisFusion4, sensor_id: u8, -) -> f64 { +) -> Real { if fusion.is_null() { - return f64::NAN; + return Real::NAN; } - (*fusion).inner.sensor_health(sensor_id).unwrap_or(f64::NAN) + (*fusion) + .inner + .sensor_health(sensor_id) + .unwrap_or(Real::NAN) } #[cfg(test)] diff --git a/src/fusion.rs b/src/fusion.rs index c60bc89..56e1c99 100644 --- a/src/fusion.rs +++ b/src/fusion.rs @@ -3,7 +3,7 @@ //! Designed for mission-critical aerospace, robotics, and maritime telemetry where redundant sensors //! (e.g., GPS, Barometer, Rangefinder/LiDAR) must be fused safely with real-time adversarial isolation. -use crate::common::{FilterError, FloatOps}; +use crate::common::{FilterError, FloatOps, Real}; /// Represents an individual sensor source participating in the multi-sensor fusion pipeline. #[derive(Debug, Clone, Copy)] @@ -12,9 +12,9 @@ pub struct SensorSource { /// Sensor unique identifier pub id: u8, /// Measurement noise variance (R) - pub variance: f64, + pub variance: Real, /// 3-sigma innovation gating threshold - pub gate_threshold: f64, + pub gate_threshold: Real, /// Consecutive anomaly count for automated isolation pub anomaly_count: u32, /// Maximum allowable consecutive anomalies before quarantine @@ -22,7 +22,7 @@ pub struct SensorSource { /// Whether this sensor is currently isolated / quarantined pub is_quarantined: bool, /// Health score from 0.0 (failed) to 1.0 (nominal) - pub health_score: f64, + pub health_score: Real, /// Consecutive healthy measurements observed while quarantined pub consecutive_healthy: u32, /// Threshold of consecutive healthy measurements required to auto-recover (0 to disable) @@ -31,7 +31,7 @@ pub struct SensorSource { impl SensorSource { /// Creates a new sensor source configuration. - pub fn new(id: u8, variance: f64, max_anomalies: u32) -> Result { + pub fn new(id: u8, variance: Real, max_anomalies: u32) -> Result { if !variance.is_valid_float() || variance <= 0.0 { return Err(FilterError::InvalidParameter); } @@ -49,7 +49,7 @@ impl SensorSource { } /// Sets custom chi-square / sigma gating threshold. - pub fn with_gate_threshold(mut self, threshold: f64) -> Result { + pub fn with_gate_threshold(mut self, threshold: Real) -> Result { if !threshold.is_valid_float() || threshold <= 0.0 { return Err(FilterError::InvalidParameter); } @@ -79,7 +79,7 @@ pub struct SensorMeasurement { /// Sensor ID matching the configured `SensorSource` pub sensor_id: u8, /// Measured observation value - pub value: f64, + pub value: Real, } /// Result of a sensor fusion update step. @@ -116,18 +116,18 @@ pub enum FusionStatus { #[derive(Debug, Clone)] pub struct MultiSensorFusion1D { /// Estimated state mean - pub x: f64, + pub x: Real, /// Estimated state error variance - pub p: f64, + pub p: Real, /// Process noise variance (Q) - pub q: f64, + pub q: Real, /// Array of registered sensor sources pub sensors: [Option; N], } impl MultiSensorFusion1D { /// Creates a new multi-sensor fusion filter. - pub fn new(x0: f64, p0: f64, q: f64) -> Result { + pub fn new(x0: Real, p0: Real, q: Real) -> Result { if !x0.is_valid_float() || !p0.is_valid_float() || !q.is_valid_float() { return Err(FilterError::NonFiniteInput); } @@ -160,7 +160,7 @@ impl MultiSensorFusion1D { } /// Predicts state forward using linear physical transition. - pub fn predict(&mut self, control_input: f64) -> Result<(), FilterError> { + pub fn predict(&mut self, control_input: Real) -> Result<(), FilterError> { if !control_input.is_valid_float() { return Err(FilterError::NonFiniteInput); } @@ -258,17 +258,17 @@ impl MultiSensorFusion1D { } /// Returns current state estimate. - pub fn state(&self) -> f64 { + pub fn state(&self) -> Real { self.x } /// Returns current estimation error variance. - pub fn variance(&self) -> f64 { + pub fn variance(&self) -> Real { self.p } /// Returns the active health score of a sensor by ID. - pub fn sensor_health(&self, sensor_id: u8) -> Option { + pub fn sensor_health(&self, sensor_id: u8) -> Option { for s in self.sensors.iter().flatten() { if s.id == sensor_id { return Some(s.health_score); diff --git a/src/lib.rs b/src/lib.rs index 07a23b2..c8d4ffa 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -34,7 +34,7 @@ pub mod robust; pub mod stats; pub mod unscented; -pub use common::{FilterError, DEFAULT_OUTLIER_GATE_SIGMA}; +pub use common::{FilterError, Real, DEFAULT_OUTLIER_GATE_SIGMA}; pub use eskf::{ ErrorStateKalmanFilter, EskfNoiseParameters, EskfNominalState, Quaternion, Vector3, EARTH_GRAVITY_Z_UP, @@ -96,7 +96,7 @@ mod tests { // h(x) = x[0]^2 — non-linear. With tight initial covariance sigma // points stay close to mean, so linearisation error is small and a // single update step visibly moves the state toward the true value. - let true_pos: f64 = 3.0; + let true_pos: Real = 3.0; let mut ukf = UnscentedKalmanFilter2D::new( [true_pos + 0.5, 0.0], [[0.04, 0.0], [0.0, 0.04]], // tight — sigma points ±0.2 from mean @@ -133,21 +133,26 @@ mod tests { ) .unwrap(); + #[cfg(feature = "single-precision")] + let tol: Real = 1e-4; + #[cfg(not(feature = "single-precision"))] + let tol: Real = 1e-12; + ukf.predict(0.1, |x, _dt| [x[0] + 0.1, x[1]]).unwrap(); let est = ukf.update(1.0, |x| x[0], None).unwrap(); assert!( - (est[0] - 0.7019867549668872).abs() < 1e-12, + (est[0] - 0.7019867549668872).abs() < tol, "est[0] = {}", est[0] ); - assert!((est[1] - 0.0).abs() < 1e-12, "est[1] = {}", est[1]); + assert!((est[1] - 0.0).abs() < tol, "est[1] = {}", est[1]); assert!( - (ukf.p[0][0] - 0.3344370860927156).abs() < 1e-12, + (ukf.p[0][0] - 0.3344370860927156).abs() < tol, "p00 = {}", ukf.p[0][0] ); assert!( - (ukf.p[1][1] - 1.0099999999999998).abs() < 1e-12, + (ukf.p[1][1] - 1.0099999999999998).abs() < tol, "p11 = {}", ukf.p[1][1] ); @@ -158,12 +163,12 @@ mod tests { .unwrap(); let est2 = ukf.update(1.2, |x| x[0], None).unwrap(); assert!( - (est2[0] - 0.9643322092384911).abs() < 1e-12, + (est2[0] - 0.9643322092384911).abs() < tol, "est2[0] = {}", est2[0] ); assert!( - (est2[1] - 0.050000000000000044).abs() < 1e-12, + (est2[1] - 0.050000000000000044).abs() < tol, "est2[1] = {}", est2[1] ); @@ -194,12 +199,12 @@ mod tests { #[test] fn test_nan_inf_rejection() { - let result = ScalarKalmanFilter::new(f64::NAN, 1.0, 0.1, 1.0); + let result = ScalarKalmanFilter::new(Real::NAN, 1.0, 0.1, 1.0); assert_eq!(result.err(), Some(FilterError::NonFiniteInput)); let mut f = ScalarKalmanFilter::new(0.0, 1.0, 0.1, 1.0).unwrap(); assert_eq!( - f.update(f64::INFINITY, None).err(), + f.update(Real::INFINITY, None).err(), Some(FilterError::NonFiniteInput) ); } @@ -506,16 +511,22 @@ mod tests { #[test] fn test_zero_panics_property_stress() { let extreme_floats = [ - f64::NAN, - f64::INFINITY, - f64::NEG_INFINITY, + Real::NAN, + Real::INFINITY, + Real::NEG_INFINITY, 0.0, -0.0, - f64::MIN_POSITIVE, - f64::MAX, - -f64::MAX, + Real::MIN_POSITIVE, + Real::MAX, + -Real::MAX, + #[cfg(not(feature = "single-precision"))] 1e-300, + #[cfg(not(feature = "single-precision"))] 1e300, + #[cfg(feature = "single-precision")] + 1e-35, + #[cfg(feature = "single-precision")] + 1e35, ]; let mut kf = ScalarKalmanFilter::new(0.0, 1.0, 0.01, 0.5).unwrap(); diff --git a/src/linear.rs b/src/linear.rs index 89513e5..c68ef45 100644 --- a/src/linear.rs +++ b/src/linear.rs @@ -1,6 +1,6 @@ //! Classical Linear & Kinematic Kalman Filters with Joseph stabilized covariance. -use crate::common::{FilterError, FloatOps}; +use crate::common::{FilterError, FloatOps, Real}; /// 1D Scalar Kalman Filter with Joseph-stabilized variance update. /// @@ -16,20 +16,20 @@ use crate::common::{FilterError, FloatOps}; #[derive(Debug, Clone, Copy)] #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] pub struct ScalarKalmanFilter { - x: f64, - p: f64, - q: f64, - r: f64, - default_gate: Option, + x: Real, + p: Real, + q: Real, + r: Real, + default_gate: Option, } impl ScalarKalmanFilter { #[allow(clippy::needless_range_loop)] pub fn new( - initial_state: f64, - initial_variance: f64, - process_noise: f64, - measurement_noise: f64, + initial_state: Real, + initial_variance: Real, + process_noise: Real, + measurement_noise: Real, ) -> Result { if !initial_state.is_finite() || !initial_variance.is_finite() @@ -54,10 +54,10 @@ impl ScalarKalmanFilter { /// Creates a fail-closed, cyber-resilient filter with default 3-sigma innovation gating enabled. pub fn new_resilient( - initial_state: f64, - initial_variance: f64, - process_noise: f64, - measurement_noise: f64, + initial_state: Real, + initial_variance: Real, + process_noise: Real, + measurement_noise: Real, ) -> Result { let mut filter = Self::new( initial_state, @@ -72,7 +72,7 @@ impl ScalarKalmanFilter { /// Sets or overrides the persistent default gating threshold. /// /// When configured, calls to [`Self::update`] with `None` will enforce this gate (fail-closed). - pub fn with_default_gate(mut self, threshold_sigma: f64) -> Result { + pub fn with_default_gate(mut self, threshold_sigma: Real) -> Result { if !threshold_sigma.is_finite() || threshold_sigma <= 0.0 { return Err(FilterError::InvalidParameter); } @@ -86,7 +86,7 @@ impl ScalarKalmanFilter { self } - pub fn predict(&mut self, control_input: f64) -> Result<(), FilterError> { + pub fn predict(&mut self, control_input: Real) -> Result<(), FilterError> { if !control_input.is_finite() { return Err(FilterError::NonFiniteInput); } @@ -111,9 +111,9 @@ impl ScalarKalmanFilter { /// [`Self::new_resilient`]). If no default was configured, gating is bypassed. pub fn update( &mut self, - measurement: f64, - gate_threshold_sigma: Option, - ) -> Result { + measurement: Real, + gate_threshold_sigma: Option, + ) -> Result { if !measurement.is_finite() { return Err(FilterError::NonFiniteInput); } @@ -152,12 +152,12 @@ impl ScalarKalmanFilter { } #[inline(always)] - pub fn state(&self) -> f64 { + pub fn state(&self) -> Real { self.x } #[inline(always)] - pub fn variance(&self) -> f64 { + pub fn variance(&self) -> Real { self.p } } @@ -165,24 +165,24 @@ impl ScalarKalmanFilter { /// Generic N-dimensional Linear Kalman Filter using const-generics. #[derive(Debug, Clone)] pub struct LinearKalmanFilter { - pub x: [f64; S], - pub p: [[f64; S]; S], - pub f: [[f64; S]; S], - pub q: [[f64; S]; S], - pub h: [[f64; S]; M], - pub r: [[f64; M]; M], - pub default_gate: Option, + pub x: [Real; S], + pub p: [[Real; S]; S], + pub f: [[Real; S]; S], + pub q: [[Real; S]; S], + pub h: [[Real; S]; M], + pub r: [[Real; M]; M], + pub default_gate: Option, } impl LinearKalmanFilter { #[allow(clippy::needless_range_loop)] pub fn new( - initial_x: [f64; S], - initial_p: [[f64; S]; S], - f: [[f64; S]; S], - q: [[f64; S]; S], - h: [[f64; S]; M], - r: [[f64; M]; M], + initial_x: [Real; S], + initial_p: [[Real; S]; S], + f: [[Real; S]; S], + q: [[Real; S]; S], + h: [[Real; S]; M], + r: [[Real; M]; M], ) -> Result { for val in initial_x.iter() { if !val.is_finite() { @@ -224,12 +224,12 @@ impl LinearKalmanFilter { /// Creates a fail-closed, cyber-resilient filter with default 3-sigma Mahalanobis gating enabled. pub fn new_resilient( - initial_x: [f64; S], - initial_p: [[f64; S]; S], - f: [[f64; S]; S], - q: [[f64; S]; S], - h: [[f64; S]; M], - r: [[f64; M]; M], + initial_x: [Real; S], + initial_p: [[Real; S]; S], + f: [[Real; S]; S], + q: [[Real; S]; S], + h: [[Real; S]; M], + r: [[Real; M]; M], ) -> Result { let mut filter = Self::new(initial_x, initial_p, f, q, h, r)?; filter.default_gate = Some(crate::DEFAULT_OUTLIER_GATE_SIGMA); @@ -239,7 +239,7 @@ impl LinearKalmanFilter { /// Sets or overrides the persistent default gating threshold. /// /// When configured, calls to [`Self::update`] with `None` will enforce this gate (fail-closed). - pub fn with_default_gate(mut self, threshold_sigma: f64) -> Result { + pub fn with_default_gate(mut self, threshold_sigma: Real) -> Result { if !threshold_sigma.is_finite() || threshold_sigma <= 0.0 { return Err(FilterError::InvalidParameter); } @@ -254,7 +254,7 @@ impl LinearKalmanFilter { } fn validate_matrix( - matrix: &[[f64; C]; R], + matrix: &[[Real; C]; R], ) -> Result<(), FilterError> { for row in matrix.iter() { for val in row.iter() { @@ -363,9 +363,9 @@ impl LinearKalmanFilter { #[allow(clippy::needless_range_loop)] // manual index loops over const-generic buffers pub fn update( &mut self, - measurement: &[f64; M], - gate_threshold_sigma: Option, - ) -> Result<[f64; S], FilterError> { + measurement: &[Real; M], + gate_threshold_sigma: Option, + ) -> Result<[Real; S], FilterError> { // Boundary check: non-finite measurements are rejected, like the 1D path. for &val in measurement.iter() { if !val.is_finite() { @@ -563,7 +563,7 @@ impl LinearKalmanFilter { /// [`FilterError::NumericalInstability`] rather than divide by it. Threshold `1e-15` /// matches the singularity guard the 1D filter uses for its scalar innovation covariance. #[allow(clippy::needless_range_loop)] // index loops over const-generic buffers - fn invert_matrix(m: &[[f64; N]; N]) -> Result<[[f64; N]; N], FilterError> { + fn invert_matrix(m: &[[Real; N]; N]) -> Result<[[Real; N]; N], FilterError> { // Two N x N buffers instead of one 2*N augmented block: a generic const param may // not appear in const arithmetic (`2 * N`) under edition 2021. `mat` accumulates the // pivots so its left side becomes I; `inv` follows the identical row ops and so @@ -762,11 +762,11 @@ mod tests { LinearKalmanFilter::<1, 1>::new([0.0], [[1.0]], [[1.0]], [[0.01]], [[1.0]], [[0.5]]) .unwrap(); assert_eq!( - f.update(&[f64::NAN], None).err(), + f.update(&[Real::NAN], None).err(), Some(FilterError::NonFiniteInput) ); assert_eq!( - f.update(&[f64::INFINITY], None).err(), + f.update(&[Real::INFINITY], None).err(), Some(FilterError::NonFiniteInput) ); } @@ -789,8 +789,12 @@ mod tests { ) .unwrap(); let out = f.update(&[4.0, 2.0], None).unwrap(); - assert!((out[0] - 1.2).abs() < 1e-9, "out[0] = {}", out[0]); - assert!((out[1] - 1.6).abs() < 1e-9, "out[1] = {}", out[1]); + #[cfg(feature = "single-precision")] + let tol: Real = 1e-4; + #[cfg(not(feature = "single-precision"))] + let tol: Real = 1e-9; + assert!((out[0] - 1.2).abs() < tol, "out[0] = {}", out[0]); + assert!((out[1] - 1.6).abs() < tol, "out[1] = {}", out[1]); } // (9) Factored O(S^3) Joseph update test for S=4, M=2: verifies that the factored // (E*P)*E^T + (K*S)*K^T produces identical positive semi-definite, symmetric covariance. diff --git a/src/robust.rs b/src/robust.rs index ac72998..e366fca 100644 --- a/src/robust.rs +++ b/src/robust.rs @@ -15,7 +15,7 @@ //! - Xiang Shen and Li Deng, "Game theory approach to discrete H-infinity filter design", //! *IEEE Transactions on Signal Processing*, 45(4):1092-1095, 1997. -use crate::common::FilterError; +use crate::common::{FilterError, Real}; /// 1D Robust $H_\infty$ (H-infinity) Minimax Filter. /// @@ -61,26 +61,26 @@ use crate::common::FilterError; #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] pub struct HInfinityFilter1D { /// State estimate ($x$) - x: f64, + x: Real, /// Covariance / error bound parameter ($P$) - p: f64, + p: Real, /// Process disturbance bound ($Q$) - q: f64, + q: Real, /// Measurement disturbance bound ($R$) - r: f64, + r: Real, /// Performance attenuation bound ($\gamma$). Smaller values enforce tighter worst-case error bounds. - gamma: f64, + gamma: Real, } impl HInfinityFilter1D { /// Creates a new $H_\infty$ filter. /// `gamma` must be strictly greater than 0 (typically $\ge 1.0$). pub fn new( - initial_state: f64, - initial_variance: f64, - process_bound_q: f64, - measurement_bound_r: f64, - gamma: f64, + initial_state: Real, + initial_variance: Real, + process_bound_q: Real, + measurement_bound_r: Real, + gamma: Real, ) -> Result { if !initial_state.is_finite() || initial_variance <= 0.0 @@ -103,7 +103,7 @@ impl HInfinityFilter1D { /// Time update: /// $$x = x + u$$ /// $$P = P + Q$$ - pub fn predict(&mut self, control_input: f64) -> Result<(), FilterError> { + pub fn predict(&mut self, control_input: Real) -> Result<(), FilterError> { if !control_input.is_finite() { return Err(FilterError::NonFiniteInput); } @@ -116,7 +116,7 @@ impl HInfinityFilter1D { /// /// Stability condition requires positive-definiteness of Riccati operator: /// $$D = 1 - \frac{P}{\gamma^2} + \frac{P}{R} > 0$$ - pub fn update(&mut self, measurement: f64) -> Result { + pub fn update(&mut self, measurement: Real) -> Result { if !measurement.is_finite() { return Err(FilterError::NonFiniteInput); } @@ -151,17 +151,17 @@ impl HInfinityFilter1D { } #[inline(always)] - pub fn state(&self) -> f64 { + pub fn state(&self) -> Real { self.x } #[inline(always)] - pub fn bound(&self) -> f64 { + pub fn bound(&self) -> Real { self.p } #[inline(always)] - pub fn gamma(&self) -> f64 { + pub fn gamma(&self) -> Real { self.gamma } } @@ -195,21 +195,21 @@ impl HInfinityFilter1D { #[derive(Debug, Clone)] #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] pub struct AdaptiveFilter1D { - pub x: f64, - pub p: f64, - pub q: f64, - pub r: f64, + pub x: Real, + pub p: Real, + pub q: Real, + pub r: Real, /// Smoothing factor for online noise estimation (0.01 to 0.1) - pub alpha: f64, + pub alpha: Real, } impl AdaptiveFilter1D { pub fn new( - initial_x: f64, - initial_p: f64, - q: f64, - initial_r: f64, - adaptation_rate: f64, + initial_x: Real, + initial_p: Real, + q: Real, + initial_r: Real, + adaptation_rate: Real, ) -> Result { if !initial_x.is_finite() || initial_p <= 0.0 @@ -230,7 +230,7 @@ impl AdaptiveFilter1D { }) } - pub fn predict(&mut self, u: f64) -> Result<(), FilterError> { + pub fn predict(&mut self, u: Real) -> Result<(), FilterError> { if !u.is_finite() { return Err(FilterError::NonFiniteInput); } @@ -239,7 +239,7 @@ impl AdaptiveFilter1D { Ok(()) } - pub fn update(&mut self, z: f64) -> Result { + pub fn update(&mut self, z: Real) -> Result { if !z.is_finite() { return Err(FilterError::NonFiniteInput); } @@ -273,7 +273,7 @@ impl AdaptiveFilter1D { } #[inline(always)] - pub fn estimated_measurement_noise(&self) -> f64 { + pub fn estimated_measurement_noise(&self) -> Real { self.r } } diff --git a/src/stats.rs b/src/stats.rs index 9450cd0..aab7e2e 100644 --- a/src/stats.rs +++ b/src/stats.rs @@ -2,12 +2,12 @@ //! //! Designed for `#![no_std]` environments with const-generic fixed buffers and zero heap allocations. -use crate::common::{FilterError, FloatOps}; +use crate::common::{FilterError, FloatOps, Real}; /// Deterministic, zero-allocation rolling window statistics over a fixed capacity `N`. #[derive(Debug, Clone, Copy)] pub struct WindowedStats { - buffer: [f64; N], + buffer: [Real; N], count: usize, head: usize, } @@ -44,7 +44,7 @@ impl WindowedStats { } /// Pushes a new sample into the window, overwriting the oldest sample if capacity is reached. - pub fn push(&mut self, sample: f64) -> Result<(), FilterError> { + pub fn push(&mut self, sample: Real) -> Result<(), FilterError> { if N == 0 { return Err(FilterError::CapacityExceeded); } @@ -60,7 +60,7 @@ impl WindowedStats { } /// Computes the arithmetic mean of the samples in the current window. - pub fn mean(&self) -> Option { + pub fn mean(&self) -> Option { if self.count == 0 { return None; } @@ -68,11 +68,11 @@ impl WindowedStats { for i in 0..self.count { sum += self.buffer[i]; } - Some(sum / self.count as f64) + Some(sum / self.count as Real) } /// Computes sample variance of the windowed samples. - pub fn variance(&self) -> Option { + pub fn variance(&self) -> Option { if self.count < 2 { return Some(0.0); } @@ -82,17 +82,17 @@ impl WindowedStats { let diff = self.buffer[i] - m; sum_sq += diff * diff; } - Some(sum_sq / (self.count - 1) as f64) + Some(sum_sq / (self.count - 1) as Real) } /// Computes standard deviation of the windowed samples. - pub fn std_dev(&self) -> Option { + pub fn std_dev(&self) -> Option { let v = self.variance()?; Some(v.sqrt_val()) } /// Returns the minimum value in the current window. - pub fn min(&self) -> Option { + pub fn min(&self) -> Option { if self.count == 0 { return None; } @@ -106,7 +106,7 @@ impl WindowedStats { } /// Returns the maximum value in the current window. - pub fn max(&self) -> Option { + pub fn max(&self) -> Option { if self.count == 0 { return None; } @@ -120,7 +120,7 @@ impl WindowedStats { } /// Returns peak-to-peak spread (max - min). - pub fn peak_to_peak(&self) -> Option { + pub fn peak_to_peak(&self) -> Option { if self.count == 0 { return None; } diff --git a/src/unscented.rs b/src/unscented.rs index 19f2c00..3e1c47a 100644 --- a/src/unscented.rs +++ b/src/unscented.rs @@ -5,7 +5,7 @@ //! //! Formulated for `#![no_std]` execution without heap allocations using compile-time const generics. -use crate::common::{FilterError, FloatOps}; +use crate::common::{FilterError, FloatOps, Real}; /// 2-State / 1-Measurement Unscented Kalman Filter. /// @@ -30,30 +30,30 @@ use crate::common::{FilterError, FloatOps}; #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] pub struct UnscentedKalmanFilter2D { /// State mean `[x0, x1]` - pub x: [f64; 2], + pub x: [Real; 2], /// State covariance `[2x2]` - pub p: [[f64; 2]; 2], + pub p: [[Real; 2]; 2], /// Process noise covariance `[2x2]` - pub q: [[f64; 2]; 2], + pub q: [[Real; 2]; 2], /// Measurement noise variance (R) - pub r: f64, + pub r: Real, // Merwe scaled unscented transform parameters: - alpha: f64, - beta: f64, - kappa: f64, + alpha: Real, + beta: Real, + kappa: Real, } /// Sigma points container: (weights_mean, weights_cov, points) -pub type SigmaPoints2D = ([f64; 5], [f64; 5], [[f64; 2]; 5]); +pub type SigmaPoints2D = ([Real; 5], [Real; 5], [[Real; 2]; 5]); impl UnscentedKalmanFilter2D { /// Initializes a new 2D Unscented Kalman Filter. pub fn new( - initial_x: [f64; 2], - initial_p: [[f64; 2]; 2], - q: [[f64; 2]; 2], - r: f64, + initial_x: [Real; 2], + initial_p: [[Real; 2]; 2], + q: [[Real; 2]; 2], + r: Real, ) -> Result { if !initial_x[0].is_finite() || !initial_x[1].is_finite() @@ -126,9 +126,9 @@ impl UnscentedKalmanFilter2D { } /// Predicts the state forward through an arbitrary non-linear dynamic transition function `f(x, dt)`. - pub fn predict(&mut self, dt: f64, mut transition_fn: F) -> Result<(), FilterError> + pub fn predict(&mut self, dt: Real, mut transition_fn: F) -> Result<(), FilterError> where - F: FnMut([f64; 2], f64) -> [f64; 2], + F: FnMut([Real; 2], Real) -> [Real; 2], { let (wm, wc, sigma_points) = self.generate_sigma_points()?; @@ -180,12 +180,12 @@ impl UnscentedKalmanFilter2D { /// Measurement update through an arbitrary non-linear observation function `h(x) -> scalar`. pub fn update( &mut self, - measurement: f64, + measurement: Real, mut observe_fn: H, - gate_sigma: Option, - ) -> Result<[f64; 2], FilterError> + gate_sigma: Option, + ) -> Result<[Real; 2], FilterError> where - H: FnMut([f64; 2]) -> f64, + H: FnMut([Real; 2]) -> Real, { if !measurement.is_finite() { return Err(FilterError::NonFiniteInput);