From 8dda6714b5b8c515bc25004115125d622abe6f32 Mon Sep 17 00:00:00 2001
From: Christopher Rabotin <christopher.rabotin@gmail.com>
Date: Thu, 24 Sep 2020 23:21:13 -0600
Subject: [PATCH] Untested UDU implementation

Pushing to trigger build

Signed-off-by: Christopher Rabotin <christopher.rabotin@gmail.com>
---
 src/linalg/mod.rs   |  2 ++
 src/linalg/udu.rs   | 68 +++++++++++++++++++++++++++++++++++++++++++++
 tests/linalg/mod.rs |  1 +
 tests/linalg/udu.rs | 68 +++++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 139 insertions(+)
 create mode 100644 src/linalg/udu.rs
 create mode 100644 tests/linalg/udu.rs

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<N: ComplexField, D: DimName>
+where
+    DefaultAllocator: 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: MatrixN<N, D>,
+}
+
+impl<N: ComplexField, D: DimName> Copy for UDU<N, D>
+where
+    DefaultAllocator: Allocator<N, D, D>,
+    MatrixN<N, D>: Copy,
+{
+}
+
+impl<N: ComplexField, D: DimName> UDU<N, D>
+where
+    DefaultAllocator: Allocator<N, D, D>,
+{
+    /// 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<N, D>) -> Self {
+        let mut d = MatrixN::<N, D>::zeros();
+        let mut u = MatrixN::<N, D>::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<f64>);
+    gen_tests!(f64, RandScalar<f64>);
+}