⚙️ Core: Refactor Kalman Filter Error Handling

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 {}

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<M: KalmanModel> KalmanFilter<M> {
         }
     }
 
+    /// Helper to validate matrix dimensions.
+    fn validate_dims(
+        matrix: &DMatrix<f64>,
+        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<M: KalmanModel> KalmanFilter<M> {
     /// # Arguments
     ///
     /// * `measurement` - The measurement vector $z_k$.
-    pub fn update(&mut self, measurement: &DVector<f64>) -> Result<(), String> {
+    pub fn update(&mut self, measurement: &DVector<f64>) -> 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<M: KalmanModel> KalmanFilter<M> {
         // 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"),
+        }
+    }
 }

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;

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).

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;
+}