diff --git a/src/linalg/mod.rs b/src/linalg/mod.rs index df9ae7de..075d91c2 100644 --- a/src/linalg/mod.rs +++ b/src/linalg/mod.rs @@ -25,6 +25,7 @@ mod solve; mod svd; mod symmetric_eigen; mod symmetric_tridiagonal; +mod udu; //// TODO: Not complete enough for publishing. //// This handles only cases where each eigenvalue has multiplicity one. @@ -45,3 +46,4 @@ pub use self::schur::*; pub use self::svd::*; pub use self::symmetric_eigen::*; pub use self::symmetric_tridiagonal::*; +pub use self::udu::*; diff --git a/src/linalg/udu.rs b/src/linalg/udu.rs new file mode 100644 index 00000000..50c2e20d --- /dev/null +++ b/src/linalg/udu.rs @@ -0,0 +1,68 @@ +#[cfg(feature = "serde-serialize")] +use serde::{Deserialize, Serialize}; + +use crate::allocator::Allocator; +use crate::base::{DefaultAllocator, MatrixN}; +use crate::dimension::DimName; +use simba::scalar::ComplexField; + +/// UDU factorization +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Debug)] +pub struct UDU +where + DefaultAllocator: Allocator, +{ + /// The upper triangular matrix resulting from the factorization + pub u: MatrixN, + /// The diagonal matrix resulting from the factorization + pub d: MatrixN, +} + +impl Copy for UDU +where + DefaultAllocator: Allocator, + MatrixN: Copy, +{ +} + +impl UDU +where + DefaultAllocator: Allocator, +{ + /// Computes the UDU^T factorization as per NASA's "Navigation Filter Best Practices", NTRS document ID 20180003657 + /// section 7.2 page 64. + /// NOTE: The provided matrix MUST be symmetric. + pub fn new(matrix: MatrixN) -> Self { + let mut d = MatrixN::::zeros(); + let mut u = MatrixN::::zeros(); + + let n = matrix.ncols(); + + d[(n, n)] = matrix[(n, n)]; + u[(n, n)] = N::one(); + + for j in (0..n - 1).rev() { + u[(j, n)] = matrix[(j, n)] / d[(n, n)]; + } + + for j in (1..n).rev() { + d[(j, j)] = matrix[(j, j)]; + for k in (j + 1..n).rev() { + d[(j, j)] = d[(j, j)] + d[(k, k)] * u[(j, k)].powi(2); + } + + u[(j, j)] = N::one(); + + for i in (0..j - 1).rev() { + u[(i, j)] = matrix[(i, j)]; + for k in j + 1..n { + u[(i, j)] = u[(i, j)] + d[(k, k)] * u[(i, k)] * u[(j, k)]; + } + u[(i, j)] /= d[(j, j)]; + } + } + + Self { u, d } + } +} diff --git a/tests/linalg/mod.rs b/tests/linalg/mod.rs index 66a0c06a..9c252bfd 100644 --- a/tests/linalg/mod.rs +++ b/tests/linalg/mod.rs @@ -14,3 +14,4 @@ mod schur; mod solve; mod svd; mod tridiagonal; +mod udu; diff --git a/tests/linalg/udu.rs b/tests/linalg/udu.rs new file mode 100644 index 00000000..443178d5 --- /dev/null +++ b/tests/linalg/udu.rs @@ -0,0 +1,68 @@ +use na::Matrix3; +use na::UDU; + +#[test] +#[rustfmt::skip] +fn udu_simple() { + let m = Matrix3::new( + 2.0, -1.0, 0.0, + -1.0, 2.0, -1.0, + 0.0, -1.0, 2.0); + + let udu = UDU::new(m); + + println!("u = {}", udu.u); + println!("d = {}", udu.d); + + // Rebuild + let p = &udu.u * &udu.d * &udu.u.transpose(); + + println!("{}", p); + + assert!(relative_eq!(m, p, epsilon = 1.0e-7)); +} + +#[cfg(feature = "arbitrary")] +mod quickcheck_tests { + #[allow(unused_imports)] + use crate::core::helper::{RandComplex, RandScalar}; + + macro_rules! gen_tests( + ($module: ident, $scalar: ty) => { + mod $module { + use std::cmp; + use na::{DMatrix, Matrix4}; + #[allow(unused_imports)] + use crate::core::helper::{RandScalar, RandComplex}; + + quickcheck! { + fn udu(m: DMatrix<$scalar>) -> bool { + let mut m = m; + if m.len() == 0 { + m = DMatrix::<$scalar>::new_random(1, 1); + } + + let m = m.map(|e| e.0); + + let udu = UDU::new(m.clone()); + let p = &udu.u * &udu.d * &udu.u.transpose(); + + relative_eq!(m, p, epsilon = 1.0e-7) + } + + fn udu_static(m: Matrix4<$scalar>) -> bool { + let m = m.map(|e| e.0); + + let udu = UDU::new(m.clone()); + let p = &udu.u * &udu.d * &udu.u.transpose(); + + relative_eq!(m, p, epsilon = 1.0e-7) + } + } + } + } + ); + + gen_tests!(complex, RandComplex); + gen_tests!(f64, RandScalar); +}