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 where
DefaultAllocator: Allocator<N, D, D>, DefaultAllocator: Allocator<N, D, D>,
{ {
/// Computes the UDU^T factorization as per NASA's "Navigation Filter Best Practices", NTRS document ID 20180003657 /// Computes the UDU^T factorization
/// section 7.2 page 64. /// NOTE: The provided matrix MUST be symmetric, and no verification is done in this regard.
/// NOTE: The provided matrix MUST be symmetric. /// Ref.: "Optimal control and estimation-Dover Publications", Robert F. Stengel, (1994) page 360
pub fn new(matrix: MatrixN<N, D>) -> Self { pub fn new(p: MatrixN<N, D>) -> Self {
let mut d = MatrixN::<N, D>::zeros(); let mut d = MatrixN::<N, D>::zeros();
let mut u = 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(); u[(n, n)] = N::one();
for j in (0..n - 1).rev() { for j in (0..n).rev() {
u[(j, n)] = matrix[(j, n)] / d[(n, n)]; u[(j, n)] = p[(j, n)] / d[(n, n)];
} }
for j in (1..n).rev() { for j in (0..n).rev() {
d[(j, j)] = matrix[(j, j)]; for k in j + 1..=n {
for k in (j + 1..n).rev() {
d[(j, j)] = d[(j, j)] + d[(k, k)] * u[(j, k)].powi(2); 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() { for i in (0..=j).rev() {
u[(i, j)] = matrix[(i, j)]; for k in j + 1..=n {
for k in j + 1..n { u[(i, j)] = u[(i, j)] + d[(k, k)] * u[(j, k)] * u[(i, k)];
u[(i, j)] = u[(i, j)] + d[(k, k)] * u[(i, k)] * u[(j, k)];
} }
u[(i, j)] = p[(i, j)] - u[(i, j)];
u[(i, j)] /= d[(j, j)]; u[(i, j)] /= d[(j, j)];
} }
u[(j, j)] = N::one();
} }
Self { u, d } Self { u, d }

8
tests/linalg/udu.rs
View File

@ -11,9 +11,9 @@ fn udu_simple() {
let udu = UDU::new(m); let udu = UDU::new(m);
// Rebuild // 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")] #[cfg(feature = "arbitrary")]
@ -48,9 +48,9 @@ mod quickcheck_tests {
let m = m.map(|e| e.0); let m = m.map(|e| e.0);
let udu = UDU::new(m.clone()); 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)
} }
} }
} }