1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98

#[cfg(feature = "serde-serialize-no-std")]
use serde::{Deserialize, Serialize};

use crate::allocator::Allocator;
use crate::base::{Const, DefaultAllocator, OMatrix, OVector};
use crate::dimension::Dim;
use simba::scalar::RealField;

/// UDU factorization.
#[cfg_attr(feature = "serde-serialize-no-std", derive(Serialize, Deserialize))]
#[cfg_attr(
    feature = "serde-serialize-no-std",
    serde(bound(serialize = "OVector<T, D>: Serialize, OMatrix<T, D, D>: Serialize"))
)]
#[cfg_attr(
    feature = "serde-serialize-no-std",
    serde(bound(
        deserialize = "OVector<T, D>: Deserialize<'de>, OMatrix<T, D, D>: Deserialize<'de>"
    ))
)]
#[derive(Clone, Debug)]
pub struct UDU<T: RealField, D: Dim>
where
    DefaultAllocator: Allocator<T, D> + Allocator<T, D, D>,
{
    /// The upper triangular matrix resulting from the factorization
    pub u: OMatrix<T, D, D>,
    /// The diagonal matrix resulting from the factorization
    pub d: OVector<T, D>,
}

impl<T: RealField, D: Dim> Copy for UDU<T, D>
where
    DefaultAllocator: Allocator<T, D> + Allocator<T, D, D>,
    OVector<T, D>: Copy,
    OMatrix<T, D, D>: Copy,
{
}

impl<T: RealField, D: Dim> UDU<T, D>
where
    DefaultAllocator: Allocator<T, D> + Allocator<T, 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: OMatrix<T, D, D>) -> Option<Self> {
        let n = p.ncols();
        let n_dim = p.shape_generic().1;

        let mut d = OVector::zeros_generic(n_dim, Const::<1>);
        let mut u = OMatrix::zeros_generic(n_dim, n_dim);

        d[n - 1] = p[(n - 1, n - 1)].clone();

        if d[n - 1].is_zero() {
            return None;
        }

        u.column_mut(n - 1)
            .axpy(T::one() / d[n - 1].clone(), &p.column(n - 1), T::zero());

        for j in (0..n - 1).rev() {
            let mut d_j = d[j].clone();
            for k in j + 1..n {
                d_j += d[k].clone() * u[(j, k)].clone().powi(2);
            }

            d[j] = p[(j, j)].clone() - d_j;

            if d[j].is_zero() {
                return None;
            }

            for i in (0..=j).rev() {
                let mut u_ij = u[(i, j)].clone();
                for k in j + 1..n {
                    u_ij += d[k].clone() * u[(j, k)].clone() * u[(i, k)].clone();
                }

                u[(i, j)] = (p[(i, j)].clone() - u_ij) / d[j].clone();
            }

            u[(j, j)] = T::one();
        }

        Some(Self { u, d })
    }

    /// Returns the diagonal elements as a matrix
    #[must_use]
    pub fn d_matrix(&self) -> OMatrix<T, D, D> {
        OMatrix::from_diagonal(&self.d)
    }
}