diff --git a/.gitignore b/.gitignore index ff72ff1..cca66ab 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,7 @@ /strapdown_py/.pytest_cache /strapdown_py/dist /core/*.csv +/core/db # pixi environments .pixi @@ -22,3 +23,4 @@ __pycache__ .python-version pyproject.toml uv.lock +batch_process.nu diff --git a/Cargo.lock b/Cargo.lock index 20dafd6..dc8b873 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -680,7 +680,7 @@ dependencies = [ [[package]] name = "strapdown-rs" -version = "0.3.6" +version = "0.3.7" dependencies = [ "angle", "assert_approx_eq", diff --git a/core/Cargo.toml b/core/Cargo.toml index af00962..d96cb1f 100644 --- a/core/Cargo.toml +++ b/core/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "strapdown-rs" authors = ["James Brodovsky"] -version = "0.3.6" +version = "0.3.7" edition = "2024" description = "A toolbox for building and analyzing strapdown inertial navigation systems." license = "MIT" diff --git a/core/src/filter.rs b/core/src/filter.rs index 120cd96..c9b752a 100644 --- a/core/src/filter.rs +++ b/core/src/filter.rs @@ -405,6 +405,7 @@ impl UKF { ), coordinate_convention: true, }; + // println!("propagating: lat {} lon {}", state.latitude.to_degrees(), state.longitude.to_degrees()); forward(&mut state, imu_data, dt); // Update the sigma point with the new state sigma_point_vec[0] = state.latitude; @@ -509,7 +510,7 @@ impl UKF { Some(inv) => inv, None => panic!("Innovation matrix is singular"), }; - let k = &cross_covariance * s_inv; + let k = &cross_covariance * &s_inv; // check that the kalman gain and measurement diff are compatible to multiply if k.ncols() != measurement.get_dimension() { panic!("Kalman gain and measurement differential are not compatible"); @@ -522,16 +523,12 @@ impl UKF { self.mean_state[8] = wrap_to_2pi(self.mean_state[8]); // Switch to Joseph Form update here // P = (I - K * H) P (I - K H)^T + K R K^T - let i = DMatrix::::identity(self.state_size, self.state_size); - let p = self.covariance.clone(); - // println!("Updating covariance matrix"); - // println!("Kalman gain: {:?}", k.shape()); - // println!("Cross covariance: {:?}", cross_covariance.shape()); - let m = &i - &k * &cross_covariance.transpose(); - self.covariance = - &m * &p * &m.transpose() - + &k * measurement.get_noise() * &k.transpose(); - //self.covariance -= &k * s * &k.transpose(); + // UKF form: + // P -= K * S * K^T + K R K^T + self.covariance -= &k * s * &k.transpose(); + // Re-symmetrize to fight round-off + self.covariance = 0.5 * (&self.covariance + self.covariance.transpose()); + // self.covariance -= &k * s * &k.transpose(); } } #[derive(Clone, Debug, Default)] diff --git a/core/src/linalg.rs b/core/src/linalg.rs index 6858a96..ee23486 100644 --- a/core/src/linalg.rs +++ b/core/src/linalg.rs @@ -1,29 +1,28 @@ -/// Basic linear algebra utilities for matrix square root computation. -/// -/// This module provides linear algebra utilities for matrix square root calculations in an attempt to -/// provide similar functionality to the `scipy.linalg.sqrtm` function in Python. That said, due to -/// the applied nature of this crate, the problem is simplified to only handle square, positive -/// definite, and positive semi-definite matrices. The inclusion of this module is to provide an implementation -/// of matrix square root calculations for the Unscented Kalman Filter (UKF) algorithm, which requires -/// the computation of the square root of a covariance matrix. Covariance matrices are symmetric and positive -/// semi-definite, making them suitable for this approach. -/// -/// Two methods are implemented: -/// 1. **Cholesky Decomposition**: Computes the square root of a symmetric positive definite matrix. -/// 2. **Eigenvalue Decomposition**: Computes the square root of a symmetric positive semi-definite matrix. -/// -/// These calculations are intended to be used through the `matrix_square_root` function, which will -/// attempt to compute the square root using Cholesky decomposition first, then eigenvalue decomposition. If both -/// methods fail, then the method will panic. +//! Linear algebra helpers for robust covariance square roots. +//! +//! Public API: +//! pub fn matrix_square_root(matrix: &DMatrix) -> DMatrix +//! +//! Internal pipeline (each step isolated for testing): +//! - symmetrize() +//! - chol_sqrt() +//! - chol_sqrt_with_jitter() +//! - evd_symmetric_sqrt_with_floor() +//! +//! Strategy: +//! 1) Symmetrize P ← 0.5 (P + Pᵀ) +//! 2) Cholesky +//! 3) Jittered Cholesky (geometric ramp) +//! 4) Symmetric EVD with eigenvalue floor → S = U * sqrt(Λ⁺) * Uᵀ + use nalgebra::DMatrix; use nalgebra::linalg::{Cholesky, SymmetricEigen}; -/// Calculates a square root of a symmetric matrix. +/// Compute a robust symmetric square root `S` such that approximately `matrix ≈ S * Sᵀ`. /// /// Attempts Cholesky decomposition first (yielding L such that matrix = L * L^T). /// If Cholesky fails (e.g., matrix is not positive definite), it attempts to compute /// the square root using eigenvalue decomposition (S = V * sqrt(D) * V^T). -/// For eigenvalue decomposition, eigenvalues are clamped to be non-negative. /// /// # Arguments /// * `matrix` - The DMatrix to find the square root of. It's assumed to be symmetric and square. @@ -35,252 +34,485 @@ use nalgebra::linalg::{Cholesky, SymmetricEigen}; /// * `None` if the matrix is not square or another fundamental issue prevents computation (though /// this implementation tries to be robust for positive semi-definite cases). pub fn matrix_square_root(matrix: &DMatrix) -> DMatrix { - if !matrix.is_square() { - panic!("Error: Matrix must be square to compute square root."); - } - // Attempt Cholesky decomposition (yields L where matrix = L * L^T) - // Cholesky requires the matrix to be symmetric positive definite. - match cholesky_pass(matrix) { - Some(chol_l) => { - return chol_l; - } - None => { - //println!("Cholesky decomposition failed. Attempting eigenvalue decomposition."); - } + assert!(matrix.is_square(), "matrix_square_root: matrix must be square"); + + // Tunable guards (conservative defaults for double precision INS scales) + const INITIAL_JITTER: f64 = 1e-12; + const MAX_JITTER: f64 = 1e-6; + const MAX_TRIES: usize = 6; + const EIGEN_FLOOR: f64 = 1e-12; + + // 1) Symmetrize to kill round-off asymmetry + let p = symmetrize(matrix); + + // 2) Cholesky (fast path) + if let Some(s) = chol_sqrt(&p) { + return s; } - // If Cholesky failed, we try eigenvalue decomposition. - match eigenvalue_pass(matrix) { - Some(eigen_sqrt) => eigen_sqrt, - None => { - panic!( - "Cholesky and Eigenvalue decomposition failed. No valid square root found for the covariance matrix: \n {:?}", - matrix - ); - } + + // 3) Jittered Cholesky + if let Some(s) = chol_sqrt_with_jitter(&p, INITIAL_JITTER, MAX_JITTER, MAX_TRIES) { + return s; } + + // 4) EVD fallback with eigenvalue floor — symmetric square root + return evd_symmetric_sqrt_with_floor(&p, EIGEN_FLOOR); } -/// Attempts to compute the matrix square root using Cholesky decomposition. -/// -/// This method is only applicable to symmetric positive definite matrices. -/// If successful, it returns the lower triangular matrix `L` such that `matrix = L * L.transpose()`. +/// Symmetrize a matrix: P ← 0.5 (P + Pᵀ) /// -/// When the computation _fails_ (e.g., the matrix is not positive definite or not square), -/// a None value is returned instead of panicking, permitting the public API to proceed to the -/// next method. +/// Simple matrix symmetrization function that reduces round-off errors associated +/// with floating point arithmetic. /// /// # Arguments -/// * `matrix` - The DMatrix to find the square root of. Assumed to be symmetric and square. -/// +/// * `m` - the matrix to symmetrize +/// /// # Returns -/// * `Some(DMatrix)` containing the lower triangular Cholesky factor `L`. -/// * `None` if the matrix is not positive definite or not square. -fn cholesky_pass(matrix: &DMatrix) -> Option> { - if !matrix.is_square() { - eprintln!("Error: Matrix must be square for Cholesky decomposition."); - return None; - } - matrix - .clone() - .cholesky() - .map(|chol: Cholesky| chol.l()) +/// A symmetrized version of the input matrix. +fn symmetrize(m: &DMatrix) -> DMatrix { + 0.5 * (m + m.transpose()) } -/// Computes a symmetric matrix square root using eigenvalue decomposition. -/// -/// This method is suitable for symmetric positive semi-definite matrices. -/// It returns a symmetric matrix `S` such that `matrix = S * S`. -/// Eigenvalues are clamped to be non-negative to handle positive semi-definite cases -/// and minor numerical inaccuracies. -/// -/// When the computation _fails_ (e.g., the matrix is not positive definite or not square), -/// a None value is returned instead of panicking, permitting the public API to proceed to the -/// next method. -/// +/// Plain Cholesky square root +/// +/// Cholesky factorization that returns L such that P ≈ L Lᵀ, or None if it fails. +/// This is a quick way to initially attempt to calculate a matrix square root. +/// /// # Arguments -/// * `matrix` - The DMatrix to find the square root of. Assumed to be symmetric and square. -/// +/// * `p` - the matrix to factor +/// /// # Returns -/// * `Some(DMatrix)` containing the symmetric matrix square root `S`. -/// * `None` if the matrix is not square (though this should be checked by the caller for symmetry assumptions). -fn eigenvalue_pass(matrix: &DMatrix) -> Option> { - if !matrix.is_square() { - eprintln!("Error: Matrix must be square for eigenvalue decomposition based square root."); - return None; +/// A lower triangular matrix L such that P ≈ L Lᵀ, or None if it fails. +fn chol_sqrt(p: &DMatrix) -> Option> { + Cholesky::new(p.clone()).map(|ch| ch.l().into_owned()) +} +/// Cholesky with diagonal jitter (geometric ramp). Returns None if all tries fail. +/// +/// Perform Cholesky decomposition with a jittered diagonal on a geometric ramp up. +/// Returns None if all tries fail. +fn chol_sqrt_with_jitter( + p: &DMatrix, + initial_jitter: f64, + max_jitter: f64, + max_tries: usize, +) -> Option> { + let n = p.nrows(); + let mut jitter = initial_jitter; + for _ in 0..max_tries { + let mut pj = p.clone(); + for i in 0..n { + pj[(i, i)] += jitter; + } + if let Some(ch) = Cholesky::new(pj) { + return Some(ch.l().into_owned()); + } + jitter *= 10.0; + if jitter > max_jitter { + break; + } } - // For eigenvalue decomposition of a symmetric matrix, - // we use `symmetric_eigen`. This returns real eigenvalues and orthogonal eigenvectors. - let eigen_decomposition: SymmetricEigen = matrix.clone().symmetric_eigen(); - let eigenvalues = eigen_decomposition.eigenvalues; - let eigenvectors = eigen_decomposition.eigenvectors; + None +} - // Check for significantly negative eigenvalues, indicating non-positive semi-definiteness. - // While we clamp them, a warning is useful for diagnosis. - if eigenvalues.iter().any(|&val| val < -1e-9) { - println!( - "Warning: Negative eigenvalues encountered during eigenvalue decomposition. The input matrix was not positive semi-definite." - ); - println!("{:?}", matrix.data); - return None; - } +/// Symmetric EVD square root with eigenvalue flooring: +/// S = U * sqrt(max(λ, floor)) * Uᵀ +fn evd_symmetric_sqrt_with_floor(p: &DMatrix, floor: f64) -> DMatrix { + let se = SymmetricEigen::new(p.clone()); + let mut lambdas = se.eigenvalues; + let u = se.eigenvectors; - // Create diagonal matrix of sqrt(eigenvalues), clamping eigenvalues to be non-negative. - // `DMatrix::from_diagonal` takes a DVector. - let sqrt_eigenvalues_diag_vec = eigenvalues.map(|val| val.max(1e-9).sqrt()); - let sqrt_eigenvalues_diag = DMatrix::from_diagonal(&sqrt_eigenvalues_diag_vec); + for i in 0..lambdas.len() { + if lambdas[i] < floor { + lambdas[i] = floor; + } + } - // Reconstruct the square root: S = V * sqrt(D) * V^T - // This S will be symmetric, and S * S = matrix (or S * S^T = matrix). - let sqrt_m = eigenvectors.clone() * sqrt_eigenvalues_diag * eigenvectors.transpose(); + let sqrt_vals = lambdas.map(|l| l.sqrt()); + let sigma_half = DMatrix::::from_diagonal(&sqrt_vals); + &u * sigma_half * u.transpose() - Some(sqrt_m) } +/* =============================== Tests ==================================== */ + #[cfg(test)] mod tests { use super::*; - use nalgebra::DMatrix; - use std::sync::LazyLock; - - static BASIC_SQRT: LazyLock> = LazyLock::new(|| { - DMatrix::from_row_slice(3, 3, &[4.0, 0.0, 0.0, 0.0, 9.0, 0.0, 0.0, 0.0, 16.0]) - }); - static POSITIVE_DEFINITE: LazyLock> = LazyLock::new(|| { - DMatrix::from_row_slice(3, 3, &[4.0, 2.0, 0.0, 2.0, 9.0, 3.0, 0.0, 3.0, 16.0]) - }); - static POSITIVE_SEMI_DEFINITE: LazyLock> = LazyLock::new(|| { - DMatrix::from_row_slice(3, 3, &[1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0]) - }); - static NEGATIVE_DEFINITE: LazyLock> = LazyLock::new(|| { - DMatrix::from_row_slice(3, 3, &[-4.0, 0.0, 0.0, 0.0, -9.0, 0.0, 0.0, 0.0, -16.0]) - }); - static NEGATIVE_SEMI_DEFINITE: LazyLock> = LazyLock::new(|| { - DMatrix::from_row_slice(3, 3, &[-1.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0]) - }); - static NON_SQUARE: LazyLock> = - LazyLock::new(|| DMatrix::from_row_slice(2, 3, &[1.0, 2.0, 3.0, 4.0, 5.0, 6.0])); - - /// Helper function to verify if a matrix is a valid square root of another matrix. - /// Returns true if sqrt_matrix * sqrt_matrix.T ≈ original_matrix within tolerance. - fn is_valid_square_root( - sqrt_matrix: &DMatrix, - original_matrix: &DMatrix, - tolerance: f64, - ) -> bool { - let reconstructed = sqrt_matrix * sqrt_matrix.transpose(); - if reconstructed.nrows() != original_matrix.nrows() - || reconstructed.ncols() != original_matrix.ncols() - { - return false; - } - - for i in 0..original_matrix.nrows() { - for j in 0..original_matrix.ncols() { - if (reconstructed[(i, j)] - original_matrix[(i, j)]).abs() > tolerance { - return false; - } + fn approx_eq(a: &DMatrix, b: &DMatrix, tol: f64) -> bool { + if a.shape() != b.shape() { return false; } + let mut max_abs = 0.0f64; + for i in 0..a.nrows() { + for j in 0..a.ncols() { + max_abs = max_abs.max((a[(i,j)] - b[(i,j)]).abs()); } } - true - } - // Test matrix square root calculation - #[test] - fn cholesky_square_root() { - let sqrt_matrix = matrix_square_root(&BASIC_SQRT); - assert!(is_valid_square_root(&sqrt_matrix, &BASIC_SQRT, 1e-9)); - } - #[test] - fn cholesky_positive_definite() { - let sqrt_matrix = matrix_square_root(&POSITIVE_DEFINITE); - assert!(is_valid_square_root(&sqrt_matrix, &POSITIVE_DEFINITE, 1e-9)); - } - #[test] - #[should_panic] - fn cholesky_negative_definite() { - // This should panic because the matrix is negative definite. - let _sqrt_matrix = matrix_square_root(&NEGATIVE_DEFINITE); - } - #[test] - #[should_panic] - fn cholesky_negative_semi_definite() { - // This should panic because the matrix is negative semi-definite. - let _sqrt_matrix = matrix_square_root(&NEGATIVE_SEMI_DEFINITE); - } - #[test] - #[should_panic] - fn cholesky_non_square() { - // This should panic because the matrix is not square. - let _sqrt_matrix = matrix_square_root(&NON_SQUARE); - } - #[test] - fn eigenvalue_square_root() { - let sqrt_matrix = matrix_square_root(&POSITIVE_SEMI_DEFINITE); - assert!(is_valid_square_root( - &sqrt_matrix, - &POSITIVE_SEMI_DEFINITE, - 1e-9 - )); - } - #[test] - fn eigenvalue_positive_definite() { - let sqrt_matrix = matrix_square_root(&POSITIVE_DEFINITE); - assert!(is_valid_square_root(&sqrt_matrix, &POSITIVE_DEFINITE, 1e-9)); + max_abs <= tol } + #[test] - fn eigenvalue_positive_semi_definite() { - let sqrt_matrix = matrix_square_root(&POSITIVE_SEMI_DEFINITE); - assert!(is_valid_square_root( - &sqrt_matrix, - &POSITIVE_SEMI_DEFINITE, - 1e-9 - )); - } - #[test] - #[should_panic] - fn eigenvalue_negative_definite() { - // This should panic because the matrix is negative definite. - let _sqrt_matrix = matrix_square_root(&NEGATIVE_DEFINITE); + fn t_symmetrize() { + let m = DMatrix::from_row_slice(2, 2, &[ + 1.0, 2.0, + 0.0, 3.0, + ]); + let s = symmetrize(&m); + let s_expected = DMatrix::from_row_slice(2, 2, &[ + 1.0, 1.0, + 1.0, 3.0, + ]); + assert!(approx_eq(&s, &s_expected, 1e-15)); } + #[test] - #[should_panic] - fn eigenvalue_negative_semi_definite() { - // This should panic because the matrix is negative semi-definite. - let _sqrt_matrix = matrix_square_root(&NEGATIVE_SEMI_DEFINITE); + fn t_chol_sqrt_spd() { + // P = A Aᵀ is SPD + let a = DMatrix::from_row_slice(3, 3, &[ + 1.0, 2.0, 0.5, + 0.0, 1.0, -1.0, + 0.0, 0.0, 0.2, + ]); + let p = &a * a.transpose(); + let s = chol_sqrt(&p).expect("Cholesky should succeed for SPD"); + let back = &s * s.transpose(); + assert!(approx_eq(&back, &p, 1e-12)); } + #[test] - #[should_panic] - fn eigenvalue_non_square() { - // This should panic because the matrix is not square. - let _sqrt_matrix = matrix_square_root(&NON_SQUARE); + fn t_chol_sqrt_with_jitter() { + // Nudge diagonal a hair negative to break plain Cholesky + let a = DMatrix::from_row_slice(3, 3, &[ + 1.0, 0.2, 0.0, + 0.0, 1.0, 0.2, + 0.0, 0.0, 1.0, + ]); + let mut p = &a * a.transpose(); + p[(2,2)] -= 1e-10; + + //assert!(chol_sqrt(&p).is_none(), "plain Cholesky should fail here"); + let s = chol_sqrt_with_jitter(&p, 1e-12, 1e-6, 6).expect("jittered Cholesky should succeed"); + let back = &s * s.transpose(); + let p_sym = symmetrize(&p); + assert!(approx_eq(&back, &p_sym, 1e-8)); } + #[test] - fn public_api_square_root() { - let sqrt_matrix = matrix_square_root(&POSITIVE_DEFINITE); - assert!(is_valid_square_root(&sqrt_matrix, &POSITIVE_DEFINITE, 1e-9)); - let sqrt_matrix = matrix_square_root(&POSITIVE_SEMI_DEFINITE); - assert!(is_valid_square_root( - &sqrt_matrix, - &POSITIVE_SEMI_DEFINITE, - 1e-9 - )); - let sqrt_matrix = matrix_square_root(&BASIC_SQRT); - assert!(is_valid_square_root(&sqrt_matrix, &BASIC_SQRT, 1e-9)); + fn t_evd_floor() { + // Make P symmetric but with a negative eigenvalue, EVD should floor it. + let p = DMatrix::from_row_slice(2, 2, &[ + 0.0, 1.0, + 1.0, 0.0, + ]); // eigenvalues {+1, -1} + let s = evd_symmetric_sqrt_with_floor(&p, 1e-12); + let back = &s * s.transpose(); + // back should be PSD and close to symmetrized p with floor effects + let p_sym = symmetrize(&p); + assert_eq!(back.nrows(), p_sym.nrows()); + assert_eq!(back.ncols(), p_sym.ncols()); + // sanity: back is symmetric + assert!(approx_eq(&back, &back.transpose(), 1e-14)); } + #[test] - #[should_panic] - fn public_api_negative_definite() { - // This should panic because the matrix is negative definite. - let _sqrt_matrix = matrix_square_root(&NEGATIVE_DEFINITE); + fn t_public_identity() { + let i = DMatrix::::identity(4,4); + let s = matrix_square_root(&i); + assert!(approx_eq(&s, &i, 1e-14)); + let back = &s * s.transpose(); + assert!(approx_eq(&back, &i, 1e-12)); } + #[test] - #[should_panic] - fn public_api_negative_semi_definite() { - // This should panic because the matrix is negative semi-definite. - let _sqrt_matrix = matrix_square_root(&NEGATIVE_SEMI_DEFINITE); + fn t_public_nearly_spd() { + let a = DMatrix::from_row_slice(3, 3, &[ + 1.0, 0.1, 0.0, + 0.0, 1.0, 0.2, + 0.0, 0.0, 1.0, + ]); + let mut p = &a * a.transpose(); + p[(2,2)] -= 1e-10; + p[(0,2)] += 1e-12; // asymmetry + + let s = matrix_square_root(&p); + let back = &s * s.transpose(); + let p_sym = symmetrize(&p); + assert!(approx_eq(&back, &p_sym, 1e-8)); } + #[test] #[should_panic] - fn public_api_non_square() { - // This should panic because the matrix is not square. - let _sqrt_matrix = matrix_square_root(&NON_SQUARE); + fn t_public_non_square_panics() { + let m = DMatrix::::zeros(3, 2); + let _ = matrix_square_root(&m); } } + + + +// ============ OLD ==================================== + +// Calculates a square root of a symmetric matrix. +// +// Attempts Cholesky decomposition first (yielding L such that matrix = L * L^T). +// If Cholesky fails (e.g., matrix is not positive definite), it attempts to compute +// the square root using eigenvalue decomposition (S = V * sqrt(D) * V^T). +// For eigenvalue decomposition, eigenvalues are clamped to be non-negative. +// +// # Arguments +// * `matrix` - The DMatrix to find the square root of. It's assumed to be symmetric and square. +// +// # Returns +// * `Some(DMatrix)` containing a matrix square root. +// The result from Cholesky is lower triangular. The result from eigenvalue decomposition is symmetric. +// In both cases, if the result is `M`, then `matrix` approx `M * M.transpose()`. +// * `None` if the matrix is not square or another fundamental issue prevents computation (though +// this implementation tries to be robust for positive semi-definite cases). +//pub fn matrix_square_root(matrix: &DMatrix) -> DMatrix { +// if !matrix.is_square() { +// panic!("Error: Matrix must be square to compute square root."); +// } +// // Attempt Cholesky decomposition (yields L where matrix = L * L^T) +// // Cholesky requires the matrix to be symmetric positive definite. +// match cholesky_pass(matrix) { +// Some(chol_l) => { +// return chol_l; +// } +// None => { +// //println!("Cholesky decomposition failed. Attempting eigenvalue decomposition."); +// } +// } +// // If Cholesky failed, we try eigenvalue decomposition. +// match eigenvalue_pass(matrix) { +// Some(eigen_sqrt) => eigen_sqrt, +// None => { +// panic!( +// "Cholesky and Eigenvalue decomposition failed. No valid square root found for the covariance matrix: \n {:?}", +// matrix +// ); +// } +// } +//} +// Attempts to compute the matrix square root using Cholesky decomposition. +// +// This method is only applicable to symmetric positive definite matrices. +// If successful, it returns the lower triangular matrix `L` such that `matrix = L * L.transpose()`. +// +// When the computation _fails_ (e.g., the matrix is not positive definite or not square), +// a None value is returned instead of panicking, permitting the public API to proceed to the +// next method. +// +// # Arguments +// * `matrix` - The DMatrix to find the square root of. Assumed to be symmetric and square. +// +// # Returns +// * `Some(DMatrix)` containing the lower triangular Cholesky factor `L`. +// * `None` if the matrix is not positive definite or not square. +// fn cholesky_pass(matrix: &DMatrix) -> Option> { +// if !matrix.is_square() { +// eprintln!("Error: Matrix must be square for Cholesky decomposition."); +// return None; +// } +// matrix +// .clone() +// .cholesky() +// .map(|chol: Cholesky| chol.l()) +// } +// Computes a symmetric matrix square root using eigenvalue decomposition. +// +// This method is suitable for symmetric positive semi-definite matrices. +// It returns a symmetric matrix `S` such that `matrix = S * S`. +// Eigenvalues are clamped to be non-negative to handle positive semi-definite cases +// and minor numerical inaccuracies. +// +// When the computation _fails_ (e.g., the matrix is not positive definite or not square), +// a None value is returned instead of panicking, permitting the public API to proceed to the +// next method. +// +// # Arguments +// * `matrix` - The DMatrix to find the square root of. Assumed to be symmetric and square. +// +// # Returns +// * `Some(DMatrix)` containing the symmetric matrix square root `S`. +// * `None` if the matrix is not square (though this should be checked by the caller for symmetry assumptions). +// fn eigenvalue_pass(matrix: &DMatrix) -> Option> { +// if !matrix.is_square() { +// eprintln!("Error: Matrix must be square for eigenvalue decomposition based square root."); +// return None; +// } +// // For eigenvalue decomposition of a symmetric matrix, +// // we use `symmetric_eigen`. This returns real eigenvalues and orthogonal eigenvectors. +// let eigen_decomposition: SymmetricEigen = matrix.clone().symmetric_eigen(); +// let eigenvalues = eigen_decomposition.eigenvalues; +// let eigenvectors = eigen_decomposition.eigenvectors; +// +// // Check for significantly negative eigenvalues, indicating non-positive semi-definiteness. +// // While we clamp them, a warning is useful for diagnosis. +// if eigenvalues.iter().any(|&val| val < -1e-9) { +// println!( +// "Warning: Negative eigenvalues encountered during eigenvalue decomposition. The input matrix was not positive semi-definite." +// ); +// // println!("{:?}", matrix.data); +// // // return None; +// } +// +// // Create diagonal matrix of sqrt(eigenvalues), clamping eigenvalues to be non-negative. +// // `DMatrix::from_diagonal` takes a DVector. +// let sqrt_eigenvalues_diag_vec = eigenvalues.map(|val| val.max(1e-9).sqrt()); +// let sqrt_eigenvalues_diag = DMatrix::from_diagonal(&sqrt_eigenvalues_diag_vec); +// +// // Reconstruct the square root: S = V * sqrt(D) * V^T +// // This S will be symmetric, and S * S = matrix (or S * S^T = matrix). +// let sqrt_m = eigenvectors.clone() * sqrt_eigenvalues_diag * eigenvectors.transpose(); +// +// Some(sqrt_m) +// } +// +// #[cfg(test)] +// mod tests { +// use super::*; +// use nalgebra::DMatrix; +// use std::sync::LazyLock; +// +// static BASIC_SQRT: LazyLock> = LazyLock::new(|| { +// DMatrix::from_row_slice(3, 3, &[4.0, 0.0, 0.0, 0.0, 9.0, 0.0, 0.0, 0.0, 16.0]) +// }); +// static POSITIVE_DEFINITE: LazyLock> = LazyLock::new(|| { +// DMatrix::from_row_slice(3, 3, &[4.0, 2.0, 0.0, 2.0, 9.0, 3.0, 0.0, 3.0, 16.0]) +// }); +// static POSITIVE_SEMI_DEFINITE: LazyLock> = LazyLock::new(|| { +// DMatrix::from_row_slice(3, 3, &[1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0]) +// }); +// static NEGATIVE_DEFINITE: LazyLock> = LazyLock::new(|| { +// DMatrix::from_row_slice(3, 3, &[-4.0, 0.0, 0.0, 0.0, -9.0, 0.0, 0.0, 0.0, -16.0]) +// }); +// static NEGATIVE_SEMI_DEFINITE: LazyLock> = LazyLock::new(|| { +// DMatrix::from_row_slice(3, 3, &[-1.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0, 0.0, -1.0]) +// }); +// static NON_SQUARE: LazyLock> = +// LazyLock::new(|| DMatrix::from_row_slice(2, 3, &[1.0, 2.0, 3.0, 4.0, 5.0, 6.0])); +// +// /// Helper function to verify if a matrix is a valid square root of another matrix. +// /// Returns true if sqrt_matrix * sqrt_matrix.T ≈ original_matrix within tolerance. +// fn is_valid_square_root( +// sqrt_matrix: &DMatrix, +// original_matrix: &DMatrix, +// tolerance: f64, +// ) -> bool { +// let reconstructed = sqrt_matrix * sqrt_matrix.transpose(); +// +// if reconstructed.nrows() != original_matrix.nrows() +// || reconstructed.ncols() != original_matrix.ncols() +// { +// return false; +// } +// +// for i in 0..original_matrix.nrows() { +// for j in 0..original_matrix.ncols() { +// if (reconstructed[(i, j)] - original_matrix[(i, j)]).abs() > tolerance { +// return false; +// } +// } +// } +// true +// } +// // Test matrix square root calculation +// #[test] +// fn cholesky_square_root() { +// let sqrt_matrix = matrix_square_root(&BASIC_SQRT); +// assert!(is_valid_square_root(&sqrt_matrix, &BASIC_SQRT, 1e-9)); +// } +// #[test] +// fn cholesky_positive_definite() { +// let sqrt_matrix = matrix_square_root(&POSITIVE_DEFINITE); +// assert!(is_valid_square_root(&sqrt_matrix, &POSITIVE_DEFINITE, 1e-9)); +// } +// #[test] +// #[should_panic] +// fn cholesky_negative_definite() { +// // This should panic because the matrix is negative definite. +// let _sqrt_matrix = matrix_square_root(&NEGATIVE_DEFINITE); +// } +// #[test] +// #[should_panic] +// fn cholesky_negative_semi_definite() { +// // This should panic because the matrix is negative semi-definite. +// let _sqrt_matrix = matrix_square_root(&NEGATIVE_SEMI_DEFINITE); +// } +// #[test] +// #[should_panic] +// fn cholesky_non_square() { +// // This should panic because the matrix is not square. +// let _sqrt_matrix = matrix_square_root(&NON_SQUARE); +// } +// #[test] +// fn eigenvalue_square_root() { +// let sqrt_matrix = matrix_square_root(&POSITIVE_SEMI_DEFINITE); +// assert!(is_valid_square_root( +// &sqrt_matrix, +// &POSITIVE_SEMI_DEFINITE, +// 1e-9 +// )); +// } +// #[test] +// fn eigenvalue_positive_definite() { +// let sqrt_matrix = matrix_square_root(&POSITIVE_DEFINITE); +// assert!(is_valid_square_root(&sqrt_matrix, &POSITIVE_DEFINITE, 1e-9)); +// } +// #[test] +// fn eigenvalue_positive_semi_definite() { +// let sqrt_matrix = matrix_square_root(&POSITIVE_SEMI_DEFINITE); +// assert!(is_valid_square_root( +// &sqrt_matrix, +// &POSITIVE_SEMI_DEFINITE, +// 1e-9 +// )); +// } +// #[test] +// #[should_panic] +// fn eigenvalue_negative_definite() { +// // This should panic because the matrix is negative definite. +// let _sqrt_matrix = matrix_square_root(&NEGATIVE_DEFINITE); +// } +// #[test] +// #[should_panic] +// fn eigenvalue_negative_semi_definite() { +// // This should panic because the matrix is negative semi-definite. +// let _sqrt_matrix = matrix_square_root(&NEGATIVE_SEMI_DEFINITE); +// } +// #[test] +// #[should_panic] +// fn eigenvalue_non_square() { +// // This should panic because the matrix is not square. +// let _sqrt_matrix = matrix_square_root(&NON_SQUARE); +// } +// #[test] +// fn public_api_square_root() { +// let sqrt_matrix = matrix_square_root(&POSITIVE_DEFINITE); +// assert!(is_valid_square_root(&sqrt_matrix, &POSITIVE_DEFINITE, 1e-9)); +// let sqrt_matrix = matrix_square_root(&POSITIVE_SEMI_DEFINITE); +// assert!(is_valid_square_root( +// &sqrt_matrix, +// &POSITIVE_SEMI_DEFINITE, +// 1e-9 +// )); +// let sqrt_matrix = matrix_square_root(&BASIC_SQRT); +// assert!(is_valid_square_root(&sqrt_matrix, &BASIC_SQRT, 1e-9)); +// } +// #[test] +// #[should_panic] +// fn public_api_negative_definite() { +// // This should panic because the matrix is negative definite. +// let _sqrt_matrix = matrix_square_root(&NEGATIVE_DEFINITE); +// } +// #[test] +// #[should_panic] +// fn public_api_negative_semi_definite() { +// // This should panic because the matrix is negative semi-definite. +// let _sqrt_matrix = matrix_square_root(&NEGATIVE_SEMI_DEFINITE); +// } +// #[test] +// #[should_panic] +// fn public_api_non_square() { +// // This should panic because the matrix is not square. +// let _sqrt_matrix = matrix_square_root(&NON_SQUARE); +// } +// } +// \ No newline at end of file diff --git a/core/src/main.rs b/core/src/main.rs index 9058042..0e5f37a 100644 --- a/core/src/main.rs +++ b/core/src/main.rs @@ -73,6 +73,22 @@ fn main() -> Result<(), Box> { _ => {} } // Read the input CSV file + // Validate that the input file exists and is readable + if !args.input.exists() { + return Err(format!("Input file '{}' does not exist.", args.input.display()).into()); + } + if !args.input.is_file() { + return Err(format!("Input path '{}' is not a file.", args.input.display()).into()); + } + // Validate that the output file is writable + if let Some(parent) = args.output.parent() { + if !parent.exists() && parent.is_dir() { + return Err(format!("Output directory '{}' does not exist.", parent.display()).into()); + } + // if !parent.is_dir() { + // return Err(format!("Output path '{}' is not a directory.", parent.display()).into()); + // } + } let mut rdr = ReaderBuilder::new().from_path(&args.input)?; let mut records: Vec = rdr.deserialize().collect::>()?; println!( diff --git a/core/src/sim.rs b/core/src/sim.rs index 2d1f540..2ecbf9c 100644 --- a/core/src/sim.rs +++ b/core/src/sim.rs @@ -844,6 +844,7 @@ pub fn closed_loop(records: &[TestDataRecord], gps_interval: Option) -> Vec let gps_interval = gps_interval.unwrap_or(0.0); // Default to every record if not specified let reference_altitude = records[0].altitude; // Use the first record's pressure as reference let start_time = records[0].time; + // Add total elapsed time let records_with_elapsed: Vec<(f64, &TestDataRecord)> = records .iter() .map(|r| ((r.time - start_time).num_milliseconds() as f64 / 1000.0, r)) @@ -910,6 +911,7 @@ pub fn closed_loop(records: &[TestDataRecord], gps_interval: Option) -> Vec && !record.speed.is_nan() && (*elapsed - last_gps_update_time) >= gps_interval { + //println!("GPS measurement available at {:.2?} seconds", *elapsed); let measurement = GPSPositionAndVelocityMeasurement { latitude: record.latitude, longitude: record.longitude,