PX4 Firmware
PX4 Autopilot Software http://px4.io
inverse.cpp
Go to the documentation of this file.
1 #include "test_macros.hpp"
2 #include <matrix/math.hpp>
3 
4 using namespace matrix;
5 
6 static const size_t n_large = 50;
7 
8 int main()
9 {
10  float data[9] = {0, 2, 3,
11  4, 5, 6,
12  7, 8, 10
13  };
14  float data_check[9] = {
15  -0.4f, -0.8f, 0.6f,
16  -0.4f, 4.2f, -2.4f,
17  0.6f, -2.8f, 1.6f
18  };
19 
20  SquareMatrix<float, 3> A(data);
21  SquareMatrix<float, 3> A_I = inv(A);
22  SquareMatrix<float, 3> A_I_check(data_check);
23  TEST((A_I - A_I_check).abs().max() < 1e-6f);
24 
25  // stess test
27  A_large.setIdentity();
29  A_large_I.setZero();
30 
31  for (size_t i = 0; i < n_large; i++) {
32  A_large_I = inv(A_large);
33  TEST(isEqual(A_large, A_large_I));
34  }
35 
36  SquareMatrix<float, 3> zero_test = zeros<float, 3, 3>();
37  inv(zero_test);
38 
39  // test pivotting
40  float data2[81] = {
41  -2, 1, 1, -1, -5, 1, 2, -1, 0,
42  -3, 2, -1, 0, 2, 2, -1, -5, 3,
43  0, 0, 0, 1, 4, -3, 3, 0, -2,
44  2, 2, -1, -2, -1, 0, 3, 0, 1,
45  -1, 2, -1, -1, -3, 3, 0, -2, 3,
46  0, 1, 1, -3, 3, -2, 0, -4, 0,
47  1, 0, 0, 0, 0, 0, -2, 4, -3,
48  1, -1, 0, -1, -1, 1, -1, -3, 4,
49  0, 3, -1, -2, 2, 1, -2, 0, -1
50  };
51 
52  float data2_check[81] = {
53  6, -4, 3, -3, -9, -8, -10, 8, 14,
54  -2, -7, -5, -3, -2, -2, -16, -5, 8,
55  -2, 0, -23, 7, -24, -5, -28, -14, 9,
56  3, -7, 2, -5, -4, -6, -13, 4, 13,
57  -1, 4, -8, 5, -8, 0, -3, -5, -2,
58  6, 7, -7, 7, -21, -7, -5, 3, 6,
59  1, 4, -4, 4, -7, -1, 0, -1, -1,
60  -7, 3, -11, 5, 1, 6, -1, -13, -10,
61  -8, 0, -11, 3, 3, 6, -5, -14, -8
62  };
63  SquareMatrix<float, 9> A2(data2);
64  SquareMatrix<float, 9> A2_I = inv(A2);
65  SquareMatrix<float, 9> A2_I_check(data2_check);
66  TEST((A2_I - A2_I_check).abs().max() < 1e-3f);
67  float data3[9] = {
68  0, 1, 2,
69  3, 4, 5,
70  6, 7, 9
71  };
72  float data3_check[9] = {
73  -0.3333333f, -1.6666666f, 1,
74  -1, 4, -2,
75  1, -2, 1
76  };
77  SquareMatrix<float, 3> A3(data3);
78  SquareMatrix<float, 3> A3_I = inv(A3);
79  SquareMatrix<float, 3> A3_I_check(data3_check);
80  TEST(isEqual(inv(A3), A3_I_check));
81  TEST(isEqual(A3_I, A3_I_check));
82  TEST(A3.I(A3_I));
83  TEST(isEqual(A3_I, A3_I_check));
84 
85  // cover singular matrices
86  A3(0, 0) = 0;
87  A3(0, 1) = 0;
88  A3(0, 2) = 0;
89  A3_I = inv(A3);
90  SquareMatrix<float, 3> Z3 = zeros<float, 3, 3>();
91  TEST(!A3.I(A3_I));
92  TEST(!Z3.I(A3_I));
93  TEST(isEqual(A3_I, Z3));
94  TEST(isEqual(A3.I(), Z3));
95 
96  // cover NaN
97  A3(0, 0) = NAN;
98  A3(0, 1) = 0;
99  A3(0, 2) = 0;
100  A3_I = inv(A3);
101  TEST(isEqual(A3_I, Z3));
102  TEST(isEqual(A3.I(), Z3));
103 
104  float data4[9] = {
105  1.33471626f, 0.74946721f, -0.0531679f,
106  0.74946721f, 1.07519593f, 0.08036323f,
107  -0.0531679f, 0.08036323f, 1.01618474f
108  };
109  SquareMatrix<float, 3> A4(data4);
110 
111  float data4_cholesky[9] = {
112  1.15529921f, 0.f, 0.f,
113  0.6487213f, 0.80892311f, 0.f,
114  -0.04602089f, 0.13625271f, 0.99774847f
115  };
116  SquareMatrix<float, 3> A4_cholesky_check(data4_cholesky);
117  SquareMatrix<float, 3> A4_cholesky = cholesky(A4);
118  TEST(isEqual(A4_cholesky_check, A4_cholesky));
119 
121  I3.setIdentity();
122  TEST(isEqual(choleskyInv(A4)*A4, I3));
123  TEST(isEqual(cholesky(Z3), Z3));
124  return 0;
125 }
126 
127 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
SquareMatrix< Type, M > cholesky(const SquareMatrix< Type, M > &A)
cholesky decomposition
int main()
Definition: inverse.cpp:8
bool isEqual(const Matrix< Type, M, N > &x, const Matrix< Type, M, N > &y, const Type eps=1e-4f)
Definition: Matrix.hpp:571
uint8_t * data
Definition: dataman.cpp:149
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
bool inv(const SquareMatrix< Type, M > &A, SquareMatrix< Type, M > &inv)
inverse based on LU factorization with partial pivotting
SquareMatrix< Type, M > I() const
#define TEST(X)
Definition: test_macros.hpp:14
Dual< Scalar, N > max(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:224
SquareMatrix< Type, M > choleskyInv(const SquareMatrix< Type, M > &A)
cholesky inverse
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
Definition: Dual.hpp:196
static const size_t n_large
Definition: inverse.cpp:6