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

nalgebra/linalg/
udu.rs
