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

Fix matrix inverse pivotting logic.

Browse Source
This commit is contained in:
James Goppert 2016-11-22 10:04:45 -05:00
parent 34fccdd680
commit f3e478cbff
2 changed files with 43 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
matrix/SquareMatrix.hpp
View File

@ -128,16 +128,16 @@ SquareMatrix <Type, M> inv(const SquareMatrix<Type, M> & A)
// if diagonal is zero, swap with row below
if (fabsf(U(n, n)) < 1e-8f) {
//printf("trying pivot for row %d\n",n);
for (size_t i = 0; i < M; i++) {
if (i == n) {
continue;
}
for (size_t i = n + 1; i < M; i++) {
//printf("\ttrying row %d\n",i);
if (fabsf(U(i, n)) > 1e-8f) {
//printf("swapped %d\n",i);
U.swapRows(i, n);
P.swapRows(i, n);
L.swapRows(i, n);
L.swapCols(i, n);
break;
}
}
}

39
test/inverse.cpp
View File

@ -41,6 +41,45 @@ int main()
SquareMatrix<float, 3> zero_test = zeros<float, 3, 3>();
inv(zero_test);
// test pivotting
float data2[81] = {
-2, 1, 1, -1, -5, 1, 2, -1, 0,
-3, 2, -1, 0, 2, 2, -1, -5, 3,
0, 0, 0, 1, 4, -3, 3, 0, -2,
2, 2, -1, -2, -1, 0, 3, 0, 1,
-1, 2, -1, -1, -3, 3, 0, -2, 3,
0, 1, 1, -3, 3, -2, 0, -4, 0,
1, 0, 0, 0, 0, 0, -2, 4, -3,
1, -1, 0, -1, -1, 1, -1, -3, 4,
0, 3, -1, -2, 2, 1, -2, 0, -1};
float data2_check[81] = {
6, -4, 3, -3 , -9, -8, -10, 8, 14,
-2, -7, -5, -3 , -2, -2, -16, -5, 8,
-2, 0, -23, 7, -24, -5, -28, -14, 9,
3, -7, 2, -5, -4, -6, -13, 4, 13,
-1, 4, -8, 5, -8, 0, -3, -5, -2,
6, 7, -7, 7, -21, -7, -5, 3, 6,
1, 4, -4, 4, -7, -1, 0, -1, -1,
-7, 3, -11, 5, 1, 6, -1, -13, -10,
-8, 0, -11, 3, 3, 6, -5, -14, -8};
SquareMatrix<float, 9> A2(data2);
SquareMatrix<float, 9> A2_I = inv(A2);
SquareMatrix<float, 9> A2_I_check(data2_check);
TEST((A2_I - A2_I_check).abs().max() < 1e-3);
float data3[9] = {
0, 1, 2,
3, 4, 5,
6, 7, 9};
float data3_check[9] = {
-0.3333333f, -1.6666666f, 1,
-1, 4, -2,
1, -2, 1
};
SquareMatrix<float, 3> A3(data3);
SquareMatrix<float, 3> A3_I = inv(A3);
SquareMatrix<float, 3> A3_I_check(data3_check);
TEST((A3_I - A3_I_check).abs().max() < 1e-5);
return 0;
}