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

Fixed UDU algorithm 

Signed-off-by: Christopher Rabotin <christopher.rabotin@gmail.com>
This commit is contained in:
Christopher Rabotin 2020-09-25 19:21:14 -06:00 committed by Crozet Sébastien
parent d534c3bf9d
commit a8d40423ea
2 changed files with 23 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
src/linalg/udu.rs
View File

@ -30,37 +30,40 @@ 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 {
/// Computes the UDU^T factorization
/// NOTE: The provided matrix MUST be symmetric, and no verification is done in this regard.
/// Ref.: "Optimal control and estimation-Dover Publications", Robert F. Stengel, (1994) page 360
pub fn new(p: MatrixN<N, D>) -> Self {
let mut d = MatrixN::<N, D>::zeros();
let mut u = MatrixN::<N, D>::zeros();
let n = matrix.ncols();
let n = p.ncols() - 1;
d[(n, n)] = matrix[(n, n)];
d[(n, n)] = p[(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 (0..n).rev() {
u[(j, n)] = p[(j, n)] / d[(n, n)];
}
for j in (1..n).rev() {
d[(j, j)] = matrix[(j, j)];
for k in (j + 1..n).rev() {
for j in (0..n).rev() {
for k in j + 1..=n {
d[(j, j)] = d[(j, j)] + d[(k, k)] * u[(j, k)].powi(2);
}
u[(j, j)] = N::one();
d[(j, j)] = p[(j, j)] - d[(j, j)];
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)];
for i in (0..=j).rev() {
for k in j + 1..=n {
u[(i, j)] = u[(i, j)] + d[(k, k)] * u[(j, k)] * u[(i, k)];
}
u[(i, j)] = p[(i, j)] - u[(i, j)];
u[(i, j)] /= d[(j, j)];
}
u[(j, j)] = N::one();
}
Self { u, d }

8
tests/linalg/udu.rs
View File

@ -11,9 +11,9 @@ fn udu_simple() {
let udu = UDU::new(m);
// Rebuild
let p = &udu.u * &udu.d * &udu.u.transpose();
let p = udu.u * udu.d * udu.u.transpose();
assert!(relative_eq!(m, 2.0*p, epsilon = 1.0e-7));
assert!(relative_eq!(m, p, epsilon = 3.0e-16));
}
#[cfg(feature = "arbitrary")]
@ -48,9 +48,9 @@ mod quickcheck_tests {
let m = m.map(|e| e.0);
let udu = UDU::new(m.clone());
let p = &udu.u * &udu.d * &udu.u.transpose();
let p = udu.u * udu.d * udu.u.transpose();
relative_eq!(m, p, epsilon = 1.0e-7)
relative_eq!(m, p, epsilon = 3.0e-16)
}
}
}