PX4 Firmware
PX4 Autopilot Software http://px4.io
pseudoInverse.cpp
Go to the documentation of this file.
1 #include "test_macros.hpp"
3 
4 using namespace matrix;
5 
6 static const size_t n_large = 20;
7 
8 int main()
9 {
10  // 3x4 Matrix test
11  float data0[12] = {
12  0.f, 1.f, 2.f, 3.f,
13  4.f, 5.f, 6.f, 7.f,
14  8.f, 9.f, 10.f, 11.f
15  };
16 
17  float data0_check[12] = {
18  -0.3375f, -0.1f, 0.1375f,
19  -0.13333333f, -0.03333333f, 0.06666667f,
20  0.07083333f, 0.03333333f, -0.00416667f,
21  0.275f, 0.1f, -0.075f
22  };
23 
24  Matrix<float, 3, 4> A0(data0);
25  Matrix<float, 4, 3> A0_I = geninv(A0);
26  Matrix<float, 4, 3> A0_I_check(data0_check);
27 
28  TEST((A0_I - A0_I_check).abs().max() < 1e-5);
29 
30  // 4x3 Matrix test
31  float data1[12] = {
32  0.f, 4.f, 8.f,
33  1.f, 5.f, 9.f,
34  2.f, 6.f, 10.f,
35  3.f, 7.f, 11.f
36  };
37 
38  float data1_check[12] = {
39  -0.3375f, -0.13333333f, 0.07083333f, 0.275f,
40  -0.1f, -0.03333333f, 0.03333333f, 0.1f,
41  0.1375f, 0.06666667f, -0.00416667f, -0.075f
42  };
43 
44  Matrix<float, 4, 3> A1(data1);
45  Matrix<float, 3, 4> A1_I = geninv(A1);
46  Matrix<float, 3, 4> A1_I_check(data1_check);
47 
48  TEST((A1_I - A1_I_check).abs().max() < 1e-5);
49 
50  // Stess test
51  Matrix<float, n_large, n_large - 1> A_large;
52  A_large.setIdentity();
53  Matrix<float, n_large - 1, n_large> A_large_I;
54 
55  for (size_t i = 0; i < n_large; i++) {
56  A_large_I = geninv(A_large);
57  TEST(isEqual(A_large, A_large_I.T()));
58  }
59 
60  // Square matrix test
61  float data2[9] = {0, 2, 3,
62  4, 5, 6,
63  7, 8, 10
64  };
65  float data2_check[9] = {
66  -0.4f, -0.8f, 0.6f,
67  -0.4f, 4.2f, -2.4f,
68  0.6f, -2.8f, 1.6f
69  };
70 
71  SquareMatrix<float, 3> A2(data2);
72  SquareMatrix<float, 3> A2_I = geninv(A2);
73  SquareMatrix<float, 3> A2_I_check(data2_check);
74  TEST((A2_I - A2_I_check).abs().max() < 1e-3);
75 
76  // Null matrix test
78  Matrix<float, 16, 6> A3_I = geninv(A3);
79  Matrix<float, 16, 6> A3_I_check;
80  TEST((A3_I - A3_I_check).abs().max() < 1e-5);
81 
82  // Mock-up effectiveness matrix
83  const float B_quad_w[6][16] = {
84  {-0.5717536f, 0.43756646f, 0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
85  { 0.35355328f, -0.35355328f, 0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
86  { 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
87  { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
88  { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
89  {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
90  };
92  const float A_quad_w[16][6] = {
93  { -0.495383f, 0.707107f, 0.765306f, 0.0f, 0.0f, -1.000000f },
94  { 0.495383f, -0.707107f, 1.000000f, 0.0f, 0.0f, -1.000000f },
95  { 0.495383f, 0.707107f, -0.765306f, 0.0f, 0.0f, -1.000000f },
96  { -0.495383f, -0.707107f, -1.000000f, 0.0f, 0.0f, -1.000000f },
97  { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
98  { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
99  { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
100  { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
101  { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
102  { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
103  { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
104  { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
105  { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
106  { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
107  { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
108  { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}
109  };
110  Matrix<float, 16, 6> A_check = Matrix<float, 16, 6>(A_quad_w);
112  TEST((A - A_check).abs().max() < 1e-5);
113 
114  // Test error case with erroneous rank in internal impl functions
117  Matrix<float, 3, 2> retM_check;
120  Matrix<float, 2, 3> retN_check;
122  TEST((retM0 - retM_check).abs().max() < 1e-5);
123  TEST((retN0 - retN_check).abs().max() < 1e-5);
124 
127  TEST((retM1 - retM_check).abs().max() < 1e-5);
128  TEST((retN1 - retN_check).abs().max() < 1e-5);
129 
130  float float_scale = 1.f;
131  fullRankCholeskyTolerance(float_scale);
132  double double_scale = 1.;
133  fullRankCholeskyTolerance(double_scale);
134  TEST(static_cast<double>(float_scale) > double_scale);
135 
136  return 0;
137 }
138 
139 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
int main()
static Matrix< Type, N, M > genInvUnderdetermined(const Matrix< Type, M, N > &G, const Matrix< Type, M, M > &L, size_t rank)
static const size_t n_large
void setIdentity()
Definition: Matrix.hpp:447
bool isEqual(const Matrix< Type, M, N > &x, const Matrix< Type, M, N > &y, const Type eps=1e-4f)
Definition: Matrix.hpp:571
static Matrix< Type, N, M > genInvOverdetermined(const Matrix< Type, M, N > &G, const Matrix< Type, N, N > &L, size_t rank)
A0[0][0]
Definition: calcH_YAW312.c:14
void fullRankCholeskyTolerance(Type &tol)
Full rank Cholesky factorization of A.
Implementation of matrix pseudo inverse.
#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
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
Definition: Dual.hpp:196
Matrix< Type, N, M > geninv(const Matrix< Type, M, N > &G)
Geninv Fast pseudoinverse based on full rank cholesky factorisation.