PX4 Firmware
PX4 Autopilot Software http://px4.io
attitude.cpp
Go to the documentation of this file.
1 #include "test_macros.hpp"
2 #include <matrix/math.hpp>
3 #include <iostream>
4 
5 using namespace matrix;
6 
7 // manually instantiated all files we intend to test
8 // so that coverage works correctly
9 // doesn't matter what test this is in
10 namespace matrix {
11 template class Matrix<float, 3, 3>;
12 template class Vector3<float>;
13 template class Vector2<float>;
14 template class Vector<float, 4>;
15 template class Quaternion<float>;
16 template class AxisAngle<float>;
17 template class Scalar<float>;
18 template class SquareMatrix<float, 4>;
19 }
20 
21 int main()
22 {
23  // check data
24  Eulerf euler_check(0.1f, 0.2f, 0.3f);
25  Quatf q_check(0.98334744f, 0.0342708f, 0.10602051f, .14357218f);
26  float dcm_data[] = {
27  0.93629336f, -0.27509585f, 0.21835066f,
28  0.28962948f, 0.95642509f, -0.03695701f,
29  -0.19866933f, 0.0978434f, 0.97517033f
30  };
31  Dcmf dcm_check(dcm_data);
32 
33  // euler ctor
34  TEST(isEqual(euler_check, Vector3f(0.1f, 0.2f, 0.3f)));
35 
36  // euler default ctor
37  Eulerf e;
38  Eulerf e_zero = zeros<float, 3, 1>();
39  TEST(isEqual(e, e_zero));
40  TEST(isEqual(e, e));
41 
42  // euler vector ctor
43  Vector3f v(0.1f, 0.2f, 0.3f);
44  Eulerf euler_copy(v);
45  TEST(isEqual(euler_copy, euler_check));
46 
47  // quaternion ctor
48  Quatf q0(1, 2, 3, 4);
49  Quatf q(q0);
50  double eps = 1e-6;
51  TEST(fabs(q(0) - 1) < eps);
52  TEST(fabs(q(1) - 2) < eps);
53  TEST(fabs(q(2) - 3) < eps);
54  TEST(fabs(q(3) - 4) < eps);
55 
56  // quaternion ctor: vector to vector
57  // identity test
58  Quatf quat_v(v,v);
59  TEST(isEqual(quat_v.conjugate(v), v));
60  // random test (vector norm can not be preserved with a pure rotation)
61  Vector3f v1(-80.1f, 1.5f, -6.89f);
62  quat_v = Quatf(v1, v);
63  TEST(isEqual(quat_v.conjugate(v1).normalized() * v.norm(), v));
64  // special 180 degree case 1
65  v1 = Vector3f(0.f, 1.f, 1.f);
66  quat_v = Quatf(v1, -v1);
67  TEST(isEqual(quat_v.conjugate(v1), -v1));
68  // special 180 degree case 2
69  v1 = Vector3f(1.f, 2.f, 0.f);
70  quat_v = Quatf(v1, -v1);
71  TEST(isEqual(quat_v.conjugate(v1), -v1));
72  // special 180 degree case 3
73  v1 = Vector3f(0.f, 0.f, 1.f);
74  quat_v = Quatf(v1, -v1);
75  TEST(isEqual(quat_v.conjugate(v1), -v1));
76  // special 180 degree case 4
77  v1 = Vector3f(1.f, 1.f, 1.f);
78  quat_v = Quatf(v1, -v1);
79  TEST(isEqual(quat_v.conjugate(v1), -v1));
80 
81  // quat normalization
82  q.normalize();
83  TEST(isEqual(q, Quatf(0.18257419f, 0.36514837f,
84  0.54772256f, 0.73029674f)));
85  TEST(isEqual(q0.unit(), q));
86  TEST(isEqual(q0.unit(), q0.normalized()));
87 
88  // quat default ctor
89  q = Quatf();
90  TEST(isEqual(q, Quatf(1, 0, 0, 0)));
91 
92  // euler to quaternion
93  q = Quatf(euler_check);
94  TEST(isEqual(q, q_check));
95 
96  // euler to dcm
97  Dcmf dcm(euler_check);
98  TEST(isEqual(dcm, dcm_check));
99 
100  // quaternion to euler
101  Eulerf e1(q_check);
102  TEST(isEqual(e1, euler_check));
103 
104  // quaternion to dcm
105  Dcmf dcm1(q_check);
106  TEST(isEqual(dcm1, dcm_check));
107  // quaternion z-axis unit base vector
108  Vector3f q_z = q_check.dcm_z();
109  Vector3f R_z(dcm_check(0, 2), dcm_check(1, 2), dcm_check(2, 2));
110  TEST(isEqual(q_z, R_z));
111 
112  // dcm default ctor
113  Dcmf dcm2;
114  SquareMatrix<float, 3> I = eye<float, 3>();
115  TEST(isEqual(dcm2, I));
116 
117  // dcm to euler
118  Eulerf e2(dcm_check);
119  TEST(isEqual(e2, euler_check));
120 
121  // dcm to quaterion
122  Quatf q2(dcm_check);
123  TEST(isEqual(q2, q_check));
124 
125  // dcm renormalize
126  Dcmf A = eye<float, 3>();
127  Dcmf R(euler_check);
128  for (size_t i = 0; i < 1000; i++) {
129  A = R * A;
130  }
131 
132  A.renormalize();
133  float err = 0.0f;
134 
135  for (size_t r = 0; r < 3; r++) {
136  Vector3f rvec(matrix::Matrix<float,1,3>(A.row(r)).transpose());
137  err += fabs(1.0f - rvec.length());
138  }
139  TEST(err < eps);
140 
141  // constants
142  double deg2rad = M_PI / 180.0;
143  double rad2deg = 180.0 / M_PI;
144 
145  // euler dcm round trip check
146  for (double roll = -90; roll <= 90; roll += 90) {
147  for (double pitch = -90; pitch <= 90; pitch += 90) {
148  for (double yaw = -179; yaw <= 180; yaw += 90) {
149  // note if theta = pi/2, then roll is set to zero
150  double roll_expected = roll;
151  double yaw_expected = yaw;
152 
153  if (fabs(pitch -90) < eps) {
154  roll_expected = 0;
155  yaw_expected = yaw - roll;
156 
157  } else if (fabs(pitch + 90) < eps) {
158  roll_expected = 0;
159  yaw_expected = yaw + roll;
160  }
161 
162  if (yaw_expected < -180) {
163  yaw_expected += 360;
164  }
165 
166  if (yaw_expected > 180) {
167  yaw_expected -= 360;
168  }
169 
170  //printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
171  Euler<double> euler_expected(
172  deg2rad * roll_expected,
173  deg2rad * pitch,
174  deg2rad * yaw_expected);
175  Euler<double> euler(
176  deg2rad * roll,
177  deg2rad * pitch,
178  deg2rad * yaw);
179  Dcm<double> dcm_from_euler(euler);
180  //dcm_from_euler.print();
181  Euler<double> euler_out(dcm_from_euler);
182  TEST(isEqual(rad2deg * euler_expected, rad2deg * euler_out));
183 
184  Eulerf eulerf_expected(
185  float(deg2rad)*float(roll_expected),
186  float(deg2rad)*float(pitch),
187  float(deg2rad)*float(yaw_expected));
188  Eulerf eulerf(float(deg2rad)*float(roll),
189  float(deg2rad)*float(pitch),
190  float(deg2rad)*float(yaw));
191  Dcm<float> dcm_from_eulerf;
192  dcm_from_eulerf = eulerf;
193  Euler<float> euler_outf(dcm_from_eulerf);
194  TEST(isEqual(float(rad2deg)*eulerf_expected,
195  float(rad2deg)*euler_outf));
196  }
197  }
198  }
199 
200  // quaterion copy ctors
201  float data_v4[] = {1, 2, 3, 4};
202  Vector<float, 4> v4(data_v4);
203  Quatf q_from_v(v4);
204  TEST(isEqual(q_from_v, v4));
205 
206  Matrix<float, 4, 1> m4(data_v4);
207  Quatf q_from_m(m4);
208  TEST(isEqual(q_from_m, m4));
209 
210  // quaternion derivative in frame 1
211  Quatf q1(0, 1, 0, 0);
212  Vector<float, 4> q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
213  float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
214  Vector<float, 4> q1_dot1_check(data_q_dot1_check);
215  TEST(isEqual(q1_dot1, q1_dot1_check));
216 
217  // quaternion derivative in frame 2
218  Vector<float, 4> q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
219  float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
220  Vector<float, 4> q1_dot2_check(data_q_dot2_check);
221  TEST(isEqual(q1_dot2, q1_dot2_check));
222 
223  // quaternion product
224  Quatf q_prod_check(
225  0.93394439f, 0.0674002f, 0.20851f, 0.28236266f);
226  TEST(isEqual(q_prod_check, q_check * q_check));
227  q_check *= q_check;
228  TEST(isEqual(q_prod_check, q_check));
229 
230  // Quaternion scalar multiplication
231  float scalar = 0.5;
232  Quatf q_scalar_mul(1.0f, 2.0f, 3.0f, 4.0f);
233  Quatf q_scalar_mul_check(1.0f * scalar, 2.0f * scalar,
234  3.0f * scalar, 4.0f * scalar);
235  Quatf q_scalar_mul_res = scalar * q_scalar_mul;
236  TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res));
237  Quatf q_scalar_mul_res2 = q_scalar_mul * scalar;
238  TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res2));
239  Quatf q_scalar_mul_res3(q_scalar_mul);
240  q_scalar_mul_res3 *= scalar;
241  TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res3));
242 
243  // quaternion inverse
244  q = q_check.inversed();
245  TEST(fabs(q_check(0) - q(0)) < eps);
246  TEST(fabs(q_check(1) + q(1)) < eps);
247  TEST(fabs(q_check(2) + q(2)) < eps);
248  TEST(fabs(q_check(3) + q(3)) < eps);
249 
250  q = q_check;
251  q.invert();
252  TEST(fabs(q_check(0) - q(0)) < eps);
253  TEST(fabs(q_check(1) + q(1)) < eps);
254  TEST(fabs(q_check(2) + q(2)) < eps);
255  TEST(fabs(q_check(3) + q(3)) < eps);
256 
257  // quaternion canonical
258  Quatf q_non_canonical(-0.7f,0.4f, 0.3f, -0.3f);
259  Quatf q_canonical(0.7f,-0.4f, -0.3f, 0.3f);
260  Quatf q_canonical_ref(0.7f,-0.4f, -0.3f, 0.3f);
261  TEST(isEqual(q_non_canonical.canonical(),q_canonical_ref));
262  TEST(isEqual(q_canonical.canonical(),q_canonical_ref));
263  q_non_canonical.canonicalize();
264  q_canonical.canonicalize();
265  TEST(isEqual(q_non_canonical,q_canonical_ref));
266  TEST(isEqual(q_canonical,q_canonical_ref));
267 
268  // quaternion setIdentity
269  Quatf q_nonIdentity(-0.7f, 0.4f, 0.5f, -0.3f);
270  Quatf q_identity(1.0f, 0.0f, 0.0f, 0.0f);
271  q_nonIdentity.setIdentity();
272  TEST(isEqual(q_nonIdentity, q_identity));
273 
274  // non-unit quaternion invese
275  Quatf qI(1.0f, 0.0f, 0.0f, 0.0f);
276  Quatf q_nonunit(0.1f, 0.2f, 0.3f, 0.4f);
277  TEST(isEqual(qI, q_nonunit*q_nonunit.inversed()));
278 
279  // rotate quaternion (nonzero rotation)
280  Vector<float, 3> rot;
281  rot(0) = 1.0f;
282  rot(1) = rot(2) = 0.0f;
283  qI.rotate(rot);
284  Quatf q_true(cos(1.0f / 2), sin(1.0f / 2), 0.0f, 0.0f);
285  TEST(isEqual(qI, q_true));
286 
287  // rotate quaternion (zero rotation)
288  qI = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
289  rot(0) = 0.0f;
290  rot(1) = rot(2) = 0.0f;
291  qI.rotate(rot);
292  q_true = Quatf(cos(0.0f), sin(0.0f), 0.0f, 0.0f);
293  TEST(isEqual(qI, q_true));
294 
295  // rotate quaternion (random non-commutating rotation)
296  q = Quatf(AxisAnglef(5.1f, 3.2f, 8.4f));
297  rot = Vector3f(1.1f, 2.5f, 3.8f);
298  q.rotate(rot);
299  q_true = Quatf(0.3019f, 0.2645f, 0.2268f, 0.8874f);
300  TEST(isEqual(q, q_true));
301 
302  // get rotation axis from quaternion (nonzero rotation)
303  q = Quatf(cos(1.0f / 2), 0.0f, sin(1.0f / 2), 0.0f);
304  rot = q.to_axis_angle();
305  TEST(fabs(rot(0)) < eps);
306  TEST(fabs(rot(1) - 1.0f) < eps);
307  TEST(fabs(rot(2)) < eps);
308 
309  // get rotation axis from quaternion (zero rotation)
310  q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
311  rot = q.to_axis_angle();
312  TEST(fabs(rot(0)) < eps);
313  TEST(fabs(rot(1)) < eps);
314  TEST(fabs(rot(2)) < eps);
315 
316  // from axis angle (zero rotation)
317  rot(0) = rot(1) = rot(2) = 0.0f;
318  q.from_axis_angle(rot, 0.0f);
319  q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
320  TEST(isEqual(q, q_true));
321 
322  // from axis angle, with length of vector the rotation
323  float n = float(sqrt(4*M_PI*M_PI/3));
324  q.from_axis_angle(Vector3f(n, n, n));
325  TEST(isEqual(q, Quatf(-1, 0, 0, 0)));
326  q.from_axis_angle(Vector3f(0, 0, 0));
327  TEST(isEqual(q, Quatf(1, 0, 0, 0)));
328 
329  // Quaternion initialisation per array
330  float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f};
331  Quaternion<float>q_from_array(q_array);
332 
333  for (size_t i = 0; i < 4; i++) {
334  TEST(fabs(q_from_array(i) - q_array[i]) < eps);
335  }
336 
337  // axis angle
338  AxisAnglef aa_true(Vector3f(1.0f, 2.0f, 3.0f));
339  TEST(isEqual(aa_true, Vector3f(1.0f, 2.0f, 3.0f)));
340  AxisAnglef aa_empty;
341  TEST(isEqual(aa_empty, AxisAnglef(0.0f, 0.0f, 0.0f)));
342  float aa_data[] = {4.0f, 5.0f, 6.0f};
343  AxisAnglef aa_data_init(aa_data);
344  TEST(isEqual(aa_data_init, AxisAnglef(4.0f, 5.0f, 6.0f)));
345 
346  AxisAnglef aa_norm_check(Vector3f(0.0f, 0.0f, 0.0f));
347  TEST(isEqual(aa_norm_check.axis(), Vector3f(1, 0, 0)));
348  TEST(isEqualF(aa_norm_check.angle(), 0.0f));
349 
350  q = Quatf(-0.29555112749297824f, 0.25532186f, 0.51064372f, 0.76596558f);
351  TEST(isEqual(q.imag(), Vector3f(0.25532186f, 0.51064372f, 0.76596558f)));
352 
353  // from dcm
354  TEST(isEqual(Eulerf(q.from_dcm(Dcmf(q))), Eulerf(q)));
355 
356  // to dcm
357  TEST(isEqual(Dcmf(q), q.to_dcm()));
358 
359  // conjugate
360  v = Vector3f(1.5f, 2.2f, 3.2f);
361  TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1));
362  TEST(isEqual(q.conjugate(v1), Dcmf(q)*v1));
363 
364  AxisAnglef aa_q_init(q);
365  TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f)));
366 
367  AxisAnglef aa_euler_init(Eulerf(0.0f, 0.0f, 0.0f));
368  TEST(isEqual(aa_euler_init, Vector3f(0.0f, 0.0f, 0.0f)));
369 
370  Dcmf dcm_aa_check = AxisAnglef(dcm_check);
371  TEST(isEqual(dcm_aa_check, dcm_check));
372 
373  AxisAnglef aa_axis_angle_init(Vector3f(1.0f, 2.0f, 3.0f), 3.0f);
374  TEST(isEqual(aa_axis_angle_init, Vector3f(0.80178373f, 1.60356745f, 2.40535118f)));
375  TEST(isEqual(aa_axis_angle_init.axis(), Vector3f(0.26726124f, 0.53452248f, 0.80178373f)));
376  TEST(isEqualF(aa_axis_angle_init.angle(), 3.0f));
377  TEST(isEqual(Quatf((AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), 0.0f))),
378  Quatf(1.0f, 0.0f, 0.0f, 0.0f)));
379 
380 
381  // check consistentcy of quaternion and dcm product
382  Dcmf dcm3(Eulerf(1, 2, 3));
383  Dcmf dcm4(Eulerf(4, 5, 6));
384  Dcmf dcm34 = dcm3 * dcm4;
385  TEST(isEqual(Eulerf(Quatf(dcm3)*Quatf(dcm4)), Eulerf(dcm34)));
386 
387  // check corner cases of matrix to quaternion conversion
388  q = Quatf(0,1,0,0); // 180 degree rotation around the x axis
389  R = Dcmf(q);
390  TEST(isEqual(q, Quatf(R)));
391  q = Quatf(0,0,1,0); // 180 degree rotation around the y axis
392  R = Dcmf(q);
393  TEST(isEqual(q, Quatf(R)));
394  q = Quatf(0,0,0,1); // 180 degree rotation around the z axis
395  R = Dcmf(q);
396  TEST(isEqual(q, Quatf(R)));
397 
398 #if defined(SUPPORT_STDIOSTREAM)
399  std::cout << "q:" << q;
400 #endif
401  return 0;
402 }
403 
404 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
void canonicalize()
Bring quaternion to canonical form.
Definition: Quaternion.hpp:357
Vector< Type, 3 > axis()
Definition: AxisAngle.hpp:147
Matrix< Type, N, M > T() const
Definition: Matrix.hpp:368
Dcm< float > Dcmf
Definition: Dcm.hpp:185
Dcm< Type > to_dcm()
XXX DEPRECATED, can use assignment or ctor.
Definition: Quaternion.hpp:538
Vector3< Type > dcm_z() const
Corresponding body z-axis to an attitude quaternion / last orthogonal unit basis vector.
Definition: Quaternion.hpp:514
Quaternion from_dcm(const Matrix< Type, 3, 3 > &dcm)
XXX DEPRECATED, can use assignment or ctor.
Definition: Quaternion.hpp:531
void invert()
Invert quaternion in place.
Definition: Quaternion.hpp:333
Quaternion canonical() const
Return canonical form of the quaternion.
Definition: Quaternion.hpp:368
void normalize()
Definition: Vector.hpp:87
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
Quaternion class.
Definition: Dcm.hpp:24
Vector normalized() const
Definition: Vector.hpp:103
Vector< Type, 3 > to_axis_angle() const
Rotation vector from quaternion XXX DEPRECATED, use AxisAngle class.
Definition: Quaternion.hpp:481
void rotate(const AxisAngle< Type > &vec)
Rotate quaternion from rotation vector.
Definition: Quaternion.hpp:383
void setIdentity()
Definition: Matrix.hpp:447
Matrix41 derivative2(const Matrix31 &w) const
Computes the derivative of q_21 when rotated with angular velocity expressed in frame 2 v_2 = q_21 * ...
Definition: Quaternion.hpp:323
Dual< Scalar, N > cos(const Dual< Scalar, N > &a)
Definition: Dual.hpp:286
void from_axis_angle(Vector< Type, 3 > vec)
Rotation quaternion from vector.
Definition: Quaternion.hpp:431
void renormalize()
Definition: Dcm.hpp:175
Matrix41 derivative1(const Matrix31 &w) const
Computes the derivative of q_21 when rotated with angular velocity expressed in frame 1 v_2 = q_21 * ...
Definition: Quaternion.hpp:308
bool isEqual(const Matrix< Type, M, N > &x, const Matrix< Type, M, N > &y, const Type eps=1e-4f)
Definition: Matrix.hpp:571
Vector unit() const
Definition: Vector.hpp:91
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Euler< float > Eulerf
Definition: Euler.hpp:156
Euler angles class.
Definition: AxisAngle.hpp:18
Vector3< Type > imag() const
Imaginary components of quaternion.
Definition: Quaternion.hpp:501
#define err(eval,...)
Definition: err.h:83
Vector3< float > Vector3f
Definition: Vector3.hpp:136
Quaternion inversed() const
Invert quaternion.
Definition: Quaternion.hpp:343
#define TEST(X)
Definition: test_macros.hpp:14
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
int main()
Definition: attitude.cpp:21
#define M_PI
Definition: gps_helper.cpp:38
const Slice< Type, 1, N, M, N > row(size_t i) const
Definition: Matrix.hpp:385
AxisAngle< float > AxisAnglef
Definition: AxisAngle.hpp:160
AxisAngle class.
Definition: AxisAngle.hpp:21
bool isEqualF(const Type x, const Type y, const Type eps=1e-4f)
Compare if two floating point numbers are equal.
Type norm() const
Definition: Vector.hpp:73
Vector3< Type > conjugate_inversed(const Vector3< Type > &vec) const
Rotates vector v_2 in frame 2 to vector v_1 in frame 1 using the rotation quaternion q_21 describing ...
Definition: Quaternion.hpp:414
Dual< Scalar, N > sin(const Dual< Scalar, N > &a)
Definition: Dual.hpp:279
Dual< Scalar, N > sqrt(const Dual< Scalar, N > &a)
Definition: Dual.hpp:188