diff --git a/math_explorer/src/applied/algorithms/error.rs b/math_explorer/src/applied/algorithms/error.rs new file mode 100644 index 0000000..659f0db --- /dev/null +++ b/math_explorer/src/applied/algorithms/error.rs @@ -0,0 +1,34 @@ +//! Errors for the Algorithm module. + +use std::fmt; + +/// Errors that can occur during algorithm execution. +#[derive(Debug, Clone, PartialEq, Eq)] +pub enum AlgorithmError { + /// Indicates that a matrix operation failed because the matrix was singular (non-invertible). + SingularMatrix, + /// Indicates that input dimensions do not match the expected dimensions (e.g., matrix multiplication). + DimensionMismatch { expected: String, actual: String }, + /// Indicates invalid input parameters. + InvalidInput(String), +} + +impl fmt::Display for AlgorithmError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + match self { + AlgorithmError::SingularMatrix => { + write!(f, "Matrix is singular and cannot be inverted.") + } + AlgorithmError::DimensionMismatch { expected, actual } => { + write!( + f, + "Dimension mismatch: expected {}, got {}", + expected, actual + ) + } + AlgorithmError::InvalidInput(msg) => write!(f, "Invalid input: {}", msg), + } + } +} + +impl std::error::Error for AlgorithmError {} diff --git a/math_explorer/src/applied/algorithms/kalman.rs b/math_explorer/src/applied/algorithms/kalman.rs index cf28573..32df229 100644 --- a/math_explorer/src/applied/algorithms/kalman.rs +++ b/math_explorer/src/applied/algorithms/kalman.rs @@ -3,6 +3,7 @@ //! A dimension-agnostic implementation of the Discrete Kalman Filter using `nalgebra::DMatrix`. //! This module allows for state estimation of linear systems with arbitrary state and measurement dimensions. +use super::error::AlgorithmError; use nalgebra::{DMatrix, DVector}; /// Defines the physics/dynamics model for the Kalman Filter. @@ -72,18 +73,39 @@ impl KalmanFilter { } } + /// Helper to validate matrix dimensions. + fn validate_dims( + matrix: &DMatrix, + expected_rows: usize, + expected_cols: usize, + name: &str, + ) -> Result<(), AlgorithmError> { + if matrix.nrows() != expected_rows || matrix.ncols() != expected_cols { + return Err(AlgorithmError::DimensionMismatch { + expected: format!("{} {}x{}", name, expected_rows, expected_cols), + actual: format!("{}x{}", matrix.nrows(), matrix.ncols()), + }); + } + Ok(()) + } + /// Performs the **Prediction Step**. /// /// Projects the current state estimate and covariance forward in time. /// /// $$ \hat{x}_{k|k-1} = F_k \hat{x}_{k-1|k-1} $$ /// $$ P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k $$ - pub fn predict(&mut self) { + pub fn predict(&mut self) -> Result<(), AlgorithmError> { let f = self.model.transition_matrix(self.dt); let q = self.model.process_noise(self.dt); + let n = self.state.len(); + + Self::validate_dims(&f, n, n, "Transition Matrix")?; + Self::validate_dims(&q, n, n, "Process Noise")?; self.state = &f * &self.state; self.covariance = &f * &self.covariance * f.transpose() + q; + Ok(()) } /// Performs the **Update Step** with a new measurement. @@ -99,9 +121,14 @@ impl KalmanFilter { /// # Arguments /// /// * `measurement` - The measurement vector $z_k$. - pub fn update(&mut self, measurement: &DVector) -> Result<(), String> { + pub fn update(&mut self, measurement: &DVector) -> Result<(), AlgorithmError> { let h = self.model.measurement_matrix(); let r = self.model.measurement_noise(); + let n = self.state.len(); + let m = measurement.len(); + + Self::validate_dims(&h, m, n, "Measurement Matrix")?; + Self::validate_dims(&r, m, m, "Measurement Noise")?; // Innovation let y = measurement - &h * &self.state; @@ -112,9 +139,7 @@ impl KalmanFilter { // Invert S. // For 1D measurements, this is trivial. For nD, we need matrix inversion. // Kalman Filter requires S to be invertible (positive definite). - let s_inv = s - .try_inverse() - .ok_or("Failed to invert innovation covariance matrix (singular)")?; + let s_inv = s.try_inverse().ok_or(AlgorithmError::SingularMatrix)?; // Kalman Gain K = P H^T S^-1 let k = &self.covariance * h.transpose() * s_inv; @@ -169,11 +194,54 @@ mod tests { let p_init = DMatrix::identity(2, 2); let mut kf = KalmanFilter::new(x_init, p_init, model, dt); - kf.predict(); + kf.predict().unwrap(); // New Pos = 0 + 10*1 = 10 // New Vel = 10 assert!((kf.state[0] - 10.0).abs() < 1e-6); assert!((kf.state[1] - 10.0).abs() < 1e-6); } + + #[test] + fn test_singular_matrix_error() { + let dt = 1.0; + let model = MockCvModel { + process_noise: 0.0, + measurement_noise: 0.0, // Zero noise might cause singular S if HPH' is singular + }; + // H = [1, 0] + // P = [0, 0; 0, 0] + // S = HPH' + R = [0] + [0] = [0] -> Singular! + + let x_init = DVector::from_vec(vec![0.0, 0.0]); + let p_init = DMatrix::zeros(2, 2); + + let mut kf = KalmanFilter::new(x_init, p_init, model, dt); + let measurement = DVector::from_vec(vec![1.0]); + + let result = kf.update(&measurement); + assert_eq!(result, Err(AlgorithmError::SingularMatrix)); + } + + #[test] + fn test_dimension_mismatch() { + let dt = 1.0; + let model = MockCvModel { + process_noise: 0.1, + measurement_noise: 0.1, + }; + let x_init = DVector::from_vec(vec![0.0, 0.0]); + let p_init = DMatrix::identity(2, 2); + + let mut kf = KalmanFilter::new(x_init, p_init, model, dt); + + // MockCvModel expects measurement of dim 1. Let's give it 2. + let measurement = DVector::from_vec(vec![1.0, 2.0]); + + let result = kf.update(&measurement); + match result { + Err(AlgorithmError::DimensionMismatch { .. }) => (), + _ => panic!("Expected DimensionMismatch error"), + } + } } diff --git a/math_explorer/src/applied/algorithms/mod.rs b/math_explorer/src/applied/algorithms/mod.rs index 3db160d..4322ce9 100644 --- a/math_explorer/src/applied/algorithms/mod.rs +++ b/math_explorer/src/applied/algorithms/mod.rs @@ -2,5 +2,6 @@ //! //! A collection of general-purpose algorithms. +pub mod error; pub mod kalman; pub mod sorting; diff --git a/math_explorer/src/physics/medical/radar_gating/tracking.rs b/math_explorer/src/physics/medical/radar_gating/tracking.rs index 8993df1..2d75c68 100644 --- a/math_explorer/src/physics/medical/radar_gating/tracking.rs +++ b/math_explorer/src/physics/medical/radar_gating/tracking.rs @@ -75,7 +75,9 @@ impl TrackingFilter { /// Predicts the next state. pub fn predict(&mut self) { - self.inner.predict(); + self.inner + .predict() + .expect("Kalman prediction failed: Dimension mismatch in system model"); } /// Updates the state with a new measurement. @@ -84,8 +86,10 @@ impl TrackingFilter { // though here we just need a 1-element vector. from_element is cleaner. let measurement = DVector::from_element(1, measured_amplitude); // Unwrap logic: In this controlled domain (scalar update), inversion should rarely fail given R > 0. - // If it fails, we simply skip the update (robustness). - let _ = self.inner.update(&measurement); + // We expect it to succeed. + self.inner + .update(&measurement) + .expect("Kalman update failed: Singular matrix or dimension mismatch"); } /// Returns the current estimated amplitude (position). diff --git a/reproduction.rs b/reproduction.rs new file mode 100644 index 0000000..e054acf --- /dev/null +++ b/reproduction.rs @@ -0,0 +1,9 @@ +use nalgebra::{DMatrix, DVector}; + +fn main() { + let m = DMatrix::from_element(2, 2, 1.0); + let v = DVector::from_element(3, 1.0); // Mismatch! + + // This should panic + let _res = &m * &v; +}