PX4 Firmware
PX4 Autopilot Software http://px4.io
least_squares.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 int test_4x3(void);
7 template<typename Type> int test_4x4(void);
8 int test_4x4_type_double(void);
9 int test_div_zero(void);
10 
11 int main()
12 {
13  int ret;
14 
15  ret = test_4x4<float>();
16  if (ret != 0) return ret;
17 
18  ret = test_4x4<double>();
19  if (ret != 0) return ret;
20 
21  ret = test_4x3();
22  if (ret != 0) return ret;
23 
24  ret = test_div_zero();
25  if (ret != 0) return ret;
26 
27  return 0;
28 }
29 
30 int test_4x3() {
31  // Start with an (m x n) A matrix
32  float data[12] = {20.f, -10.f, -13.f,
33  17.f, 16.f, -18.f,
34  0.7f, -0.8f, 0.9f,
35  -1.f, -1.1f, -1.2f
36  };
37  Matrix<float, 4, 3> A(data);
38 
39  float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
40  Vector<float, 4> b(b_data);
41 
42  float x_check_data[3] = {-0.69168233f,
43  -0.26227593f,
44  -1.03767522f
45  };
46  Vector<float, 3> x_check(x_check_data);
47 
49 
50  Vector<float, 3> x = qrd.solve(b);
51  TEST(isEqual(x, x_check));
52  return 0;
53 }
54 
55 template<typename Type>
56 int test_4x4() {
57  // Start with an (m x n) A matrix
58  const Type data[16] = { 20.f, -10.f, -13.f, 21.f,
59  17.f, 16.f, -18.f, -14.f,
60  0.7f, -0.8f, 0.9f, -0.5f,
61  -1.f, -1.1f, -1.2f, -1.3f
62  };
63  Matrix<Type, 4, 4> A(data);
64 
65  Type b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
66  Vector<Type, 4> b(b_data);
67 
68  Type x_check_data[4] = { 0.97893433f,
69  -2.80798701f,
70  -0.03175765f,
71  -2.19387649f
72  };
73  Vector<Type, 4> x_check(x_check_data);
74 
76 
77  Vector<Type, 4> x = qrd.solve(b);
78  TEST(isEqual(x, x_check));
79  return 0;
80 }
81 
83  float data[4] = {0.0f, 0.0f, 0.0f, 0.0f};
84  Matrix<float, 2, 2> A(data);
85 
86  float b_data[2] = {1.0f, 1.0f};
87  Vector<float, 2> b(b_data);
88 
89  // Implement such that x returns zeros if it reaches div by zero
90  float x_check_data[2] = {0.0f, 0.0f};
91  Vector<float, 2> x_check(x_check_data);
92 
94 
95  Vector<float, 2> x = qrd.solve(b);
96  TEST(isEqual(x, x_check));
97  return 0;
98 }
99 
100 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
int test_div_zero(void)
int test_4x3(void)
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
int test_4x4_type_double(void)
Vector< Type, N > solve(const Vector< Type, M > &b)
Solve Ax=b for x.
#define TEST(X)
Definition: test_macros.hpp:14
int test_4x4(void)
int main()