fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 05:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

remove unnecessary duplicate matrices from inverse

Browse Source
This commit is contained in:
Siddharth Bharat Purohit
2017-02-03 16:12:17 -05:00
committed by James Goppert
parent 47c0a93140
commit 7e3eff7b2d
1 changed files with 51 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/SquareMatrix.hpp
+51 -12
View File
@@ -47,9 +47,23 @@ public:
// inverse alias // inverse alias
inline SquareMatrix<Type, M> I() const inline SquareMatrix<Type, M> I() const
{ {
return inv(*this); SquareMatrix<Type, M> i;
if(inv(*this, i)) {
return i;
} else {
i.setZero();
return i;
}
} }
// inverse alias
inline bool I(SquareMatrix<Type, M> &i) const
{
return inv(*this, i);
}
Vector<Type, M> diag() const Vector<Type, M> diag() const
{ {
Vector<Type, M> res; Vector<Type, M> res;
@@ -108,11 +122,12 @@ SquareMatrix<Type, M> expm(const Matrix<Type, M, M> & A, size_t order=5)
return res; return res;
} }
/** /**
* inverse based on LU factorization with partial pivotting * inverse based on LU factorization with partial pivotting
*/ */
template<typename Type, size_t M> template<typename Type, size_t M>
SquareMatrix <Type, M> inv(const SquareMatrix<Type, M> & A) bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
{ {
SquareMatrix<Type, M> L; SquareMatrix<Type, M> L;
L.setIdentity(); L.setIdentity();
@@ -147,14 +162,12 @@ SquareMatrix <Type, M> inv(const SquareMatrix<Type, M> & A)
//printf("U:\n"); U.print(); //printf("U:\n"); U.print();
//printf("P:\n"); P.print(); //printf("P:\n"); P.print();
//fflush(stdout); //fflush(stdout);
ASSERT(fabsf(U(n, n)) > 1e-8f); //ASSERT(fabsf(U(n, n)) > 1e-8f);
#endif #endif
// failsafe, return zero matrix // failsafe, return zero matrix
if (fabsf(U(n, n)) < 1e-8f) { if (fabsf(U(n, n)) < 1e-8f) {
SquareMatrix<Type, M> zero; return false;
zero.setZero();
return zero;
} }
// for all rows below diagonal // for all rows below diagonal
@@ -173,7 +186,7 @@ SquareMatrix <Type, M> inv(const SquareMatrix<Type, M> & A)
//printf("U:\n"); U.print(); //printf("U:\n"); U.print();
// solve LY=P*I for Y by forward subst // solve LY=P*I for Y by forward subst
SquareMatrix<Type, M> Y = P; //SquareMatrix<Type, M> Y = P;
// for all columns of Y // for all columns of Y
for (size_t c = 0; c < M; c++) { for (size_t c = 0; c < M; c++) {
@@ -184,7 +197,7 @@ SquareMatrix <Type, M> inv(const SquareMatrix<Type, M> & A)
// for all existing y // for all existing y
// subtract the component they // subtract the component they
// contribute to the solution // contribute to the solution
Y(i, c) -= L(i, j) * Y(j, c); P(i, c) -= L(i, j) * P(j, c);
} }
// divide by the factor // divide by the factor
@@ -198,7 +211,7 @@ SquareMatrix <Type, M> inv(const SquareMatrix<Type, M> & A)
//printf("Y:\n"); Y.print(); //printf("Y:\n"); Y.print();
// solve Ux=y for x by back subst // solve Ux=y for x by back subst
SquareMatrix<Type, M> X = Y; //SquareMatrix<Type, M> X = Y;
// for all columns of X // for all columns of X
for (size_t c = 0; c < M; c++) { for (size_t c = 0; c < M; c++) {
@@ -212,20 +225,46 @@ SquareMatrix <Type, M> inv(const SquareMatrix<Type, M> & A)
// for all existing x // for all existing x
// subtract the component they // subtract the component they
// contribute to the solution // contribute to the solution
X(i, c) -= U(i, j) * X(j, c); P(i, c) -= U(i, j) * P(j, c);
} }
// divide by the factor // divide by the factor
// on current // on current
// term to be solved // term to be solved
X(i, c) /= U(i, i); if(fabsf(U(i,i)) < 1e-8f) {
return false;
}
P(i, c) /= U(i, i);
} }
} }
//check sanity of results
for (uint8_t i = 0; i < M; i++) {
for (uint8_t j = 0; j < M; j++) {
if (!PX4_ISFINITE(P(i,j))) {
return false;
}
}
}
//printf("X:\n"); X.print(); //printf("X:\n"); X.print();
return X; inv = P;
return true;
} }
/**
* inverse based on LU factorization with partial pivotting
*/
template<typename Type, size_t M>
SquareMatrix<Type, M> inv(const SquareMatrix<Type, M> & A)
{
SquareMatrix<Type, M> i;
if(inv(A, i)) {
return i;
} else {
i.setZero();
return i;
}
}
typedef SquareMatrix<float, 3> Matrix3f; typedef SquareMatrix<float, 3> Matrix3f;
} // namespace matrix } // namespace matrix