PX4 Firmware
PX4 Autopilot Software http://px4.io
test_mathlib.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2013-2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file test_mathlib.cpp
36  * Tests for the PX4 math library.
37  */
38 
39 #include <unit_test.h>
40 
41 #include <errno.h>
42 #include <fcntl.h>
43 #include <float.h>
44 #include <math.h>
45 #include <px4_platform_common/px4_config.h>
46 #include <stdint.h>
47 #include <stdio.h>
48 #include <stdlib.h>
49 #include <sys/types.h>
50 #include <unistd.h>
51 
52 #include <px4_platform_common/log.h>
53 #include <stdio.h>
54 #include <stdlib.h>
55 #include <unistd.h>
56 #include <string.h>
57 #include <time.h>
58 #include <mathlib/mathlib.h>
59 #include <systemlib/err.h>
60 #include <drivers/drv_hrt.h>
61 #include <matrix/math.hpp>
62 
63 #include "tests_main.h"
64 
65 class MathlibTest : public UnitTest
66 {
67 public:
68  virtual bool run_tests();
69 
70 private:
71  bool testVector2();
72  bool testVector3();
73  bool testVector10();
74  bool testMatrix3x3();
75  bool testMatrix10x10();
80  bool testQuaternionRotate();
81  bool testFinite();
82 };
83 
84 #define TEST_OP(_title, _op) { unsigned int n = 30000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); PX4_INFO(_title ": %.6fus", (double)(t1 - t0) / n); }
85 
86 using namespace math;
87 
89 {
90  {
92  matrix::Vector2f v1(1.0f, 2.0f);
93  matrix::Vector2f v2(1.0f, -1.0f);
94  float data[2] = {1.0f, 2.0f};
95  TEST_OP("Constructor matrix::Vector2f()", matrix::Vector2f v3);
96  TEST_OP("Constructor matrix::Vector2f(matrix::Vector2f)", matrix::Vector2f v3(v1); ut_assert_true(v3 == v1); v3.zero());
97  TEST_OP("Constructor matrix::Vector2f(float[])", matrix::Vector2f v3(data));
98  TEST_OP("Constructor matrix::Vector2f(float, float)", matrix::Vector2f v3(1.0f, 2.0f));
99  TEST_OP("matrix::Vector2f = matrix::Vector2f", v = v1);
100  TEST_OP("matrix::Vector2f + matrix::Vector2f", v + v1);
101  TEST_OP("matrix::Vector2f - matrix::Vector2f", v - v1);
102  TEST_OP("matrix::Vector2f += matrix::Vector2f", v += v1);
103  TEST_OP("matrix::Vector2f -= matrix::Vector2f", v -= v1);
104  TEST_OP("matrix::Vector2f * matrix::Vector2f", v * v1);
105  TEST_OP("matrix::Vector2f %% matrix::Vector2f", v1 % v2);
106  }
107  return true;
108 }
109 
111 {
112 
113  {
115  matrix::Vector3f v1(1.0f, 2.0f, 0.0f);
116  matrix::Vector3f v2(1.0f, -1.0f, 2.0f);
117  float data[3] = {1.0f, 2.0f, 3.0f};
118  TEST_OP("Constructor matrix::Vector3f()", matrix::Vector3f v3);
119  TEST_OP("Constructor matrix::Vector3f(matrix::Vector3f)", matrix::Vector3f v3(v1); ut_assert_true(v3 == v1); v3.zero());
120  TEST_OP("Constructor matrix::Vector3f(float[])", matrix::Vector3f v3(data));
121  TEST_OP("Constructor matrix::Vector3f(float, float, float)", matrix::Vector3f v3(1.0f, 2.0f, 3.0f));
122  TEST_OP("matrix::Vector3f = matrix::Vector3f", v = v1);
123  TEST_OP("matrix::Vector3f + matrix::Vector3f", v + v1);
124  TEST_OP("matrix::Vector3f - matrix::Vector3f", v - v1);
125  TEST_OP("matrix::Vector3f += matrix::Vector3f", v += v1);
126  TEST_OP("matrix::Vector3f -= matrix::Vector3f", v -= v1);
127  TEST_OP("matrix::Vector3f * float", v1 * 2.0f);
128  TEST_OP("matrix::Vector3f / float", v1 / 2.0f);
129  TEST_OP("matrix::Vector3f *= float", v1 *= 2.0f);
130  TEST_OP("matrix::Vector3f /= float", v1 /= 2.0f);
131  TEST_OP("matrix::Vector3f * matrix::Vector3f", v * v1);
132  TEST_OP("matrix::Vector3f %% matrix::Vector3f", v1 % v2);
133  TEST_OP("matrix::Vector3f length", v1.length());
134 #pragma GCC diagnostic push
135 #pragma GCC diagnostic ignored "-Wunused-variable"
136  // Need pragma here instead of moving variable out of TEST_OP and just reference because
137  // TEST_OP measures performance of vector operations.
138  TEST_OP("matrix::Vector3f element read", volatile float a = v1(0));
139 #pragma GCC diagnostic pop
140  TEST_OP("matrix::Vector3f element write", v1(0) = 1.0f);
141  }
142  return true;
143 }
144 
146 {
147  {
148  matrix::Matrix3f m1;
149  m1.identity();
150  matrix::Matrix3f m2;
151  m2.identity();
152  matrix::Vector3f v1(1.0f, 2.0f, 0.0f);
153  TEST_OP("matrix::Matrix3f * matrix::Vector3f", m1 * v1);
154  TEST_OP("matrix::Matrix3f + matrix::Matrix3f", m1 + m2);
155  TEST_OP("matrix::Matrix3f * matrix::Matrix3f", m1 * m2);
156  }
157  return true;
158 }
159 
161 {
162  int rc = true;
163  {
164  //PX4_INFO("Nonsymmetric matrix operations test");
165  // test nonsymmetric +, -, +=, -=
166 
167  float data1[2][3] = {{1, 2, 3}, {4, 5, 6}};
168  float data2[2][3] = {{2, 4, 6}, {8, 10, 12}};
169  float data3[2][3] = {{3, 6, 9}, {12, 15, 18}};
170 
171  matrix::Matrix<float, 2, 3> m1(data1);
172  matrix::Matrix<float, 2, 3> m2(data2);
173  matrix::Matrix<float, 2, 3> m3(data3);
174 
175  if (m1 + m2 != m3) {
176  PX4_ERR("matrix::Matrix<float, 2, 3> + matrix::Matrix<float, 2, 3> failed!");
177  (m1 + m2).print();
178  printf("!=\n");
179  m3.print();
180  rc = false;
181  }
182 
183  ut_assert("m1 + m2 == m3", m1 + m2 == m3);
184 
185  if (m3 - m2 != m1) {
186  PX4_ERR("matrix::Matrix<float, 2, 3> - matrix::Matrix<float, 2, 3> failed!");
187  (m3 - m2).print();
188  printf("!=\n");
189  m1.print();
190  rc = false;
191  }
192 
193  ut_assert("m3 - m2 == m1", m3 - m2 == m1);
194 
195  m1 += m2;
196 
197  if (m1 != m3) {
198  PX4_ERR("matrix::Matrix<float, 2, 3> += matrix::Matrix<float, 2, 3> failed!");
199  m1.print();
200  printf("!=\n");
201  m3.print();
202  rc = false;
203  }
204 
205  ut_assert("m1 == m3", m1 == m3);
206 
207  m1 -= m2;
208  matrix::Matrix<float, 2, 3> m1_orig(data1);
209 
210  if (m1 != m1_orig) {
211  PX4_ERR("matrix::Matrix<float, 2, 3> -= matrix::Matrix<float, 2, 3> failed!");
212  m1.print();
213  printf("!=\n");
214  m1_orig.print();
215  rc = false;
216  }
217 
218  ut_assert("m1 == m1_orig", m1 == m1_orig);
219 
220  }
221  return rc;
222 }
223 
225 {
226  // test conversion rotation matrix to quaternion and back
227  matrix::Dcmf R_orig;
228  matrix::Dcmf R;
229  matrix::Quatf q;
230  float diff = 0.2f;
231  float tol = 0.00001f;
232 
233  //PX4_INFO("Quaternion transformation methods test.");
234 
235  for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
236  for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
237  for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
238  R_orig = matrix::Eulerf(roll, pitch, yaw);
239  q = R_orig;
240  R = q;
241 
242  for (int i = 0; i < 3; i++) {
243  for (int j = 0; j < 3; j++) {
244  ut_assert("matrix::Quatf method 'from_dcm' or 'to_dcm' outside tolerance!", fabsf(R_orig(i, j) - R(i, j)) < tol);
245  }
246  }
247  }
248  }
249  }
250 
251  return true;
252 }
253 
254 
256 {
257  // test against some known values
258  float tol = 0.0001f;
259  matrix::Quatf q_true = {1.0f, 0.0f, 0.0f, 0.0f};
260 
261  matrix::Matrix3f R_orig;
262  R_orig.identity();
263 
264  matrix::Quatf q;
265  q.from_dcm(R_orig);
266 
267  for (unsigned i = 0; i < 4; i++) {
268  ut_assert("matrix::Quatf method 'from_dcm()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
269  }
270 
271  return true;
272 }
273 
275 {
276  float tol = 0.0001f;
277  matrix::Quatf q_true = {1.0f, 0.0f, 0.0f, 0.0f};
278 
279  matrix::Matrix3f R_orig;
280  R_orig.identity();
281 
282  matrix::Quatf q;
283  q.from_dcm(R_orig);
284 
285  q_true = matrix::Eulerf(0.3f, 0.2f, 0.1f);
286  q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
287 
288  for (unsigned i = 0; i < 4; i++) {
289  ut_assert("matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
290  }
291 
292  q_true = matrix::Eulerf(-1.5f, -0.2f, 0.5f);
293  q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
294 
295  for (unsigned i = 0; i < 4; i++) {
296  ut_assert("matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
297  }
298 
299  q_true = matrix::Eulerf(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
300  q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
301 
302  for (unsigned i = 0; i < 4; i++) {
303  ut_assert("matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
304  }
305 
306  return true;
307 }
308 
310 {
311  // test quaternion method "rotate" (rotate vector by quaternion)
312  matrix::Vector3f vector = {1.0f, 1.0f, 1.0f};
313  matrix::Vector3f vector_q;
314  matrix::Vector3f vector_r;
315  matrix::Quatf q;
316  matrix::Dcmf R;
317  float diff = 0.2f;
318  float tol = 0.00001f;
319 
320  //PX4_INFO("matrix::Quatf vector rotation method test.");
321 
322  for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
323  for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
324  for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
325  R = matrix::Eulerf(roll, pitch, yaw);
326  q = matrix::Eulerf(roll, pitch, yaw);
327  vector_r = R * vector;
328  vector_q = q.conjugate(vector);
329 
330  for (int i = 0; i < 3; i++) {
331  ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol);
332  }
333  }
334  }
335  }
336 
337 
338  // test some values calculated with matlab
339  tol = 0.0001f;
340  q = matrix::Eulerf(M_PI_2_F, 0.0f, 0.0f);
341  vector_q = q.conjugate(vector);
342  matrix::Vector3f vector_true = {1.00f, -1.00f, 1.00f};
343 
344  for (unsigned i = 0; i < 3; i++) {
345  ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
346  }
347 
348  q = matrix::Eulerf(0.3f, 0.2f, 0.1f);
349  vector_q = q.conjugate(vector);
350  vector_true = {1.1566, 0.7792, 1.0273};
351 
352  for (unsigned i = 0; i < 3; i++) {
353  ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
354  }
355 
356  q = matrix::Eulerf(-1.5f, -0.2f, 0.5f);
357  vector_q = q.conjugate(vector);
358  vector_true = {0.5095, 1.4956, -0.7096};
359 
360  for (unsigned i = 0; i < 3; i++) {
361  ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
362  }
363 
364  q = matrix::Eulerf(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
365  vector_q = q.conjugate(vector);
366  vector_true = { -1.3660, 0.3660, 1.0000};
367 
368  for (unsigned i = 0; i < 3; i++) {
369  ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
370  }
371 
372  return true;
373 }
374 
376 {
377  ut_assert("PX4_ISFINITE(0.0f)", PX4_ISFINITE(0.0f) == true);
378  ut_assert("PX4_ISFINITE(-0.0f)", PX4_ISFINITE(-0.0f) == true);
379  ut_assert("PX4_ISFINITE(1.0f)", PX4_ISFINITE(1.0f) == true);
380  ut_assert("PX4_ISFINITE(-1.0f)", PX4_ISFINITE(-1.0f) == true);
381 
382  ut_assert("PX4_ISFINITE(NAN)", PX4_ISFINITE(NAN) == false);
383  ut_assert("PX4_ISFINITE(1/0)", PX4_ISFINITE(1.0f / 0.0f) == false);
384  ut_assert("PX4_ISFINITE(0/0)", PX4_ISFINITE(0.0f / 0.0f) == false);
385  ut_assert("PX4_ISFINITE(INFINITY)", PX4_ISFINITE(INFINITY) == false);
386  ut_assert("PX4_ISFINITE(NAN * INFINITY)", PX4_ISFINITE(NAN * INFINITY) == false);
387  ut_assert("PX4_ISFINITE(NAN * 1.0f)", PX4_ISFINITE(NAN * 1.0f) == false);
388  ut_assert("PX4_ISFINITE(INFINITY * 2.0f)", PX4_ISFINITE(INFINITY * 2.0f) == false);
389 
390  return true;
391 }
392 
394 {
404 
405  return (_tests_failed == 0);
406 }
407 
#define ut_declare_test_c(test_function, test_class)
Definition: unit_test.h:40
bool testRotationMatrixQuaternion()
Quaternion from_dcm(const Matrix< Type, 3, 3 > &dcm)
XXX DEPRECATED, can use assignment or ctor.
Definition: Quaternion.hpp:531
Vector3< Type > conjugate(const Vector3< Type > &vec) const
Rotates vector v_1 in frame 1 to vector v_2 in frame 2 using the rotation quaternion q_21 describing ...
Definition: Quaternion.hpp:398
Base class to be used for unit tests.
Definition: unit_test.h:54
bool testVector2()
bool testMatrix10x10()
Quaternion class.
Definition: Dcm.hpp:24
High-resolution timer with callouts and timekeeping.
virtual bool run_tests()
Override to run your unit tests.
int _tests_failed
The number of unit tests which failed.
Definition: unit_test.h:206
int test_mathlib(int argc, char *argv[])
uint8_t * data
Definition: dataman.cpp:149
bool testVector10()
void print(FILE *stream=stdout) const
Definition: Matrix.hpp:343
#define ut_assert(message, test)
Used to assert a value within a unit test.
Definition: unit_test.h:113
bool testMatrixNonsymmetric()
bool testQuaternionfrom_dcm()
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
bool testMatrix3x3()
Euler< float > Eulerf
Definition: Euler.hpp:156
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
Tests declaration file.
void zero()
Definition: Matrix.hpp:421
#define ut_assert_true(test)
To assert specifically to true.
Definition: unit_test.h:127
bool testVector3()
#define TEST_OP(_title, _op)
bool testQuaternionRotate()
bool testFinite()
#define ut_run_test(test)
Runs a single unit test.
Definition: unit_test.h:96
#define M_PI_F
Definition: ashtech.cpp:44
void identity()
Definition: Matrix.hpp:457
bool testQuaternionfrom_euler()