M-Labs/nalgebra
3
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Implement requested changes

Browse Source 
Signed-off-by: Christopher Rabotin <christopher.rabotin@gmail.com>
This commit is contained in:
Christopher Rabotin 2020-10-26 22:06:37 -06:00 committed by Crozet Sébastien
parent 06861a9755
commit 4ff4911ac3
1 changed files with 22 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/linalg/udu.rs
View File

@ -4,10 +4,21 @@ 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::ComplexField;
/// 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: ComplexField, D: Dim>
where
@ -36,33 +47,30 @@ where
/// 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_as_dim = D::from_usize(n);
let n_dim = p.data.shape().1;
let mut d = VectorN::<N, D>::zeros_generic(n_as_dim, U1);
let mut u = MatrixN::<N, D>::zeros_generic(n_as_dim, n_as_dim);
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[(n - 1, n - 1)] = N::one();
for j in (0..n - 1).rev() {
u[(j, n - 1)] = p[(j, n - 1)] / d[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[j] + d[k] * u[(j, k)].powi(2);
d_j += d[k] * u[(j, k)].powi(2);
}
d[j] = p[(j, j)] - d[j];
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[(i, j)] = u[(i, j)] + d[k] * u[(j, k)] * u[(i, k)];
u_ij += d[k] * u[(j, k)] * u[(i, k)];
}
u[(i, j)] = p[(i, j)] - u[(i, j)];
u[(i, j)] /= d[j];
u[(i, j)] = (p[(i, j)] - u_ij) / d[j];
}
u[(j, j)] = N::one();