fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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-08-21 19:06:49 +02:00
parent 3d1c9b988d
commit dc7f119b99
1 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
matrix/PseudoInverse.hpp
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> L = fullRankCholesky(A, rank);
SquareMatrix<Type, M> LtL = L.transpose() * L;
A = L.transpose() * L;
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 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 {
SquareMatrix<Type, N> A = G.transpose() * G;
SquareMatrix<Type, N> L = fullRankCholesky(A, rank);
SquareMatrix<Type, N> LtL = L.transpose() * L;
A = L.transpose() * L;
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 L * X * X * L.transpose() * G.transpose();
// doing an intermediate assignment reduces stack usage
A = X * X * L.transpose();
return (L * A) * G.transpose();
}
}