PX4 Firmware
PX4 Autopilot Software http://px4.io
squareMatrix.cpp
Go to the documentation of this file.
1 #include "test_macros.hpp"
2 
3 #include <matrix/math.hpp>
4 
5 using namespace matrix;
6 
7 int main()
8 {
9  float data[9] = {1, 2, 3,
10  4, 5, 6,
11  7, 8, 10
12  };
13  SquareMatrix<float, 3> A(data);
14  Vector3<float> diag_check(1, 5, 10);
15 
16  TEST(isEqual(A.diag(), diag_check));
17  TEST(A.trace() - 16 < FLT_EPSILON);
18 
19  float data_check[9] = {
20  1.01158503f, 0.02190432f, 0.03238144f,
21  0.04349195f, 1.05428524f, 0.06539627f,
22  0.07576783f, 0.08708946f, 1.10894048f
23  };
24 
25  float dt = 0.01f;
27  SquareMatrix<float, 3> eA_check(data_check);
28  TEST((eA - eA_check).abs().max() < 1e-3f);
29 
30  SquareMatrix<float, 2> A_bottomright = A.slice<2,2>(1,1);
31  SquareMatrix<float, 2> A_bottomright2;
32  A_bottomright2 = A.slice<2,2>(1,1);
33 
34  float data_bottomright[4] = {5, 6,
35  8, 10
36  };
37  SquareMatrix<float, 2> bottomright_check(data_bottomright);
38  TEST(isEqual(A_bottomright, bottomright_check));
39  TEST(isEqual(A_bottomright2, bottomright_check));
40 
41 
42  return 0;
43 }
44 
45 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
#define FLT_EPSILON
int main()
Definition: squareMatrix.cpp:7
const Slice< Type, P, Q, M, N > slice(size_t x0, size_t y0) const
Definition: Matrix.hpp:374
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
Vector< Type, M > diag() const
#define TEST(X)
Definition: test_macros.hpp:14
float dt
Definition: px4io.c:73
Dual< Scalar, N > max(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Definition: Dual.hpp:224
SquareMatrix< Type, M > expm(const Matrix< Type, M, M > &A, size_t order=5)
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
Definition: Dual.hpp:196