fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 02:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

geninv(): improve runtime performance and reduce stack usage

Browse Source 
- use associativity of matrix operations to reduce size of temporary
  matrices and number of multiplications in the M <= N case
- minimize the number of temporary matrices required
This commit is contained in:
Beat Küng
2021-09-03 12:07:26 +02:00
parent 3d1c9b988d
commit dc7f119b99
1 changed files with 10 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/PseudoInverse.hpp
+10 -6
View File
@@ -28,23 +28,27 @@ Matrix<Type, N, M> geninv(const Matrix<Type, M, N> & G)
SquareMatrix<Type, M> A = G * G.transpose(); SquareMatrix<Type, M> A = G * G.transpose();
SquareMatrix<Type, M> L = fullRankCholesky(A, rank); SquareMatrix<Type, M> L = fullRankCholesky(A, rank);
SquareMatrix<Type, M> LtL = L.transpose() * L; A = L.transpose() * L;
SquareMatrix<Type, M> X; SquareMatrix<Type, M> X;
if (!inv(LtL, X, rank)) { if (!inv(A, X, rank)) {
return Matrix<Type, N, M>(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues return Matrix<Type, N, M>(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues
} }
return G.transpose() * L * X * X * L.transpose(); // doing an intermediate assignment reduces stack usage
A = X * X * L.transpose();
return G.transpose() * (L * A);
} else { } else {
SquareMatrix<Type, N> A = G.transpose() * G; SquareMatrix<Type, N> A = G.transpose() * G;
SquareMatrix<Type, N> L = fullRankCholesky(A, rank); SquareMatrix<Type, N> L = fullRankCholesky(A, rank);
SquareMatrix<Type, N> LtL = L.transpose() * L; A = L.transpose() * L;
SquareMatrix<Type, N> X; SquareMatrix<Type, N> X;
if(!inv(LtL, X, rank)) { if(!inv(A, X, rank)) {
return Matrix<Type, N, M>(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues return Matrix<Type, N, M>(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues
} }
return L * X * X * L.transpose() * G.transpose(); // doing an intermediate assignment reduces stack usage
A = X * X * L.transpose();
return (L * A) * G.transpose();
} }
} }