fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

lib: matrix: SquareMatrix: Deal with the special case of M=1

Browse Source 
Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
This commit is contained in:
Patrick José Pereira 2023-06-19 09:35:48 -03:00 committed by Beat Küng
parent 724f5a97a9
commit 39e04d9712
1 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/lib/matrix/matrix/SquareMatrix.hpp
View File

@ -322,6 +322,19 @@ SquareMatrix<Type, M> expm(const Matrix<Type, M, M> &A, size_t order = 5)
return res;
}
/**
* Deal with the special case where the square matrix is 1
*/
template<typename Type>
bool inv(const SquareMatrix<Type, 1> &A, SquareMatrix<Type, 1> &inv, size_t rank = 1)
{
if (std::fabs(A(0, 0)) < Type(FLT_EPSILON)) {
return false;
}
inv(0, 0) = Type(1) / A(0, 0);
return true;
}
/**
* inverse based on LU factorization with partial pivotting