Merge pull request #766 from ChristopherRabotin/762-udu-factorization

Add UDU factorization

src/linalg/col_piv_qr.rs

@@ -66,7 +66,8 @@ where
         let min_nrows_ncols = nrows.min(ncols);
         let mut p = PermutationSequence::identity_generic(min_nrows_ncols);
 
-        let mut diag = unsafe { MatrixMN::new_uninitialized_generic(min_nrows_ncols, U1) };
+        let mut diag =
+            unsafe { crate::unimplemented_or_uninitialized_generic!(min_nrows_ncols, U1) };
 
         if min_nrows_ncols.value() == 0 {
             return ColPivQR {

src/linalg/decomposition.rs

@@ -1,8 +1,8 @@
 use crate::storage::Storage;
 use crate::{
     Allocator, Bidiagonal, Cholesky, ColPivQR, ComplexField, DefaultAllocator, Dim, DimDiff,
-    DimMin, DimMinimum, DimSub, FullPivLU, Hessenberg, Matrix, Schur, SymmetricEigen,
-    SymmetricTridiagonal, LU, QR, SVD, U1,
+    DimMin, DimMinimum, DimSub, FullPivLU, Hessenberg, Matrix, RealField, Schur, SymmetricEigen,
+    SymmetricTridiagonal, LU, QR, SVD, U1, UDU,
 };
 
 /// # Rectangular matrix decomposition
@@ -134,6 +134,7 @@ impl<N: ComplexField, R: Dim, C: Dim, S: Storage<N, R, C>> Matrix<N, R, C, S> {
 /// | -------------------------|---------------------------|--------------|
 /// | Hessenberg               | `Q * H * Qᵀ`             | `Q` is a unitary matrix and `H` an upper-Hessenberg matrix. |
 /// | Cholesky                 | `L * Lᵀ`                 | `L` is a lower-triangular matrix. |
+/// | UDU                      | `U * D * Uᵀ`             | `U` is a upper-triangular matrix, and `D` a diagonal matrix. |
 /// | Schur decomposition      | `Q * T * Qᵀ`             | `Q` is an unitary matrix and `T` a quasi-upper-triangular matrix. |
 /// | Symmetric eigendecomposition | `Q ~ Λ ~ Qᵀ`   | `Q` is an unitary matrix, and `Λ` is a real diagonal matrix. |
 /// | Symmetric tridiagonalization | `Q ~ T ~ Qᵀ`   | `Q` is an unitary matrix, and `T` is a tridiagonal matrix. |
@@ -149,6 +150,18 @@ impl<N: ComplexField, D: Dim, S: Storage<N, D, D>> Matrix<N, D, D, S> {
         Cholesky::new(self.into_owned())
     }
 
+    /// Attempts to compute the UDU decomposition of this matrix.
+    ///
+    /// The input matrix `self` is assumed to be symmetric and this decomposition will only read
+    /// the upper-triangular part of `self`.
+    pub fn udu(self) -> UDU<N, D>
+    where
+        N: RealField,
+        DefaultAllocator: Allocator<N, D> + Allocator<N, D, D>,
+    {
+        UDU::new(self.into_owned())
+    }
+
     /// Computes the Hessenberg decomposition of this matrix using householder reflections.
     pub fn hessenberg(self) -> Hessenberg<N, D>
     where

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::*;

src/linalg/udu.rs

@@ -0,0 +1,89 @@
+#[cfg(feature = "serde-serialize")]
+use serde::{Deserialize, Serialize};
+
+use crate::allocator::Allocator;
+use crate::base::{DefaultAllocator, MatrixN, VectorN, U1};
+use crate::dimension::Dim;
+use crate::storage::Storage;
+use simba::scalar::RealField;
+
+/// UDU factorization.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[cfg_attr(
+    feature = "serde-serialize",
+    serde(bound(serialize = "VectorN<N, D>: Serialize, MatrixN<N, D>: Serialize"))
+)]
+#[cfg_attr(
+    feature = "serde-serialize",
+    serde(bound(
+        deserialize = "VectorN<N, D>: Deserialize<'de>, MatrixN<N, D>: Deserialize<'de>"
+    ))
+)]
+#[derive(Clone, Debug)]
+pub struct UDU<N: RealField, D: Dim>
+where
+    DefaultAllocator: Allocator<N, D> + Allocator<N, D, D>,
+{
+    /// The upper triangular matrix resulting from the factorization
+    pub u: MatrixN<N, D>,
+    /// The diagonal matrix resulting from the factorization
+    pub d: VectorN<N, D>,
+}
+
+impl<N: RealField, D: Dim> Copy for UDU<N, D>
+where
+    DefaultAllocator: Allocator<N, D> + Allocator<N, D, D>,
+    VectorN<N, D>: Copy,
+    MatrixN<N, D>: Copy,
+{
+}
+
+impl<N: RealField, D: Dim> UDU<N, D>
+where
+    DefaultAllocator: Allocator<N, D> + Allocator<N, D, D>,
+{
+    /// Computes the UDU^T factorization.
+    ///
+    /// The input matrix `p` is assumed to be symmetric and this decomposition will only read
+    /// the upper-triangular part of `p`.
+    ///
+    /// Ref.: "Optimal control and estimation-Dover Publications", Robert F. Stengel, (1994) page 360
+    pub fn new(p: MatrixN<N, D>) -> Self {
+        let n = p.ncols();
+        let n_dim = p.data.shape().1;
+
+        let mut d = VectorN::zeros_generic(n_dim, U1);
+        let mut u = MatrixN::zeros_generic(n_dim, n_dim);
+
+        d[n - 1] = p[(n - 1, n - 1)];
+        u.column_mut(n - 1)
+            .axpy(N::one() / d[n - 1], &p.column(n - 1), N::zero());
+
+        for j in (0..n - 1).rev() {
+            let mut d_j = d[j];
+            for k in j + 1..n {
+                d_j += d[k] * u[(j, k)].powi(2);
+            }
+
+            d[j] = p[(j, j)] - d_j;
+
+            for i in (0..=j).rev() {
+                let mut u_ij = u[(i, j)];
+                for k in j + 1..n {
+                    u_ij += d[k] * u[(j, k)] * u[(i, k)];
+                }
+
+                u[(i, j)] = (p[(i, j)] - u_ij) / d[j];
+            }
+
+            u[(j, j)] = N::one();
+        }
+
+        Self { u, d }
+    }
+
+    /// Returns the diagonal elements as a matrix
+    pub fn d_matrix(&self) -> MatrixN<N, D> {
+        MatrixN::from_diagonal(&self.d)
+    }
+}

tests/linalg/mod.rs

@@ -14,3 +14,4 @@ mod schur;
 mod solve;
 mod svd;
 mod tridiagonal;
+mod udu;

tests/linalg/udu.rs

@@ -0,0 +1,72 @@
+use na::Matrix3;
+
+#[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 = m.udu();
+    
+    // Rebuild
+    let p = udu.u * udu.d_matrix() * udu.u.transpose();
+
+    assert!(relative_eq!(m, p, epsilon = 3.0e-16));
+}
+
+#[test]
+#[should_panic]
+#[rustfmt::skip]
+fn udu_non_sym_panic() {
+    let m = Matrix3::new(
+        2.0, -1.0,  0.0,
+        1.0, -2.0,  3.0,
+       -2.0,  1.0,  0.0);
+
+    let udu = m.udu();
+    // Rebuild
+    let p = udu.u * udu.d_matrix() * udu.u.transpose();
+
+    assert!(relative_eq!(m, p, epsilon = 3.0e-16));
+}
+
+#[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 na::{DMatrix, Matrix4};
+                #[allow(unused_imports)]
+                use crate::core::helper::{RandScalar, RandComplex};
+
+                quickcheck! {
+                    fn udu(n: usize) -> bool {
+                        let n = std::cmp::max(1, std::cmp::min(n, 10));
+                        let m = DMatrix::<$scalar>::new_random(n, n).map(|e| e.0).hermitian_part();
+
+                        let udu = m.clone().udu();
+                        let p = &udu.u * &udu.d_matrix() * &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).hermitian_part();
+
+                        let udu = m.udu();
+                        let p = udu.u * udu.d_matrix() * udu.u.transpose();
+
+                        relative_eq!(m, p, epsilon = 1.0e-7)
+                    }
+                }
+            }
+        }
+    );
+
+    gen_tests!(f64, RandScalar<f64>);
+}