nalgebra/linalg/
udu.rs

1#[cfg(feature = "serde-serialize-no-std")]
2use serde::{Deserialize, Serialize};
3
4use crate::allocator::Allocator;
5use crate::base::{Const, DefaultAllocator, OMatrix, OVector};
6use crate::dimension::Dim;
7use simba::scalar::RealField;
8
9/// UDU factorization.
10#[cfg_attr(feature = "serde-serialize-no-std", derive(Serialize, Deserialize))]
11#[cfg_attr(
12    feature = "serde-serialize-no-std",
13    serde(bound(serialize = "OVector<T, D>: Serialize, OMatrix<T, D, D>: Serialize"))
14)]
15#[cfg_attr(
16    feature = "serde-serialize-no-std",
17    serde(bound(
18        deserialize = "OVector<T, D>: Deserialize<'de>, OMatrix<T, D, D>: Deserialize<'de>"
19    ))
20)]
21#[cfg_attr(feature = "defmt", derive(defmt::Format))]
22#[derive(Clone, Debug)]
23pub struct UDU<T: RealField, D: Dim>
24where
25    DefaultAllocator: Allocator<D> + Allocator<D, D>,
26{
27    /// The upper triangular matrix resulting from the factorization
28    pub u: OMatrix<T, D, D>,
29    /// The diagonal matrix resulting from the factorization
30    pub d: OVector<T, D>,
31}
32
33impl<T: RealField, D: Dim> Copy for UDU<T, D>
34where
35    DefaultAllocator: Allocator<D> + Allocator<D, D>,
36    OVector<T, D>: Copy,
37    OMatrix<T, D, D>: Copy,
38{
39}
40
41impl<T: RealField, D: Dim> UDU<T, D>
42where
43    DefaultAllocator: Allocator<D> + Allocator<D, D>,
44{
45    /// Computes the UDU^T factorization.
46    ///
47    /// The input matrix `p` is assumed to be symmetric and this decomposition will only read
48    /// the upper-triangular part of `p`.
49    ///
50    /// Ref.: "Optimal control and estimation-Dover Publications", Robert F. Stengel, (1994) page 360
51    pub fn new(p: OMatrix<T, D, D>) -> Option<Self> {
52        let n = p.ncols();
53        let n_dim = p.shape_generic().1;
54
55        let mut d = OVector::zeros_generic(n_dim, Const::<1>);
56        let mut u = OMatrix::zeros_generic(n_dim, n_dim);
57
58        d[n - 1] = p[(n - 1, n - 1)].clone();
59
60        if d[n - 1].is_zero() {
61            return None;
62        }
63
64        u.column_mut(n - 1)
65            .axpy(T::one() / d[n - 1].clone(), &p.column(n - 1), T::zero());
66
67        for j in (0..n - 1).rev() {
68            let mut d_j = d[j].clone();
69            for k in j + 1..n {
70                d_j += d[k].clone() * u[(j, k)].clone().powi(2);
71            }
72
73            d[j] = p[(j, j)].clone() - d_j;
74
75            if d[j].is_zero() {
76                return None;
77            }
78
79            for i in (0..=j).rev() {
80                let mut u_ij = u[(i, j)].clone();
81                for k in j + 1..n {
82                    u_ij += d[k].clone() * u[(j, k)].clone() * u[(i, k)].clone();
83                }
84
85                u[(i, j)] = (p[(i, j)].clone() - u_ij) / d[j].clone();
86            }
87
88            u[(j, j)] = T::one();
89        }
90
91        Some(Self { u, d })
92    }
93
94    /// Returns the diagonal elements as a matrix
95    #[must_use]
96    pub fn d_matrix(&self) -> OMatrix<T, D, D> {
97        OMatrix::from_diagonal(&self.d)
98    }
99}
nalgebra/linalg/udu.rs

nalgebra/linalg/
udu.rs
