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

Analytic inverse implementation (#122)

Browse Source 
* Add analytic 2x2 matrix inverse

* Add analytical 3x3 matrix inverse
This commit is contained in:
kritz
2020-03-04 09:14:04 +01:00
committed by GitHub
parent 649c837b6b
commit 4873dc1c1e
2 changed files with 79 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/SquareMatrix.hpp
+41
View File
@@ -435,6 +435,47 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
return true;
}
template<typename Type>
bool inv(const SquareMatrix<Type, 2> & A, SquareMatrix<Type, 2> & inv)
{
Type det = A(0, 0) * A(1, 1) - A(1, 0) * A(0, 1);
if(fabs(static_cast<float>(det)) < FLT_EPSILON || !is_finite(det)) {
return false;
}
inv(0, 0) = A(1, 1);
inv(1, 0) = -A(1, 0);
inv(0, 1) = -A(0, 1);
inv(1, 1) = A(0, 0);
inv /= det;
return true;
}
template<typename Type>
bool inv(const SquareMatrix<Type, 3> & A, SquareMatrix<Type, 3> & inv)
{
Type det = A(0, 0) * (A(1, 1) * A(2, 2) - A(2, 1) * A(1, 2)) -
A(0, 1) * (A(1, 0) * A(2, 2) - A(1, 2) * A(2, 0)) +
A(0, 2) * (A(1, 0) * A(2, 1) - A(1, 1) * A(2, 0));
if(fabs(static_cast<float>(det)) < FLT_EPSILON || !is_finite(det)) {
return false;
}
inv(0, 0) = A(1, 1) * A(2, 2) - A(2, 1) * A(1, 2);
inv(0, 1) = A(0, 2) * A(2, 1) - A(0, 1) * A(2, 2);
inv(0, 2) = A(0, 1) * A(1, 2) - A(0, 2) * A(1, 1);
inv(1, 0) = A(1, 2) * A(2, 0) - A(1, 0) * A(2, 2);
inv(1, 1) = A(0, 0) * A(2, 2) - A(0, 2) * A(2, 0);
inv(1, 2) = A(1, 0) * A(0, 2) - A(0, 0) * A(1, 2);
inv(2, 0) = A(1, 0) * A(2, 1) - A(2, 0) * A(1, 1);
inv(2, 1) = A(2, 0) * A(0, 1) - A(0, 0) * A(2, 1);
inv(2, 2) = A(0, 0) * A(1, 1) - A(1, 0) * A(0, 1);
inv /= det;
return true;
}
/**
* inverse based on LU factorization with partial pivotting
*/